957 lines
35 KiB
C++
957 lines
35 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 scales[JOINT_NUM]; // 每关节补偿系数（用于独立微调）
|
||
} 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_.scales[i] = 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 底层返回 raw_torques 后，再按每关节系数 scales[i] 独立缩放
|
||
*/
|
||
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_.scales[i];
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 位置优先的粗解耦逆运动学（J1~J3主控，J4~J6保持）
|
||
* @details
|
||
* - 目标：实现“上下左右/前后”粗到位，先到目标附近，再手动微调腕部。
|
||
* - 求解：仅对前三关节做 3x3 阻尼最小二乘迭代，误差用末端位置误差。
|
||
* - 姿态：不强制跟踪输入姿态，J4~J6保持 q_seed（或当前角度）不变。
|
||
*/
|
||
int8_t InverseKinematicsAnalytical(const Arm6dof_Pose_t* pose,
|
||
Arm6dof_JointAngles_t* q_out,
|
||
const Arm6dof_JointAngles_t* q_seed = nullptr) {
|
||
if (!pose || !q_out) return -1;
|
||
|
||
// 以 q_seed 为优先初值，保证轨迹连续；无 seed 时读取当前关节角。
|
||
Arm6dof_JointAngles_t seed;
|
||
if (q_seed) {
|
||
seed = *q_seed;
|
||
|
||
} else {
|
||
FillCurrentJointAngles(seed);
|
||
}
|
||
|
||
Arm6dof_JointAngles_t q = seed;
|
||
auto clampToLimit = [&](size_t idx, float qv) -> float {
|
||
if (!joints_[idx]) return qv;
|
||
const auto& p = joints_[idx]->GetParams();
|
||
if (qv < p.qmin) qv = p.qmin;
|
||
if (qv > p.qmax) qv = p.qmax;
|
||
return qv;
|
||
};
|
||
|
||
auto solve3x3 = [](const float A[3][3], const float b[3], float x[3]) -> bool {
|
||
float aug[3][4] = {
|
||
{A[0][0], A[0][1], A[0][2], b[0]},
|
||
{A[1][0], A[1][1], A[1][2], b[1]},
|
||
{A[2][0], A[2][1], A[2][2], b[2]}
|
||
};
|
||
|
||
for (int col = 0; col < 3; ++col) {
|
||
int pivot = col;
|
||
float max_abs = fabsf(aug[col][col]);
|
||
for (int row = col + 1; row < 3; ++row) {
|
||
float v = fabsf(aug[row][col]);
|
||
if (v > max_abs) {
|
||
max_abs = v;
|
||
pivot = row;
|
||
}
|
||
}
|
||
if (max_abs < 1e-8f) {
|
||
return false;
|
||
}
|
||
if (pivot != col) {
|
||
for (int k = col; k < 4; ++k) {
|
||
float tmp = aug[col][k];
|
||
aug[col][k] = aug[pivot][k];
|
||
aug[pivot][k] = tmp;
|
||
}
|
||
}
|
||
|
||
float diag = aug[col][col];
|
||
for (int k = col; k < 4; ++k) {
|
||
aug[col][k] /= diag;
|
||
}
|
||
for (int row = 0; row < 3; ++row) {
|
||
if (row == col) continue;
|
||
float f = aug[row][col];
|
||
for (int k = col; k < 4; ++k) {
|
||
aug[row][k] -= f * aug[col][k];
|
||
}
|
||
}
|
||
}
|
||
|
||
x[0] = aug[0][3];
|
||
x[1] = aug[1][3];
|
||
x[2] = aug[2][3];
|
||
return true;
|
||
};
|
||
|
||
constexpr float eps = 1e-3f;
|
||
constexpr float lambda = 3e-3f;
|
||
constexpr float max_joint_step = 0.12f; // 单次迭代关节最大更新量(rad)
|
||
constexpr int max_iter = 20;
|
||
constexpr float pos_tol = 0.015f; // 1.5cm 认为到达目标附近
|
||
constexpr float fallback_tol = 0.05f; // 5cm 内也视作可接受粗到位
|
||
|
||
float best_err = 1e9f;
|
||
Arm6dof_JointAngles_t best_q = q;
|
||
|
||
for (int iter = 0; iter < max_iter; ++iter) {
|
||
Arm6dof_Pose_t cur_pose;
|
||
if (Arm6dof_ForwardKinematics(&q, &cur_pose) != 0) {
|
||
break;
|
||
}
|
||
|
||
float ex = pose->x - cur_pose.x;
|
||
float ey = pose->y - cur_pose.y;
|
||
float ez = pose->z - cur_pose.z;
|
||
float err_norm = sqrtf(ex * ex + ey * ey + ez * ez);
|
||
|
||
if (err_norm < best_err) {
|
||
best_err = err_norm;
|
||
best_q = q;
|
||
}
|
||
if (err_norm <= pos_tol) {
|
||
break;
|
||
}
|
||
|
||
// 仅对 J1~J3 构造位置雅可比，J4~J6保持不动。
|
||
float J[3][3];
|
||
for (int j = 0; j < 3; ++j) {
|
||
Arm6dof_JointAngles_t q_pert = q;
|
||
q_pert.q[j] += eps;
|
||
Arm6dof_Pose_t pose_pert;
|
||
if (Arm6dof_ForwardKinematics(&q_pert, &pose_pert) != 0) {
|
||
return -1;
|
||
}
|
||
J[0][j] = (pose_pert.x - cur_pose.x) / eps;
|
||
J[1][j] = (pose_pert.y - cur_pose.y) / eps;
|
||
J[2][j] = (pose_pert.z - cur_pose.z) / eps;
|
||
}
|
||
|
||
// 阻尼最小二乘：dq = (J^T J + λI)^-1 J^T e
|
||
float H[3][3] = {{0}};
|
||
float g[3] = {0};
|
||
float e[3] = {ex, ey, ez};
|
||
for (int r = 0; r < 3; ++r) {
|
||
for (int c = 0; c < 3; ++c) {
|
||
H[r][c] = J[0][r] * J[0][c] + J[1][r] * J[1][c] + J[2][r] * J[2][c];
|
||
}
|
||
H[r][r] += lambda;
|
||
g[r] = J[0][r] * e[0] + J[1][r] * e[1] + J[2][r] * e[2];
|
||
}
|
||
|
||
float dq[3] = {0};
|
||
if (!solve3x3(H, g, dq)) {
|
||
break;
|
||
}
|
||
|
||
for (int j = 0; j < 3; ++j) {
|
||
if (dq[j] > max_joint_step) dq[j] = max_joint_step;
|
||
if (dq[j] < -max_joint_step) dq[j] = -max_joint_step;
|
||
q.q[j] = clampToLimit((size_t)j, q.q[j] + dq[j]);
|
||
}
|
||
}
|
||
|
||
if (best_err > fallback_tol) {
|
||
return -1;
|
||
}
|
||
|
||
// 维持腕部姿态关节，便于用户后续遥杆精细操作。
|
||
best_q.q[3] = seed.q[3];
|
||
best_q.q[4] = seed.q[4];
|
||
best_q.q[5] = seed.q[5];
|
||
|
||
// 最终写回前再次进行限位钳制。
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
best_q.q[i] = clampToLimit(i, best_q.q[i]);
|
||
}
|
||
|
||
*q_out = best_q;
|
||
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, &q_init) == 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]);
|
||
}
|
||
}
|
||
}
|
||
|
||
if(control_.mode == ControlMode::JOINT_POSITION) {
|
||
// JOINT_POSITION 模式由上层写入目标角，这里做边界校验与状态同步
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (!joints_[i]) {
|
||
continue;
|
||
}
|
||
const float target = joints_[i]->GetTargetAngle();
|
||
const auto& p = joints_[i]->GetParams();
|
||
if (!std::isfinite(target) || target < p.qmin || target > p.qmax) {
|
||
RelaxAllJoints();
|
||
control_.state = MotionState::ERROR;
|
||
return -1;
|
||
}
|
||
inverseKinematics_.angles.q[i] = target;
|
||
}
|
||
inverseKinematics_.valid = true;
|
||
traj_.valid = true;
|
||
traj_.active = false;
|
||
}
|
||
|
||
// 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 设置全部关节统一补偿系数（兼容旧接口）
|
||
*/
|
||
void SetGravityCompScale(float scale) {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
gravity_comp_.scales[i] = scale;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 设置单个关节补偿系数
|
||
*/
|
||
void SetGravityCompScale(size_t index, float scale) {
|
||
if (index < JOINT_NUM) {
|
||
gravity_comp_.scales[index] = scale;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 批量设置6个关节补偿系数
|
||
*/
|
||
void SetGravityCompScales(const float scales[JOINT_NUM]) {
|
||
if (!scales) return;
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
gravity_comp_.scales[i] = scales[i];
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 获取单个关节补偿系数
|
||
*/
|
||
float GetGravityCompScale(size_t index) const {
|
||
return (index < JOINT_NUM) ? gravity_comp_.scales[index] : 1.0f;
|
||
}
|
||
|
||
/**
|
||
* @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
|