827 lines
31 KiB
C++
827 lines
31 KiB
C++
/**
|
||
* @file arm_oop.hpp
|
||
* @brief 机械臂类 - C++面向对象实现
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#include <cstring>
|
||
#include <cmath>
|
||
#include <stdint.h>
|
||
#include <array>
|
||
#include "joint.hpp"
|
||
#include "bsp/time.h"
|
||
#include "component/arm_kinematics/arm6dof.h"
|
||
#include "component/toolbox/robotics.h"
|
||
#define qLimitTolerance 0.75// 关节角度误差容限（rad），用于IK解的有效性验证
|
||
namespace arm {
|
||
|
||
// 控制模式
|
||
enum class ControlMode {
|
||
IDLE = 0,
|
||
JOINT_POSITION,
|
||
CARTESIAN_POSITION,
|
||
CARTESIAN_ANALYTICAL,
|
||
TRAJECTORY,
|
||
TEACH, // 示教模式（重力补偿 + 零刚度）
|
||
GRAVITY_COMP, // 重力补偿模式（带位置保持）
|
||
};
|
||
|
||
// 运动状态
|
||
enum class MotionState {
|
||
STOPPED = 0,
|
||
MOVING,
|
||
REACHED,
|
||
ERROR,
|
||
};
|
||
// ============================================================================
|
||
// RoboticArm 类 - 机械臂
|
||
// ============================================================================
|
||
class RoboticArm {
|
||
private:
|
||
static constexpr size_t JOINT_NUM = 6;
|
||
|
||
std::array<Joint*, JOINT_NUM> joints_; // 关节数组（多态指针）
|
||
|
||
// 末端状态
|
||
struct {
|
||
Arm6dof_Pose_t current; // 当前末端位姿（FK计算）
|
||
Arm6dof_Pose_t target; // 目标末端位姿
|
||
} end_effector_;
|
||
|
||
// 控制状态
|
||
struct {
|
||
ControlMode mode;
|
||
MotionState state;
|
||
float position_tolerance;
|
||
float velocity_tolerance;
|
||
bool enable;
|
||
bool gravity_comp_enable; // 重力补偿使能
|
||
} control_;
|
||
|
||
// 逆运动学
|
||
struct {
|
||
Arm6dof_JointAngles_t angles; // IK解算得到的关节角度
|
||
bool valid; // IK解是否有效
|
||
} inverseKinematics_;
|
||
|
||
// 重力补偿
|
||
struct {
|
||
float torques[JOINT_NUM]; // 各关节重力补偿力矩
|
||
float scale; // 补偿比例系数（用于微调）
|
||
} gravity_comp_;
|
||
|
||
// 笛卡尔空间轨迹规划
|
||
// 原理：将大步运动拆分为每帧一小步的线性插值，IK初始猜测始终是上一帧解，
|
||
// 从而保证牛顿迭代永远在正确分支附近收敛，根本消除大跨度跳变问题
|
||
struct {
|
||
Arm6dof_Pose_t start; // 轨迹起点（启动时的当前末端位姿）
|
||
Arm6dof_Pose_t goal; // 轨迹终点（目标末端位姿）
|
||
float t; // 插值进度 [0.0, 1.0]，0=起点，1=终点
|
||
bool active; // 是否正在执行轨迹
|
||
float max_lin_vel; // 末端线速度限制 (m/s)
|
||
float max_ang_vel; // 末端角速度限制 (rad/s)
|
||
Arm6dof_JointAngles_t angles; // 轨迹模式下当前插值点的IK解算结果
|
||
bool valid; // IK解是否有效
|
||
} traj_;
|
||
|
||
// 坐标系目标缓存
|
||
// 用途：MoveCartesianTool / MoveCartesianHeading 每帧都接收坐标系下的绝对目标，
|
||
// 若每帧都用当前 end_effector_.current 做变换，机器人运动时位姿变化会导致
|
||
// 世界系目标每帧漂移，MoveCartesian 检测到 goal 变化就重置 traj_.t=0，
|
||
// t 永远无法到达 1。
|
||
// 解决：仅当输入目标发生变化时才重新用当前位姿换算世界系目标并缓存；
|
||
// 输入不变时复用缓存，轨迹可以连续推进直到 t=1。
|
||
struct FrameCache_t {
|
||
Arm6dof_Pose_t last_input; // 上次收到的坐标系输入
|
||
Arm6dof_Pose_t world_goal; // 对应的世界系目标（换算结果）
|
||
bool valid; // false=尚未初始化，首次调用强制换算
|
||
};
|
||
FrameCache_t tool_frame_cache_;
|
||
FrameCache_t heading_frame_cache_;
|
||
|
||
// 时间管理
|
||
struct {
|
||
float now;
|
||
uint64_t last_wakeup;
|
||
float dt;
|
||
} timer_;
|
||
|
||
static bool IsPoseChanged(const Arm6dof_Pose_t& lhs,
|
||
const Arm6dof_Pose_t& rhs,
|
||
float pos_eps,
|
||
float ang_eps) {
|
||
return fabsf(lhs.x - rhs.x) > pos_eps ||
|
||
fabsf(lhs.y - rhs.y) > pos_eps ||
|
||
fabsf(lhs.z - rhs.z) > pos_eps ||
|
||
fabsf(lhs.roll - rhs.roll) > ang_eps ||
|
||
fabsf(lhs.pitch - rhs.pitch) > ang_eps ||
|
||
fabsf(lhs.yaw - rhs.yaw) > ang_eps;
|
||
}
|
||
|
||
void RelaxAllJoints() {
|
||
for (Joint* joint : joints_) {
|
||
if (joint) {
|
||
joint->Relax();
|
||
}
|
||
}
|
||
}
|
||
|
||
void SetJointTargetsToCurrent() {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||
}
|
||
}
|
||
}
|
||
|
||
void FillCurrentJointAngles(Arm6dof_JointAngles_t& q) const {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||
}
|
||
}
|
||
|
||
public:
|
||
// 构造函数
|
||
RoboticArm() {
|
||
joints_.fill(nullptr);
|
||
|
||
control_.mode = ControlMode::IDLE;
|
||
control_.state = MotionState::STOPPED;
|
||
control_.position_tolerance = 0.02f; // 0.02 rad
|
||
control_.velocity_tolerance = 0.01f;
|
||
control_.enable = false;
|
||
control_.gravity_comp_enable = false;
|
||
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
gravity_comp_.torques[i] = 0.0f;
|
||
}
|
||
gravity_comp_.scale = 1.0f;
|
||
inverseKinematics_.valid = false;
|
||
memset(&inverseKinematics_.angles, 0, sizeof(inverseKinematics_.angles));
|
||
memset(&traj_, 0, sizeof(traj_));
|
||
traj_.t = 0.0f;
|
||
traj_.active = false;
|
||
traj_.max_lin_vel = 0.15f; // 150 mm/s，可通过 SetLinVelLimit() 调整
|
||
traj_.max_ang_vel = 1.0f; // ~57°/s，可通过 SetAngVelLimit() 调整
|
||
|
||
memset(&tool_frame_cache_, 0, sizeof(tool_frame_cache_));
|
||
memset(&heading_frame_cache_, 0, sizeof(heading_frame_cache_));
|
||
tool_frame_cache_.valid = false;
|
||
heading_frame_cache_.valid = false;
|
||
|
||
timer_.now = 0.0f;
|
||
timer_.last_wakeup = 0;
|
||
timer_.dt = 0.001f;
|
||
}
|
||
|
||
// 禁止拷贝
|
||
RoboticArm(const RoboticArm&) = delete;
|
||
RoboticArm& operator=(const RoboticArm&) = delete;
|
||
|
||
// 添加关节（依赖注入）
|
||
void AddJoint(size_t index, Joint* joint) {
|
||
if (index < JOINT_NUM) {
|
||
joints_[index] = joint;
|
||
}
|
||
}
|
||
|
||
// 访问关节
|
||
Joint* GetJoint(size_t index) {
|
||
return (index < JOINT_NUM) ? joints_[index] : nullptr;
|
||
}
|
||
|
||
const Joint* GetJoint(size_t index) const {
|
||
return (index < JOINT_NUM) ? joints_[index] : nullptr;
|
||
}
|
||
|
||
// 初始化所有关节
|
||
int8_t Init() {
|
||
for (Joint* joint : joints_) { // 遍历关节数组
|
||
if (joint) {
|
||
joint->Register();
|
||
joint->Enable();
|
||
}
|
||
}
|
||
memset(&end_effector_.target, 0, sizeof(end_effector_.target));
|
||
return 0;
|
||
}
|
||
|
||
// 更新所有关节状态
|
||
int8_t Update() {
|
||
// 更新每个关节
|
||
for (Joint* joint : joints_) { // 遍历关节数组
|
||
if (joint) {
|
||
joint->Enable();
|
||
joint->Update();
|
||
}
|
||
}
|
||
|
||
// 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取）
|
||
Arm6dof_JointAngles_t q;
|
||
FillCurrentJointAngles(q);
|
||
|
||
// 正运动学：计算当前末端位姿
|
||
Arm6dof_ForwardKinematics(&q, &end_effector_.current);
|
||
|
||
// 计算重力补偿力矩
|
||
if (control_.gravity_comp_enable) {
|
||
CalcGravityTorques(q);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算当前姿态下各关节的重力补偿力矩
|
||
* @param q 当前关节角度（由Update()统一读取后传入，避免重复读取）
|
||
* @details 基于牛顿-欧拉逆动力学，设速度和加速度为零，
|
||
* 求解的力矩即为平衡重力所需的各关节力矩
|
||
*/
|
||
int8_t CalcGravityTorques(const Arm6dof_JointAngles_t& q) {
|
||
float raw_torques[JOINT_NUM];
|
||
int ret = Arm6dof_GravityCompensation(&q, raw_torques);
|
||
if (ret != 0) return ret;
|
||
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scale;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 2.5D 解析降维逆运动学（专为该六轴机械臂的L型构型设计）
|
||
* 特点：无奇异点、O(1)极速求解、绝对稳定。
|
||
* 直接映射目标并进行安全边界裁剪，无需牛顿迭代。
|
||
*/
|
||
int8_t InverseKinematicsAnalytical(const Arm6dof_Pose_t* pose, Arm6dof_JointAngles_t* q_out) {
|
||
if (!pose || !q_out) return -1;
|
||
|
||
float x = pose->x;
|
||
float y = pose->y;
|
||
float z = pose->z;
|
||
|
||
// 提取的宏观参数
|
||
const float L1 = 0.3265f;
|
||
const float dX = 0.0905f;
|
||
const float dY = 0.05278f + 0.18467f; // 从 URDF 参数 J4 和 J5 中提取的垂直偏置积累
|
||
const float L2_eff = sqrtf(dX*dX + dY*dY); // 虚拟小臂斜边
|
||
const float theta_offset = atan2f(dY, dX); // 结构连杆的天然偏置仰角
|
||
const float L3 = 0.10487f; // 腕部总伸出长度
|
||
const float Z_J2 = 0.024f + 0.1015f; // 自基座到底盘的Z高度
|
||
|
||
// J1: 偏航直接通过 XY 夹角决定
|
||
float target_q1 = atan2f(y, x);
|
||
|
||
// 面向R-Z平面的投影
|
||
float R = sqrtf(x*x + y*y);
|
||
float e_pitch = pose->pitch;
|
||
|
||
// 逆推动力学关节5 (虚拟手腕端点) 的位置
|
||
float R5 = R - L3 * cosf(e_pitch);
|
||
float Z5 = z - L3 * sinf(e_pitch);
|
||
|
||
// 相对于 J2 原点的距离
|
||
float r_target = R5;
|
||
float z_target = Z5 - Z_J2;
|
||
|
||
float D_sq = r_target*r_target + z_target*z_target;
|
||
float D = sqrtf(D_sq);
|
||
|
||
// 死区保护层(极其重要)：限制最大前伸距离，强制拦下无效追踪，绝不当机
|
||
float MAX_REACH = L1 + L2_eff - 0.005f;
|
||
float MIN_REACH = fabsf(L1 - L2_eff) + 0.005f;
|
||
if (D > MAX_REACH) { D = MAX_REACH; D_sq = D*D; }
|
||
if (D < MIN_REACH) { D = MIN_REACH; D_sq = D*D; }
|
||
|
||
// 用余弦定理求 肘部内角 gamma
|
||
float cos_gamma = (L1*L1 + L2_eff*L2_eff - D_sq) / (2.0f * L1 * L2_eff);
|
||
cos_gamma = (cos_gamma > 1.0f) ? 1.0f : ((cos_gamma < -1.0f) ? -1.0f : cos_gamma);
|
||
float gamma = acosf(cos_gamma);
|
||
|
||
// 用余弦定理求 目标线与大臂的夹角 beta
|
||
float cos_beta = (L1*L1 + D_sq - L2_eff*L2_eff) / (2.0f * L1 * D);
|
||
cos_beta = (cos_beta > 1.0f) ? 1.0f : ((cos_beta < -1.0f) ? -1.0f : cos_beta);
|
||
float beta = acosf(cos_beta);
|
||
|
||
// 目标线相对于水平面的仰角 alpha
|
||
float alpha = atan2f(z_target, r_target);
|
||
|
||
// 选择 Elbow Up（肘部向上）姿态：
|
||
float target_q2 = alpha + beta;
|
||
float q3_eff = -( (float)M_PI - gamma);
|
||
float target_q3 = q3_eff - theta_offset;
|
||
|
||
// 手腕解耦补偿
|
||
float arm_pitch_eff = target_q2 + q3_eff;
|
||
float target_q5 = e_pitch - arm_pitch_eff;
|
||
|
||
q_out->q[0] = target_q1;
|
||
q_out->q[1] = target_q2;
|
||
q_out->q[2] = target_q3;
|
||
q_out->q[3] = pose->roll;
|
||
q_out->q[4] = target_q5;
|
||
q_out->q[5] = pose->yaw;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 轨迹规划核心：基于固定的 start→goal 路径，由 traj_.t 驱动进度
|
||
*
|
||
* 设计原则：轨迹由 traj_.start / traj_.goal 固定，traj_.t 按速度限制逐帧递增。
|
||
* - IK目标点由 traj_.t 插值得到，与当前末端位姿无关，手动扰动不影响轨迹目标
|
||
* - IK初始猜测优先使用上一帧轨迹解（连续性好），首帧用当前关节角
|
||
*
|
||
* 每帧执行流程：
|
||
* 1. 计算 start→goal 的总位移/角位移向量（固定）
|
||
* 2. dt = min(v_max*dt/total, 1.0) 按速度限制计算本帧进度增量
|
||
* 3. traj_.t += dt，clamp 到 [0, 1]
|
||
* 4. interp = start + t * (goal - start)
|
||
* 5. IK(interp, q_init=上一帧解 或 当前关节角) → 写入 traj_.angles
|
||
*/
|
||
void StepTrajectory() {
|
||
if (!traj_.active) return;
|
||
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
||
|
||
// 计算 start→goal 的固定总位移向量（不随手动扰动变化）
|
||
float tdx = traj_.goal.x - traj_.start.x;
|
||
float tdy = traj_.goal.y - traj_.start.y;
|
||
float tdz = traj_.goal.z - traj_.start.z;
|
||
float total_dist = sqrtf(tdx*tdx + tdy*tdy + tdz*tdz);
|
||
|
||
float tdr = traj_.goal.roll - traj_.start.roll;
|
||
float tdp = traj_.goal.pitch - traj_.start.pitch;
|
||
float tdyw = traj_.goal.yaw - traj_.start.yaw;
|
||
// 姿态差 wrap 到 (-π, π]：走最短路径
|
||
tdr -= roundf(tdr / TWO_PI) * TWO_PI;
|
||
tdp -= roundf(tdp / TWO_PI) * TWO_PI;
|
||
tdyw -= roundf(tdyw / TWO_PI) * TWO_PI;
|
||
float total_ang = sqrtf(tdr*tdr + tdp*tdp + tdyw*tdyw);
|
||
|
||
// 按速度限制计算本帧进度增量，位置/姿态约束取严格者
|
||
float dt_p = (total_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / total_dist) : 1.0f;
|
||
float dt_a = (total_ang > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / total_ang) : 1.0f;
|
||
float dt = dt_p < dt_a ? dt_p : dt_a;
|
||
if (dt > 1.0f) dt = 1.0f;
|
||
|
||
traj_.t += dt;
|
||
if (traj_.t >= 1.0f) {
|
||
traj_.t = 1.0f;
|
||
traj_.active = false;
|
||
}
|
||
|
||
// 线性插值：start + t * (goal - start)，轨迹固定，不受手动扰动影响
|
||
Arm6dof_Pose_t interp;
|
||
interp.x = traj_.start.x + traj_.t * tdx;
|
||
interp.y = traj_.start.y + traj_.t * tdy;
|
||
interp.z = traj_.start.z + traj_.t * tdz;
|
||
interp.roll = traj_.start.roll + traj_.t * tdr;
|
||
interp.pitch = traj_.start.pitch + traj_.t * tdp;
|
||
interp.yaw = traj_.start.yaw + traj_.t * tdyw;
|
||
|
||
// IK初始猜测：优先用上一帧轨迹IK解（连续性好，不受手动扰动影响）
|
||
// traj_.valid=false 表示首帧或上帧IK失败，改用当前关节角
|
||
Arm6dof_JointAngles_t q_init;
|
||
if (traj_.valid) {
|
||
q_init = traj_.angles;
|
||
} else {
|
||
FillCurrentJointAngles(q_init);
|
||
}
|
||
|
||
bool ik_success = false;
|
||
if (control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
|
||
ik_success = (InverseKinematicsAnalytical(&interp, &traj_.angles) == 0);
|
||
} else {
|
||
ik_success = (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) == 0);
|
||
}
|
||
|
||
if (!ik_success) {
|
||
traj_.valid = false;
|
||
traj_.active = false;
|
||
control_.state = MotionState::ERROR;
|
||
return;
|
||
}
|
||
|
||
// 角度连续性修正：仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）
|
||
// IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (!joints_[i]) continue;
|
||
const auto& dh = joints_[i]->GetParams();
|
||
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; // 范围 ≤ 2π，无等效分支
|
||
float diff = traj_.angles.q[i] - q_init.q[i];
|
||
diff -= roundf(diff / TWO_PI) * TWO_PI;
|
||
traj_.angles.q[i] = q_init.q[i] + diff;
|
||
}
|
||
|
||
// 限位检查
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
const auto& dh = joints_[i]->GetParams();
|
||
if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) ||
|
||
traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) {
|
||
traj_.valid = false;
|
||
traj_.active = false;
|
||
control_.state = MotionState::ERROR;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 突变检查：以上一帧轨迹解（q_init）为参考，检查轨迹自身连续性
|
||
static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
float delta = fabsf(traj_.angles.q[i] - q_init.q[i]);
|
||
if (delta > TRAJ_MAX_JOINT_DELTA) {
|
||
traj_.valid = false;
|
||
traj_.active = false;
|
||
control_.state = MotionState::ERROR;
|
||
return;
|
||
}
|
||
}
|
||
|
||
traj_.valid = true;
|
||
end_effector_.target = interp;
|
||
}
|
||
|
||
int8_t GetPose(Arm6dof_Pose_t* pose) const {
|
||
if (pose == nullptr) {
|
||
return -1;
|
||
}
|
||
std::memcpy(pose, &end_effector_.current, sizeof(Arm6dof_Pose_t));
|
||
return 0;
|
||
}
|
||
|
||
int8_t SetTargetPose(const Arm6dof_Pose_t pose) {
|
||
end_effector_.target = pose;
|
||
return 0;
|
||
}
|
||
|
||
int8_t GetTargetPose(Arm6dof_Pose_t* pose) const {
|
||
if (pose == nullptr) {
|
||
return -1;
|
||
}
|
||
std::memcpy(pose, &end_effector_.target, sizeof(Arm6dof_Pose_t));
|
||
return 0;
|
||
}
|
||
|
||
// 控制循环
|
||
int8_t Control() {
|
||
// 更新时间
|
||
uint64_t now_us = BSP_TIME_Get_us();
|
||
timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f;
|
||
timer_.last_wakeup = now_us;
|
||
timer_.now = now_us / 1000000.0f;
|
||
|
||
// 未使能则松弛
|
||
if (!control_.enable) {
|
||
RelaxAllJoints();
|
||
control_.state = MotionState::STOPPED;
|
||
return 0;
|
||
}
|
||
|
||
// 根据控制模式执行
|
||
switch (control_.mode) {
|
||
case ControlMode::IDLE:
|
||
RelaxAllJoints();
|
||
control_.state = MotionState::STOPPED;
|
||
break;
|
||
|
||
case ControlMode::JOINT_POSITION:
|
||
case ControlMode::CARTESIAN_POSITION:
|
||
case ControlMode::CARTESIAN_ANALYTICAL:
|
||
case ControlMode::GRAVITY_COMP: {
|
||
// 检查错误状态（不可达或超限）
|
||
if (control_.state == MotionState::ERROR) {
|
||
// 进入错误状态，松弛所有关节（安全停止）
|
||
RelaxAllJoints();
|
||
control_.enable = false; // 自动禁用
|
||
return -1;
|
||
}
|
||
|
||
// 将重力补偿力矩写入各关节前馈
|
||
if (control_.gravity_comp_enable) {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetFeedforwardTorque(gravity_comp_.torques[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
// GRAVITY_COMP 模式：保持当前位置 + 重力补偿
|
||
if (control_.mode == ControlMode::GRAVITY_COMP) {
|
||
SetJointTargetsToCurrent();
|
||
}
|
||
|
||
// CARTESIAN_POSITION 相关的轨迹模式：每帧推进轨迹一小步，再应用IK解到关节
|
||
if (control_.mode == ControlMode::CARTESIAN_POSITION || control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
|
||
// 轨迹规划核心调用：插值 + IK，结果写入 traj_
|
||
StepTrajectory();
|
||
|
||
if (traj_.valid) {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetTargetAngle(traj_.angles.q[i]);
|
||
}
|
||
}
|
||
} else {
|
||
// IK解无效：松弛所有关节并进入错误状态
|
||
RelaxAllJoints();
|
||
control_.state = MotionState::ERROR;
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
// 位置控制
|
||
bool all_reached = true;
|
||
for (Joint* joint : joints_) { // 遍历关节数组
|
||
if (joint) {
|
||
joint->PositionControl(joint->GetTargetAngle(), timer_.dt);
|
||
if (!joint->IsReached(control_.position_tolerance)) {
|
||
all_reached = false;
|
||
}
|
||
}
|
||
}
|
||
control_.state = all_reached ? MotionState::REACHED : MotionState::MOVING;
|
||
break;
|
||
}
|
||
|
||
case ControlMode::TEACH: {
|
||
// 示教模式：仅施加重力补偿力矩，不施加位置刚度
|
||
// 操作者可以自由拖动机械臂
|
||
if (control_.gravity_comp_enable) {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->TorqueControl(gravity_comp_.torques[i]);
|
||
}
|
||
}
|
||
} else {
|
||
// 未使能重力补偿时，示教模式下松弛
|
||
RelaxAllJoints();
|
||
}
|
||
control_.state = MotionState::MOVING;
|
||
break;
|
||
}
|
||
|
||
default:
|
||
control_.state = MotionState::ERROR;
|
||
break;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// ========================================================================
|
||
// 高层API
|
||
// ========================================================================
|
||
|
||
void Enable(bool enable) {
|
||
control_.enable = enable;
|
||
if (!enable) {
|
||
control_.mode = ControlMode::IDLE;
|
||
}
|
||
}
|
||
|
||
void SetMode(ControlMode mode) {
|
||
control_.mode = mode;
|
||
}
|
||
|
||
MotionState GetState() const {
|
||
return control_.state;
|
||
}
|
||
|
||
/**
|
||
* @brief 使能/禁用重力补偿
|
||
* @param enable true=使能, false=禁用
|
||
*/
|
||
void EnableGravityCompensation(bool enable) {
|
||
control_.gravity_comp_enable = enable;
|
||
if (!enable) {
|
||
// 清除补偿力矩
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
gravity_comp_.torques[i] = 0.0f;
|
||
if (joints_[i]) joints_[i]->SetFeedforwardTorque(0.0f);
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 设置重力补偿比例系数（用于微调）
|
||
* @param scale 补偿系数，1.0=完全补偿，<1.0=欠补偿，>1.0=过补偿
|
||
*/
|
||
void SetGravityCompScale(float scale) {
|
||
gravity_comp_.scale = scale;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取指定关节的重力补偿力矩
|
||
*/
|
||
float GetGravityTorque(size_t index) const {
|
||
return (index < JOINT_NUM) ? gravity_comp_.torques[index] : 0.0f;
|
||
}
|
||
|
||
bool IsGravityCompEnabled() const {
|
||
return control_.gravity_comp_enable;
|
||
}
|
||
|
||
// 关节空间控制（直接设置目标关节角度）
|
||
int8_t MoveJoint(const float target_angles[JOINT_NUM]) {
|
||
// 1. 检查限位
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
const auto& dh = joints_[i]->GetParams();
|
||
if (target_angles[i] < dh.qmin || target_angles[i] > dh.qmax) {
|
||
return -1; // 超限
|
||
}
|
||
}
|
||
}
|
||
|
||
// 2. 设置目标角度到各关节
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetTargetAngle(target_angles[i]);
|
||
}
|
||
}
|
||
|
||
control_.state = MotionState::MOVING;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 设置笛卡尔空间目标位姿，自动启动轨迹规划
|
||
* 目标发生变化时，从当前末端位姿重新规划轨迹。
|
||
* 实际运动由 Control() 每帧调用 StepTrajectory() 推进。
|
||
*
|
||
* @param goal 目标末端位姿
|
||
*/
|
||
int8_t MoveCartesian(const Arm6dof_Pose_t& goal) {
|
||
// 检测目标是否发生有效变化（避免每帧重置轨迹）
|
||
constexpr float POS_THRESH = 0.001f; // 1mm
|
||
constexpr float ANG_THRESH = 0.01f; // ~0.57°
|
||
bool goal_changed = IsPoseChanged(goal, traj_.goal, POS_THRESH, ANG_THRESH);
|
||
if (goal_changed) {
|
||
// 从当前末端位姿出发重新规划轨迹
|
||
traj_.start = end_effector_.current;
|
||
traj_.goal = goal;
|
||
traj_.t = 0.0f;
|
||
traj_.active = true;
|
||
traj_.valid = false; // 首帧由 StepTrajectory 用当前关节角作IK初值
|
||
control_.state = MotionState::MOVING;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// 急停
|
||
void Stop() {
|
||
control_.state = MotionState::STOPPED;
|
||
}
|
||
|
||
/**
|
||
* @brief 工具坐标系目标位姿控制
|
||
*
|
||
* 与 MoveCartesian 的区别：
|
||
* - MoveCartesian ：输入世界系绝对位姿
|
||
* - MoveCartesianTool：输入工具坐标系下的目标位姿，内部变换到世界系
|
||
*
|
||
* 变换原理：
|
||
* 位置： p_world = R_cur * [x_t, y_t, z_t]^T
|
||
* 姿态： R_world = R_cur * R_target
|
||
*
|
||
* @param target_tool 工具坐标系下的目标位姿（绝对值）
|
||
* .x/.y/.z ：工具系下的目标位置 (m)
|
||
* .roll/.pitch/.yaw ：工具系下的目标姿态 (rad)
|
||
*/
|
||
void MoveCartesianTool(const Arm6dof_Pose_t& target_tool) {
|
||
// 仅当用户输入目标发生变化时才重新换算世界系目标
|
||
// 若输入不变（用户未推杆），复用缓存的世界系目标，保证轨迹可以连续推进至 t=1
|
||
constexpr float POS_EPS = 0.001f;
|
||
constexpr float ANG_EPS = 0.01f;
|
||
const FrameCache_t& tc = tool_frame_cache_;
|
||
bool input_changed = !tc.valid || IsPoseChanged(target_tool, tc.last_input, POS_EPS, ANG_EPS);
|
||
|
||
if (input_changed) {
|
||
// 用当前末端位姿做坐标系变换（仅在用户改变目标时触发一次）
|
||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||
float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
|
||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||
|
||
Arm6dof_Pose_t goal;
|
||
float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z};
|
||
Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr);
|
||
goal.x = p_world[0][0];
|
||
goal.y = p_world[1][0];
|
||
goal.z = p_world[2][0];
|
||
|
||
float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll};
|
||
Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
|
||
goal.yaw = rpy_new[0][0];
|
||
goal.pitch = rpy_new[1][0];
|
||
goal.roll = rpy_new[2][0];
|
||
|
||
tool_frame_cache_.last_input = target_tool;
|
||
tool_frame_cache_.world_goal = goal;
|
||
tool_frame_cache_.valid = true;
|
||
}
|
||
|
||
MoveCartesian(tool_frame_cache_.world_goal);
|
||
}
|
||
|
||
/**
|
||
* @brief 航向参考系目标位姿控制
|
||
*
|
||
* 位置（航向系）：世界系绕Z轴旋转当前yaw角度得到，X/Y始终水平，Z始终世界竖直
|
||
* 姿态（航向系）：roll/pitch/yaw 为末端在航向系下的绝对目标姿态，直观地对应末端各轴旋转
|
||
*
|
||
* 变换原理：
|
||
* 位置 X/Y：p_world = Rz(yaw) * [x, y, 0]^T
|
||
* 位置 Z ：p_world.z = z (世界竖直直接赋值)
|
||
* 姿态 ：R_world = Rz(yaw) * R_target （航向系下绝对姿态→世界系）
|
||
*
|
||
* @param target_heading
|
||
* .x/.y ：航向系（水平）下的目标位置 (m)
|
||
* .z ：世界竖直方向目标高度 (m)
|
||
* .roll/.pitch/.yaw ：航向系下的末端绝对姿态 (rad)
|
||
* 直观含义：yaw转动就是末端 yaw，pitch/roll 同理
|
||
*/
|
||
void MoveCartesianHeading(const Arm6dof_Pose_t& target_heading) {
|
||
// 仅当用户输入目标发生变化时才重新换算世界系目标（同 MoveCartesianTool 原理）
|
||
constexpr float POS_EPS = 0.001f;
|
||
constexpr float ANG_EPS = 0.01f;
|
||
const FrameCache_t& hc = heading_frame_cache_;
|
||
bool input_changed = !hc.valid || IsPoseChanged(target_heading, hc.last_input, POS_EPS, ANG_EPS);
|
||
|
||
if (input_changed) {
|
||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
|
||
Arm6dof_Pose_t goal;
|
||
goal.x = cy * target_heading.x - sy * target_heading.y;
|
||
goal.y = sy * target_heading.x + cy * target_heading.y;
|
||
goal.z = target_heading.z;
|
||
|
||
float rpy_yaw_arr[3] = {cur.yaw, 0.0f, 0.0f};
|
||
float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll};
|
||
Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr))
|
||
* robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
|
||
goal.yaw = rpy_new[0][0];
|
||
goal.pitch = rpy_new[1][0];
|
||
goal.roll = rpy_new[2][0];
|
||
|
||
heading_frame_cache_.last_input = target_heading;
|
||
heading_frame_cache_.world_goal = goal;
|
||
heading_frame_cache_.valid = true;
|
||
}
|
||
|
||
MoveCartesian(heading_frame_cache_.world_goal);
|
||
}
|
||
|
||
// 获取末端位姿（FK结果，单位 m/rad）
|
||
const Arm6dof_Pose_t& GetEndPose() const {
|
||
return end_effector_.current;
|
||
}
|
||
|
||
// 获取最近一次 IK 解算结果
|
||
const Arm6dof_JointAngles_t& GetIkAngles() const {
|
||
return inverseKinematics_.angles;
|
||
}
|
||
|
||
// 获取最近一次 IK 是否有效
|
||
bool IsIkValid() const {
|
||
return inverseKinematics_.valid;
|
||
}
|
||
|
||
/** 设置末端线速度限制 (m/s)，默认 0.15 m/s */
|
||
void SetLinVelLimit(float vel) { traj_.max_lin_vel = vel; }
|
||
|
||
/** 设置末端角速度限制 (rad/s)，默认 1.0 rad/s */
|
||
void SetAngVelLimit(float vel) { traj_.max_ang_vel = vel; }
|
||
|
||
/** 获取当前轨迹进度 [0.0, 1.0]，1.0 表示已到达目标 */
|
||
float GetTrajProgress() const { return traj_.t; }
|
||
|
||
/** 轨迹是否正在执行（false 表示已到达目标或尚未启动） */
|
||
bool IsTrajActive() const { return traj_.active; }
|
||
|
||
/** 清除错误状态并同步目标位姿到当前位姿（错误恢复用） */
|
||
void ResetError() {
|
||
control_.state = MotionState::REACHED;
|
||
control_.enable = true;
|
||
inverseKinematics_.valid = true;
|
||
traj_.valid = true;
|
||
traj_.active = false;
|
||
|
||
Arm6dof_JointAngles_t q;
|
||
FillCurrentJointAngles(q);
|
||
SetJointTargetsToCurrent();
|
||
inverseKinematics_.angles = q;
|
||
|
||
|
||
// 将轨迹目标同步为当前位姿，防止下次启动时从旧目标出发
|
||
traj_.goal = end_effector_.current;
|
||
end_effector_.target = end_effector_.current;
|
||
}
|
||
};
|
||
|
||
} // namespace arm
|