707 lines
26 KiB
C++
707 lines
26 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.5// 关节角度误差容限（rad），用于IK解的有效性验证
|
||
namespace arm {
|
||
|
||
// 控制模式
|
||
enum class ControlMode {
|
||
IDLE = 0,
|
||
JOINT_POSITION,
|
||
CARTESIAN_POSITION,
|
||
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_;
|
||
|
||
// 时间管理
|
||
struct {
|
||
float now;
|
||
uint64_t last_wakeup;
|
||
float dt;
|
||
} timer_;
|
||
|
||
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(&traj_.start, 0, sizeof(traj_.start));
|
||
memset(&traj_.goal, 0, sizeof(traj_.goal));
|
||
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() 调整
|
||
|
||
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->Update();
|
||
}
|
||
}
|
||
|
||
// 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取）
|
||
Arm6dof_JointAngles_t q;
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||
}
|
||
|
||
// 正运动学：计算当前末端位姿
|
||
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 轨迹规划核心：每帧插值一小步并对插值点求IK
|
||
*
|
||
* 每帧执行流程：
|
||
* 1. 按速度限制计算本帧 t 步进量 Δt = v_max * dt / 总距离
|
||
* 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标
|
||
* 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛
|
||
* 4. 验证 + 写入 inverseKinematics_.angles
|
||
*/
|
||
void StepTrajectory() {
|
||
if (!traj_.active) return;
|
||
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
||
|
||
// 计算总距离（用于将速度限制转换为 t 步进量）
|
||
float dx = traj_.goal.x - traj_.start.x;
|
||
float dy = traj_.goal.y - traj_.start.y;
|
||
float dz = traj_.goal.z - traj_.start.z;
|
||
float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz);
|
||
|
||
float dr = traj_.goal.roll - traj_.start.roll;
|
||
float dp = traj_.goal.pitch - traj_.start.pitch;
|
||
float dyw = traj_.goal.yaw - traj_.start.yaw;
|
||
// 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯
|
||
dr -= roundf(dr / TWO_PI) * TWO_PI;
|
||
dp -= roundf(dp / TWO_PI) * TWO_PI;
|
||
dyw -= roundf(dyw / TWO_PI) * TWO_PI;
|
||
float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw);
|
||
|
||
// Δt = v_max * dt / 总距离
|
||
// 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进
|
||
// 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完
|
||
float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f;
|
||
float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f;
|
||
float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束）
|
||
|
||
traj_.t += dt_step;
|
||
if (traj_.t >= 1.0f) {
|
||
traj_.t = 1.0f;
|
||
traj_.active = false; // 到达终点
|
||
}
|
||
|
||
// 线性插值：P(t) = start + t*(goal-start)
|
||
Arm6dof_Pose_t interp;
|
||
interp.x = traj_.start.x + traj_.t * dx;
|
||
interp.y = traj_.start.y + traj_.t * dy;
|
||
interp.z = traj_.start.z + traj_.t * dz;
|
||
interp.roll = traj_.start.roll + traj_.t * dr;
|
||
interp.pitch = traj_.start.pitch + traj_.t * dp;
|
||
interp.yaw = traj_.start.yaw + traj_.t * dyw;
|
||
|
||
// IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支）
|
||
Arm6dof_JointAngles_t q_init;
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||
}
|
||
|
||
if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
|
||
traj_.valid = false;
|
||
traj_.active = false;
|
||
control_.state = MotionState::ERROR;
|
||
return;
|
||
}
|
||
|
||
// 角度连续性修正：将IK解调整到与当前关节角最近的等效角
|
||
// 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义：
|
||
// IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。
|
||
// 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。
|
||
// 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）：
|
||
// 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (!joints_[i]) continue;
|
||
const auto& dh = joints_[i]->GetDHParams();
|
||
// 关节范围跨度不足 2π——无等效分支，无需修正
|
||
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue;
|
||
|
||
float q_ik = traj_.angles.q[i];
|
||
float q_cur = q_init.q[i];
|
||
float diff = q_ik - q_cur;
|
||
// roundf(diff / 2π) * 2π = 最近的 2π 整数倍，减去后差值落在 (-π, π]
|
||
diff -= roundf(diff / TWO_PI) * TWO_PI;
|
||
traj_.angles.q[i] = q_cur + diff;
|
||
}
|
||
|
||
// 限位检查
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
const auto& dh = joints_[i]->GetDHParams();
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
// // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常
|
||
// // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
|
||
// static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
|
||
// for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
// if (joints_[i]) {
|
||
// float delta = fabsf(traj_.angles.q[i] -
|
||
// joints_[i]->GetCurrentAngle());
|
||
// 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) {
|
||
for (Joint* joint : joints_) { // 遍历关节数组
|
||
if (joint) joint->Relax();
|
||
}
|
||
control_.state = MotionState::STOPPED;
|
||
return 0;
|
||
}
|
||
|
||
// 根据控制模式执行
|
||
switch (control_.mode) {
|
||
case ControlMode::IDLE:
|
||
for (Joint* joint : joints_) { // 遍历关节数组
|
||
if (joint) joint->Relax();
|
||
}
|
||
control_.state = MotionState::STOPPED;
|
||
break;
|
||
|
||
case ControlMode::JOINT_POSITION:
|
||
case ControlMode::CARTESIAN_POSITION:
|
||
case ControlMode::GRAVITY_COMP: {
|
||
// 检查错误状态（不可达或超限）
|
||
if (control_.state == MotionState::ERROR) {
|
||
// 进入错误状态，松弛所有关节（安全停止）
|
||
for (Joint* joint : joints_) {
|
||
if (joint) joint->Relax();
|
||
}
|
||
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) {
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||
}
|
||
}
|
||
}
|
||
|
||
// CARTESIAN_POSITION 模式：每帧推进轨迹一小步，再应用IK解到关节
|
||
if (control_.mode == ControlMode::CARTESIAN_POSITION) {
|
||
// 轨迹规划核心调用：插值 + 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解无效：松弛所有关节并进入错误状态
|
||
for (Joint* joint : joints_) {
|
||
if (joint) joint->Relax();
|
||
}
|
||
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 {
|
||
// 未使能重力补偿时，示教模式下松弛
|
||
for (Joint* joint : joints_) {
|
||
if (joint) joint->Relax();
|
||
}
|
||
}
|
||
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]->GetDHParams();
|
||
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 =
|
||
fabsf(goal.x - traj_.goal.x) > POS_THRESH ||
|
||
fabsf(goal.y - traj_.goal.y) > POS_THRESH ||
|
||
fabsf(goal.z - traj_.goal.z) > POS_THRESH ||
|
||
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
|
||
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
|
||
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
|
||
|
||
if (goal_changed) {
|
||
// 从当前末端位姿出发重新规划轨迹
|
||
traj_.start = end_effector_.current;
|
||
traj_.goal = goal;
|
||
traj_.t = 0.0f;
|
||
traj_.active = true;
|
||
traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory
|
||
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) {
|
||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||
|
||
// ── R_cur = Rz(yaw)*Ry(pitch)*Rx(roll) ──
|
||
float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
|
||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||
|
||
// ── 位置：p_world = R_cur * p_tool（绝对目标，工具系坐标→世界系） ──
|
||
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];
|
||
|
||
// ── 姿态：R_world = R_cur * R_target（工具系绝对姿态→世界系） ──
|
||
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));
|
||
|
||
// ── R_new → RPY，写入目标位姿 ──
|
||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
|
||
goal.yaw = rpy_new[0][0];
|
||
goal.pitch = rpy_new[1][0];
|
||
goal.roll = rpy_new[2][0];
|
||
|
||
MoveCartesian(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) {
|
||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||
|
||
// ── 位置：航向系→世界系，Rz(yaw) 旋转水平分量，Z 直接为世界Z ──
|
||
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
|
||
Arm6dof_Pose_t goal;
|
||
goal.x = cy * target_heading.x - sy * target_heading.y; // 世界X
|
||
goal.y = sy * target_heading.x + cy * target_heading.y; // 世界Y
|
||
goal.z = target_heading.z; // 世界Z 直接赋值
|
||
|
||
// ── 姿态：航向系下绝对姿态→世界系：R_world = Rz(yaw) * R_target ──
|
||
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));
|
||
|
||
// ── R_new → RPY，写入目标位姿 ──
|
||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
|
||
goal.yaw = rpy_new[0][0];
|
||
goal.pitch = rpy_new[1][0];
|
||
goal.roll = rpy_new[2][0];
|
||
|
||
MoveCartesian(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;
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||
}
|
||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||
if (joints_[i]) {
|
||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||
}
|
||
}
|
||
inverseKinematics_.angles = q;
|
||
|
||
|
||
// 将轨迹目标同步为当前位姿，防止下次启动时从旧目标出发
|
||
traj_.goal = end_effector_.current;
|
||
end_effector_.target = end_effector_.current;
|
||
}
|
||
};
|
||
|
||
} // namespace arm
|