xm/arm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

123

Browse Source
This commit is contained in:
xxxxm 2026-03-20 00:20:40 +08:00
parent 9f59072aba
commit bf2633c3de
8 changed files with 526 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

300
User/component/arm_kinematics/arm6dof.cpp
View File

@ -16,29 +16,141 @@ static robotics::Link* s_links = nullptr;
static robotics::Serial_Link<6>* s_robot = nullptr;
static bool s_initialized = false;
/**
* dh_params = [
{"a": 0, "alpha": pi / 2, "d": 0, "theta_offset": 0}, # Joint 1
{"a": 0.404, "alpha": 0.0, "d": 0, "theta_offset": 0}, # Joint 2
{"a": 0.091579, "alpha": -pi / 2, "d": 0, "theta_offset": pi / 2}, # Joint 3
{"a": 0, "alpha": pi / 2, "d": 0.2411, "theta_offset": 0},
{"a": 0, "alpha": -pi / 2, "d": 0, "theta_offset": 0},
{"a": 0, "alpha": 0, "d": 0.1094, "theta_offset": 0},
{"a": 0.0577, "alpha": pi / 2, "d": 0.033001, "theta_offset": 0},
{"a": 0.05, "alpha": 0, "d": 0, "theta_offset": 0},
]
* @brief 根据参数创建DH参数链表
* @note 标准DH参数，参考 DH_PARAMETERS.md 文档
*
* DH参数表（标准DH）：
* 关节 | θᵢ(变量) | dᵢ | aᵢ | αᵢ | 说明
* -----|----------|-------|------|--------|------------------
* 1 | q1 | h0 | 0 | π/2 | 底座旋转（Yaw）
* 2 | q2 | 0 | L1 | 0 | 肩部俯仰（Pitch）
* 3 | q3 | 0 | L2 | 0 | 肘部俯仰（Pitch）
* 4 | q4 | d4 | 0 | π/2 | 腕部滚转（Roll）
* 5 | q5 | 0 | 0 | -π/2 | 腕部俯仰（Pitch）
* 6 | q6 | d6 | 0 | 0 | 末端旋转（Roll）
// 当前机械臂URDF(j1~j6)关节原始定义：parent->child 先 origin，再绕 joint axis 旋转。
// 这里显式按URDF链式连乘实现FK，避免将URDF joint origin强行映射到标准DH后产生几何失真。
static const float s_urdf_joint_origin_xyz[6][3] = {
{0.0f, 0.0f, 0.024f},
{-0.001395f,0.0f, 0.1015f},
{0.3265f, 0.0f, -0.0071975f},
{0.0905f, 0.052775f, 0.0058025f},
{0.001627f, 0.0f, 0.18467f},
{0.10487f, 0.0013347f,0.0f},
};
// URDF顺序为(roll, pitch, yaw)。robotics::rpy2r输入顺序为(yaw, pitch, roll)。
static const float s_urdf_joint_origin_rpy[6][3] = {
{0.0f, 0.0f, 0.0f},
{1.5708f, 0.0f, -1.5708f},
{0.0f, 0.0f, 0.0f},
{1.5708f, 0.0f, 3.1416f},
{1.5708f, 0.0f, 0.0f},
{1.5708f, 0.0f, 1.5708f},
};
// Link1~Link6 质心在各自link坐标系下的位置（直接取自 xin.urdf 的 inertial/origin）。
static const float s_urdf_link_com_xyz[6][3] = {
{-0.0071819f, 0.00031034f, 0.061598f},
{ 0.22947f, -0.0031632f, -0.0022707f},
{ 0.086963f, 0.0016734f, -0.017806f},
{ 0.000055295f,-0.002302704f,0.10972717f},
{ 0.056542f, 0.0013106f, 0.0010268f},
{-0.0000077997f,0.000012767f,0.10159f},
};
static Matrixf<4, 4> Arm6dof_UrdfJointTransform(uint8_t index, float q) {
const float roll = s_urdf_joint_origin_rpy[index][0];
const float pitch = s_urdf_joint_origin_rpy[index][1];
const float yaw = s_urdf_joint_origin_rpy[index][2];
float ypr_origin[3] = {yaw, pitch, roll};
Matrixf<3, 3> R_origin = robotics::rpy2r(Matrixf<3, 1>(ypr_origin));
float p_raw[3] = {
s_urdf_joint_origin_xyz[index][0],
s_urdf_joint_origin_xyz[index][1],
s_urdf_joint_origin_xyz[index][2],
};
Matrixf<3, 1> p_origin(p_raw);
// 当前URDF关节轴均为 local-z：R_joint = RotZ(q)
float ypr_joint[3] = {q, 0.0f, 0.0f};
Matrixf<3, 3> R_joint = robotics::rpy2r(Matrixf<3, 1>(ypr_joint));
return robotics::rp2t(R_origin, p_origin)
* robotics::rp2t(R_joint, matrixf::zeros<3, 1>());
}
static Matrixf<4, 4> Arm6dof_UrdfForwardTransform(const Arm6dof_JointAngles_t* q,
uint8_t joint_num) {
Matrixf<4, 4> T = matrixf::eye<4, 4>();
for (uint8_t i = 0; i < joint_num; ++i) {
T = T * Arm6dof_UrdfJointTransform(i, q->q[i]);
}
return T;
}
static float Arm6dof_UrdfPotentialEnergy(const Arm6dof_JointAngles_t* q) {
constexpr float g = 9.81f;
float U = 0.0f;
Matrixf<4, 4> T_link = matrixf::eye<4, 4>();
for (uint8_t i = 0; i < 6; ++i) {
T_link = T_link * Arm6dof_UrdfJointTransform(i, q->q[i]);
float p_com_raw[3] = {
s_urdf_link_com_xyz[i][0],
s_urdf_link_com_xyz[i][1],
s_urdf_link_com_xyz[i][2],
};
Matrixf<3, 1> p_com_local(p_com_raw);
Matrixf<4, 4> T_com = T_link * robotics::p2t(p_com_local);
Matrixf<3, 1> p_com_world = robotics::t2p(T_com);
U += s_links[i].m() * g * p_com_world[2][0];
}
return U;
}
static Matrixf<6, 1> Arm6dof_UrdfPoseError(const Matrixf<4, 4>& T_target,
const Matrixf<4, 4>& T_cur) {
Matrixf<6, 1> err;
Matrixf<3, 1> pe = robotics::t2p(T_target) - robotics::t2p(T_cur);
Matrixf<3, 1> we = robotics::t2twist(T_target * robotics::invT(T_cur)).block<3, 1>(3, 0);
for (int k = 0; k < 3; ++k) {
err[k][0] = pe[k][0];
err[k + 3][0] = we[k][0];
}
return err;
}
static Matrixf<6, 6> Arm6dof_UrdfNumericJacobian(const Arm6dof_JointAngles_t* q) {
Matrixf<6, 6> J = matrixf::zeros<6, 6>();
const float eps = 1e-4f;
for (int i = 0; i < 6; ++i) {
Arm6dof_JointAngles_t q_plus = *q;
Arm6dof_JointAngles_t q_minus = *q;
q_plus.q[i] += eps;
q_minus.q[i] -= eps;
Matrixf<4, 4> T_plus = Arm6dof_UrdfForwardTransform(&q_plus, 6);
Matrixf<4, 4> T_minus = Arm6dof_UrdfForwardTransform(&q_minus, 6);
Matrixf<3, 1> p_plus = robotics::t2p(T_plus);
Matrixf<3, 1> p_minus = robotics::t2p(T_minus);
Matrixf<3, 1> dp = (p_plus - p_minus) / (2.0f * eps);
Matrixf<3, 3> R_plus = robotics::t2r(T_plus);
Matrixf<3, 3> R_minus = robotics::t2r(T_minus);
Matrixf<3, 3> R_rel = R_plus * R_minus.trans();
Matrixf<4, 1> angvec = robotics::r2angvec(R_rel);
Matrixf<3, 1> w;
w[0][0] = angvec[0][0] * angvec[3][0] / (2.0f * eps);
w[1][0] = angvec[1][0] * angvec[3][0] / (2.0f * eps);
w[2][0] = angvec[2][0] * angvec[3][0] / (2.0f * eps);
J[0][i] = dp[0][0];
J[1][i] = dp[1][0];
J[2][i] = dp[2][0];
J[3][i] = w[0][0];
J[4][i] = w[1][0];
J[5][i] = w[2][0];
}
return J;
}
/**
* @brief 初始化机械臂运动学模型
@ -92,14 +204,8 @@ int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* po
return -1;
}
// 将关节角度转换为矩阵格式
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// 计算正运动学
Matrixf<4, 4> T = s_robot->fkine(q_mat);
// 使用URDF原生链式FK：T = Π( T_origin_i * RotZ(q_i) )
Matrixf<4, 4> T = Arm6dof_UrdfForwardTransform(q, 6);
// 提取位置 (x, y, z)
Matrixf<3, 1> pos = robotics::t2p(T);
@ -128,14 +234,8 @@ int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6
return -1;
}
// 将关节角度转换为矩阵格式
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// 计算到第k个关节的变换矩阵
Matrixf<4, 4> T = s_robot->fkine(q_mat, joint_num);
// 计算到第k个关节的URDF链式变换矩阵
Matrixf<4, 4> T = Arm6dof_UrdfForwardTransform(q, joint_num);
// 提取位置和姿态
Matrixf<3, 1> pos = robotics::t2p(T);
@ -182,16 +282,84 @@ int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAng
}
}
// 调用toolbox的逆运动学求解器（牛顿法）
Matrixf<6, 1> q_solved = s_robot->ikine(T_target, q_guess, tol, max_iter);
// 基于URDF链式FK的LM数值迭代，避免FK/IK模型不一致。
Matrixf<6, 1> q_cur = q_guess;
float lambda = 1.0f;
constexpr float lambda_up = 10.0f;
constexpr float lambda_down = 0.1f;
constexpr float lambda_max = 1e6f;
constexpr float lambda_min = 1e-6f;
// 将结果复制到输出
for (int i = 0; i < 6; i++) {
q_result->q[i] = q_solved[i][0];
for (uint16_t iter = 0; iter < max_iter; ++iter) {
Arm6dof_JointAngles_t q_cur_struct;
for (int i = 0; i < 6; ++i) {
q_cur_struct.q[i] = q_cur[i][0];
}
// 验证解的精度：位置误差 + 姿态误差双y重校验
Matrixf<4, 4> T_verify = s_robot->fkine(q_solved);
Matrixf<4, 4> T_cur = Arm6dof_UrdfForwardTransform(&q_cur_struct, 6);
Matrixf<6, 1> err = Arm6dof_UrdfPoseError(T_target, T_cur);
float err_norm = err.norm();
if (err_norm < tol) {
break;
}
Matrixf<6, 6> J = Arm6dof_UrdfNumericJacobian(&q_cur_struct);
Matrixf<6, 6> H = J.trans() * J + lambda * matrixf::eye<6, 6>();
Matrixf<6, 1> g = J.trans() * err;
Matrixf<6, 1> dq = matrixf::inv(H) * g;
if (dq[0][0] == INFINITY || dq[0][0] != dq[0][0]) {
lambda *= lambda_up;
if (lambda > lambda_max) {
return -1;
}
continue;
}
Matrixf<6, 1> q_new = q_cur + dq;
// 对循环关节折叠，避免等效角导致数值发散。
for (int i = 0; i < 6; ++i) {
constexpr float TWO_PI = 2.0f * (float)M_PI;
bool is_cyclic = (s_links[i].qmin_ < 0.1f) && (s_links[i].qmax_ > TWO_PI - 0.1f);
if (is_cyclic) {
q_new[i][0] = math::loopLimit(q_new[i][0], s_links[i].qmin_, s_links[i].qmax_);
}
}
Arm6dof_JointAngles_t q_new_struct;
for (int i = 0; i < 6; ++i) {
q_new_struct.q[i] = q_new[i][0];
}
Matrixf<4, 4> T_new = Arm6dof_UrdfForwardTransform(&q_new_struct, 6);
Matrixf<6, 1> new_err = Arm6dof_UrdfPoseError(T_target, T_new);
float new_err_norm = new_err.norm();
if (new_err_norm < err_norm) {
q_cur = q_new;
lambda *= lambda_down;
if (lambda < lambda_min) {
lambda = lambda_min;
}
} else {
lambda *= lambda_up;
if (lambda > lambda_max) {
return -1;
}
}
}
for (int i = 0; i < 6; ++i) {
q_result->q[i] = q_cur[i][0];
}
// 验证解的精度：位置误差 + 姿态误差双重校验
Arm6dof_JointAngles_t q_verify_struct;
for (int i = 0; i < 6; ++i) {
q_verify_struct.q[i] = q_cur[i][0];
}
Matrixf<4, 4> T_verify = Arm6dof_UrdfForwardTransform(&q_verify_struct, 6);
Matrixf<3, 1> p_verify = robotics::t2p(T_verify);
float pos_error = (p_verify - p_target).norm();
@ -221,29 +389,41 @@ int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAng
/**
* @brief 计算重力补偿力矩
* @details 利用牛顿-欧拉逆动力学(rne)，设速度qv=0、加速度qa=0，
* 计算的关节力矩即为平衡重力所需的力矩。
* @details 与当前URDF链式运动学保持一致：通过势能 U(q)=Σ(mgz) 的数值梯度
* τg = ∂U/∂q 计算重力补偿力矩。
*/
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]) {
if (!s_initialized || q == nullptr || gravity_torques == nullptr) {
return -1;
}
// 关节角度
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// // 关节角度
// Matrixf<6, 1> q_mat;
// for (int i = 0; i < 6; i++) {
// q_mat[i][0] = q->q[i];
// }
// 速度和加速度均为零 —— 只计算重力项
Matrixf<6, 1> qv = matrixf::zeros<6, 1>();
Matrixf<6, 1> qa = matrixf::zeros<6, 1>();
// // 速度和加速度均为零 —— 只计算重力项
// Matrixf<6, 1> qv = matrixf::zeros<6, 1>();
// Matrixf<6, 1> qa = matrixf::zeros<6, 1>();
// 调用牛顿-欧拉逆动力学
Matrixf<6, 1> torques = s_robot->rne(q_mat, qv, qa);
// // 调用牛顿-欧拉逆动力学
// Matrixf<6, 1> torques = s_robot->rne(q_mat, qv, qa);
for (int i = 0; i < 6; i++) {
gravity_torques[i] = torques[i][0];
// for (int i = 0; i < 6; i++) {
// gravity_torques[i] = torques[i][0];
// }
constexpr float eps = 1e-4f;
for (int i = 0; i < 6; ++i) {
Arm6dof_JointAngles_t q_plus = *q;
Arm6dof_JointAngles_t q_minus = *q;
q_plus.q[i] += eps;
q_minus.q[i] -= eps;
float U_plus = Arm6dof_UrdfPotentialEnergy(&q_plus);
float U_minus = Arm6dof_UrdfPotentialEnergy(&q_minus);
gravity_torques[i] = (U_plus - U_minus) / (2.0f * eps);
}
return 0;

33
User/device/motor_dm.c
View File

@ -109,13 +109,22 @@ static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output)
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
float cmd_pos = output->angle;
float cmd_vel = output->velocity;
float cmd_tor = output->torque;
// 与反馈侧保持一致：reverse=true 时控制指令也做符号翻转
if (motor->param.reverse) {
cmd_pos = -cmd_pos;
cmd_vel = -cmd_vel;
cmd_tor = -cmd_tor;
}
pos_tmp = float_to_uint(cmd_pos, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(cmd_vel, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
tor_tmp = float_to_uint(cmd_tor, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
@ -150,11 +159,17 @@ static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
}
uint8_t data[8] = {0};
float cmd_pos = pos;
float cmd_vel = vel;
if (motor->param.reverse) {
cmd_pos = -cmd_pos;
cmd_vel = -cmd_vel;
}
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
memcpy(&data[0], &cmd_pos, 4); // 位置，低位在前
memcpy(&data[4], &cmd_vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
@ -178,9 +193,13 @@ static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
}
uint8_t data[4] = {0};
float cmd_vel = vel;
if (motor->param.reverse) {
cmd_vel = -cmd_vel;
}
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
memcpy(&data[0], &cmd_vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;

246
User/module/arm_oop.hpp
View File

@ -68,7 +68,7 @@ private:
// 重力补偿
struct {
float torques[JOINT_NUM]; // 各关节重力补偿力矩
float scale; // 补偿比例系数（用于微调）
float scales[JOINT_NUM]; // 每关节补偿系数（用于独立微调）
} gravity_comp_;
// 笛卡尔空间轨迹规划
@ -155,8 +155,8 @@ public:
for (size_t i = 0; i < JOINT_NUM; ++i) {
gravity_comp_.torques[i] = 0.0f;
gravity_comp_.scales[i] = 1.0f;
}
gravity_comp_.scale = 1.0f;
inverseKinematics_.valid = false;
memset(&inverseKinematics_.angles, 0, sizeof(inverseKinematics_.angles));
memset(&traj_, 0, sizeof(traj_));
@ -217,7 +217,6 @@ public:
}
}
// 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取）
Arm6dof_JointAngles_t q;
FillCurrentJointAngles(q);
@ -235,8 +234,7 @@ public:
/**
* @brief 计算当前姿态下各关节的重力补偿力矩
* @param q 当前关节角度（由Update()统一读取后传入，避免重复读取）
* @details 基于牛顿-欧拉逆动力学，设速度和加速度为零，
* 求解的力矩即为平衡重力所需的各关节力矩
* @details 底层返回 raw_torques 后，再按每关节系数 scales[i] 独立缩放
*/
int8_t CalcGravityTorques(const Arm6dof_JointAngles_t& q) {
float raw_torques[JOINT_NUM];
@ -244,85 +242,170 @@ public:
if (ret != 0) return ret;
for (size_t i = 0; i < JOINT_NUM; ++i) {
gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scale;
gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scales[i];
}
return 0;
}
/**
* @brief 2.5D 解析降维逆运动学（专为该六轴机械臂的L型构型设计）
* 特点：无奇异点、O(1)极速求解、绝对稳定。
* 直接映射目标并进行安全边界裁剪，无需牛顿迭代。
* @brief 位置优先的粗解耦逆运动学（J1~J3主控，J4~J6保持）
* @details
* - 目标：实现“上下左右/前后”粗到位，先到目标附近，再手动微调腕部。
* - 求解：仅对前三关节做 3x3 阻尼最小二乘迭代，误差用末端位置误差。
* - 姿态：不强制跟踪输入姿态，J4~J6保持 q_seed（或当前角度）不变。
*/
int8_t InverseKinematicsAnalytical(const Arm6dof_Pose_t* pose, Arm6dof_JointAngles_t* q_out) {
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;
float x = pose->x;
float y = pose->y;
float z = pose->z;
// 以 q_seed 为优先初值，保证轨迹连续；无 seed 时读取当前关节角。
Arm6dof_JointAngles_t seed;
if (q_seed) {
seed = *q_seed;
// 提取的宏观参数
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高度
} else {
FillCurrentJointAngles(seed);
}
// J1: 偏航直接通过 XY 夹角决定
float target_q1 = atan2f(y, x);
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;
};
// 面向R-Z平面的投影
float R = sqrtf(x*x + y*y);
float e_pitch = pose->pitch;
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]}
};
// 逆推动力学关节5 (虚拟手腕端点) 的位置
float R5 = R - L3 * cosf(e_pitch);
float Z5 = z - L3 * sinf(e_pitch);
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;
}
}
// 相对于 J2 原点的距离
float r_target = R5;
float z_target = Z5 - Z_J2;
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];
}
}
}
float D_sq = r_target*r_target + z_target*z_target;
float D = sqrtf(D_sq);
x[0] = aug[0][3];
x[1] = aug[1][3];
x[2] = aug[2][3];
return true;
};
// 死区保护层(极其重要)：限制最大前伸距离，强制拦下无效追踪，绝不当机
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; }
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 内也视作可接受粗到位
// 用余弦定理求 肘部内角 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);
float best_err = 1e9f;
Arm6dof_JointAngles_t best_q = q;
// 用余弦定理求 目标线与大臂的夹角 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);
for (int iter = 0; iter < max_iter; ++iter) {
Arm6dof_Pose_t cur_pose;
if (Arm6dof_ForwardKinematics(&q, &cur_pose) != 0) {
break;
}
// 目标线相对于水平面的仰角 alpha
float alpha = atan2f(z_target, r_target);
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);
// 选择 Elbow Up（肘部向上）姿态：
float target_q2 = alpha + beta;
float q3_eff = -( (float)M_PI - gamma);
float target_q3 = q3_eff - theta_offset;
if (err_norm < best_err) {
best_err = err_norm;
best_q = q;
}
if (err_norm <= pos_tol) {
break;
}
// 手腕解耦补偿
float arm_pitch_eff = target_q2 + q3_eff;
float target_q5 = e_pitch - arm_pitch_eff;
// 仅对 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;
}
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;
// 阻尼最小二乘：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;
}
@ -391,7 +474,7 @@ public:
bool ik_success = false;
if (control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
ik_success = (InverseKinematicsAnalytical(&interp, &traj_.angles) == 0);
ik_success = (InverseKinematicsAnalytical(&interp, &traj_.angles, &q_init) == 0);
} else {
ik_success = (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) == 0);
}
@ -508,6 +591,10 @@ public:
}
}
if(control_.mode == ControlMode::JOINT_POSITION) {
}
// GRAVITY_COMP 模式：保持当前位置 + 重力补偿
if (control_.mode == ControlMode::GRAVITY_COMP) {
SetJointTargetsToCurrent();
@ -606,11 +693,38 @@ public:
}
/**
* @brief 设置重力补偿比例系数（用于微调）
* @param scale 补偿系数，1.0=完全补偿，<1.0=欠补偿，>1.0=过补偿
* @brief 设置全部关节统一补偿系数（兼容旧接口）
*/
void SetGravityCompScale(float scale) {
gravity_comp_.scale = 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;
}
/**

71
User/task/arm_main.cpp
View File

@ -28,7 +28,7 @@ static MOTOR_LZ_Param_t lz_params[3] = {
// DM电机参数（关节4-6）
static MOTOR_DM_Param_t dm_params[3] = {
{.can = BSP_CAN_1, .master_id = 0x14, .can_id = 0x04, .module = MOTOR_DM_J4310, .reverse = false,},
{.can = BSP_CAN_1, .master_id = 0x15, .can_id = 0x05, .module = MOTOR_DM_J4310, .reverse = false,},
{.can = BSP_CAN_1, .master_id = 0x15, .can_id = 0x05, .module = MOTOR_DM_J4310, .reverse = true,},
{.can = BSP_CAN_1, .master_id = 0x16, .can_id = 0x06, .module = MOTOR_DM_J4310, .reverse = false,},
};
@ -75,7 +75,7 @@ Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_targe
// 2 = 全部六轴输出（完整重力补偿，GRAVITY_COMP模式）
// 3 = 笛卡尔空间控制（旧版满秩库代数雅可比算法）
// 4 = 2.5D 解析降维控制（专为比赛设计的无奇异点稳定版，推荐！）
uint8_t test_stage = 0;
uint8_t test_stage = 5;
Arm6dof_JointAngles_t current_angles; // 调试观察：当前各关节角度 (rad)
// 重力补偿力矩观察数组（对应关节1~6，单位 N·m）
float gravity_torques_dbg[6] = {0.0f};
@ -85,13 +85,13 @@ struct { float x, y, z, roll, pitch, yaw; } end_pose_mm_dbg = {0};
Arm6dof_JointAngles_t ik_from_fk_result; // IK解算结果
int setzero=0;
// 重力补偿缩放系数（可在调试器中实时修改，用于测试补偿强度）
float gravity_comp_scale = 1.0f;
// 重力补偿缩放系数（每关节独立，可在调试器中实时修改）
float gravity_comp_scales[6] = {1.0f, 2.0f, 1.5f, 2.0f, 2.6f, 1.0f};
// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
float traj_progress_dbg = 0.0f;
// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
float traj_lin_vel = 0.15f; // 末端线速度 (m/s)，默认 150mm/s
float traj_ang_vel = 1.0f; // 末端角速度 (rad/s)，默认 ~57°/s
float traj_lin_vel = 0.03f; // 末端线速度 (m/s)，默认 150mm/s
float traj_ang_vel = 0.2f; // 末端角速度 (rad/s)，默认 ~57°/s
uint8_t control_frame = 1; // 选择在何种坐标系下控制 1= 世界系，2=工具系，3=航向系
// ============================================================================
@ -103,6 +103,9 @@ uint8_t control_frame = 1; // 选择在何种坐标系下控制 1= 世界系，
int ik_test_enable = 0;
Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察
int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化
int ik_test_ret_analytical = 0; // 解析IK原始返回码
int ik_test_ret_numerical = 0; // 数值IK原始返回码（仅回退时更新）
int ik_test_solver_dbg = 0; // 0=解析成功, 1=数值回退成功, -1=全部失败
// cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿
extern Arm_CMD_t *cmd_for_arm;
@ -260,7 +263,7 @@ void Task_arm_main(void* argument) {
// 使能重力补偿
robot_arm->EnableGravityCompensation(true);
robot_arm->SetGravityCompScale(gravity_comp_scale); // 补偿系数，可微调
robot_arm->SetGravityCompScales(gravity_comp_scales);
// 设置控制模式（可选以下模式之一）:
// GRAVITY_COMP: 位置保持 + 重力补偿前馈（位控+补偿）
@ -281,6 +284,9 @@ void Task_arm_main(void* argument) {
HandleSetZeroRequest();
/*--------------------------------------------------------------*/
// 每帧允许在调试器中独立修改各轴补偿系数
robot_arm->SetGravityCompScales(gravity_comp_scales);
// 更新机械臂状态（读取关节角度、FK、重力补偿计算）
robot_arm->Update();
@ -332,19 +338,19 @@ void Task_arm_main(void* argument) {
case 2:
// 阶段1：测试单个关节力矩输出（用于验证力矩计算）
// 修改下面的索引来测试不同关节：j2=1, j3=2, j5=4
motors[0]->Relax();
motors[1]->Relax();
motors[2]->Relax();
motors[3]->Relax();
motors[4]->Relax();
motors[5]->Relax();
// joints[1]->TorqueControl(gravity_torques_dbg[1]); // j2大臂
// joints[2]->TorqueControl(gravity_torques_dbg[2]); // j3小臂
// joints[4]->TorqueControl(gravity_torques_dbg[4]); // j5腕部
// motors[0]->Relax();
// motors[1]->Relax();
// motors[2]->Relax();
// motors[3]->Relax();
// motors[4]->Relax();
// motors[5]->Relax();
joints[1]->TorqueControl(gravity_torques_dbg[1]); // j2大臂
joints[2]->TorqueControl(gravity_torques_dbg[2]); // j3小臂
joints[4]->TorqueControl(gravity_torques_dbg[4]); // j5腕部
// joints[0]->TorqueControl(gravity_torques_dbg[0]);
// joints[3]->TorqueControl(gravity_torques_dbg[3]);
// joints[5]->TorqueControl(gravity_torques_dbg[5]);
joints[0]->TorqueControl(gravity_torques_dbg[0]);
joints[3]->TorqueControl(gravity_torques_dbg[3]);
joints[5]->TorqueControl(gravity_torques_dbg[5]);
break;
case 3:
default:
@ -371,9 +377,30 @@ void Task_arm_main(void* argument) {
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
// 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
if (ik_test_enable) {
ik_test_ret = Arm6dof_InverseKinematics(&arm_cmd.target_pose, &current_angles,
&ik_test_result, 0.001f, 50);
ik_test_ret++;
ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose,
&ik_test_result,
&current_angles);
ik_test_ret_numerical = 0;
ik_test_solver_dbg = -1;
// 解析解失败时回退到数值IK，便于调试观察并区分“无解”与“分支失败”。
if (ik_test_ret_analytical == 0) {
ik_test_ret = 0;
ik_test_solver_dbg = 0;
} else {
ik_test_ret_numerical = Arm6dof_InverseKinematics(&arm_cmd.target_pose,
&current_angles,
&ik_test_result,
0.001f,
50);
if (ik_test_ret_numerical == 0) {
ik_test_ret = 0;
ik_test_solver_dbg = 1;
} else {
ik_test_ret = -1;
ik_test_solver_dbg = -1;
}
}
}
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

4
User/task/blink.c
View File

@ -42,8 +42,8 @@ void Task_blink(void *argument) {
BSP_PWM_SetComp(BSP_PWM_TIM5_CH3, 0.0f);
BSP_PWM_Start(BSP_PWM_TIM5_CH3);
// BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
BUZZER_PlayMusic(&buzzer, MUSIC_RM);
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
BUZZER_PlayMusic(&buzzer, MUSIC_NOKIA);
/* USER CODE INIT END */

2
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf
View File

@ -384,7 +384,7 @@
xyz="0 0 1" />
<limit
lower="0"
upper="3.66"
upper="6.3"
effort="20"
velocity="3.14" />
</joint>

29
ozone/arm.jdebug.user
View File

@ -1,19 +1,21 @@
GraphedExpression="ik_test_ret", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f, Show=0
GraphedExpression="((robot_arm)->traj_).t", Color=#35792b, Show=0
GraphedExpression="(((robot_arm)->end_effector_).target).z", Color=#769dda, Show=0
GraphedExpression="((arm_cmd).target_pose).z", Color=#b14f0d
GraphedExpression="ik_test_ret_analytical", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b
GraphedExpression="ik_test_ret_numerical", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#769dda, Show=0
GraphedExpression="ik_test_solver_dbg", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#b14f0d, Show=0
OpenDocument="rc.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/rc.c", Line=20
OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=202
OpenDocument="main.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/main.c", Line=45
OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=265
OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=65
OpenDocument="stm32f4xx_it.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/stm32f4xx_it.c", Line=77
OpenDocument="init.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/init.c", Line=6
OpenDocument="ctrl_chassis.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/ctrl_chassis.c", Line=6
OpenDocument="cmd.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/cmd.cpp", Line=22
OpenDocument="cmd.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/cmd/cmd.c", Line=278
OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32Projects_HAL/board-F4/arm/startup_stm32f407xx.s", Line=48
OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=256
OpenDocument="motor_base.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/motor_base.hpp", Line=121
OpenDocument="blink.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/blink.c", Line=33
OpenDocument="joint.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/joint.hpp", Line=29
OpenDocument="robotics.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.cpp", Line=251
OpenDocument="arm6dof.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.h", Line=0
@ -21,7 +23,7 @@ OpenDocument="can.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/can.c
OpenDocument="arm6dof.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.cpp", Line=175
OpenDocument="matrix.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/matrix.h", Line=24
OpenDocument="robotics.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.h", Line=309
OpenDocument="rc.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/rc.c", Line=20
OpenDocument="dr16.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/device/dr16.c", Line=0
OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=2, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
@ -30,7 +32,7 @@ OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="201;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1148;-71", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;268", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1122;-71", CodeGraphLegendShown=0, CodeGraphLegendPosition="1138;0"
OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;639]
@ -39,8 +41,8 @@ TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret";" ((robot_arm)->traj_).t";" (((robot_arm)->end_effector_).target).z";" ((arm_cmd).target_pose).z"], ColWidths=[100;100;100;100;100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;144;124;154;110;144;126]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret";" ik_test_ret_analytical";" ik_test_ret_numerical";" ik_test_solver_dbg"], ColWidths=[100;100;100;100;100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;134;110;126;126]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1
@ -55,3 +57,12 @@ WatchedExpression="ik_test_enable", RefreshRate=1, Window=Watched Data 1
WatchedExpression="ik_test_result", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_test_ret", RefreshRate=5, Window=Watched Data 1
WatchedExpression="arm_cmd", RefreshRate=5, Window=Watched Data 1
WatchedExpression="rotationDirectionCheck", RefreshRate=5, Window=Watched Data 1
WatchedExpression="joints", RefreshRate=5, Window=Watched Data 1
WatchedExpression="motors", RefreshRate=2, Window=Watched Data 1
WatchedExpression="dr16", RefreshRate=5, Window=Watched Data 1
WatchedExpression="test_stage", RefreshRate=5, Window=Watched Data 1
WatchedExpression="gravity_comp_scales", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_test_ret_analytical", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_test_ret_numerical", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_test_solver_dbg", RefreshRate=5, Window=Watched Data 1