From bf2633c3deeace12f03ad596e1ce2c8bfd898c2c Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Fri, 20 Mar 2026 00:20:40 +0800 Subject: [PATCH] 123 --- User/component/arm_kinematics/arm6dof.cpp | 308 ++++++++++++++---- User/device/motor_dm.c | 33 +- User/module/arm_oop.hpp | 248 ++++++++++---- User/module/cmd/cmd.c | 2 +- User/task/arm_main.cpp | 73 +++-- User/task/blink.c | 4 +- .../缩小版2026315/xin/urdf/xin.urdf | 2 +- ozone/arm.jdebug.user | 31 +- 8 files changed, 526 insertions(+), 175 deletions(-) diff --git a/User/component/arm_kinematics/arm6dof.cpp b/User/component/arm_kinematics/arm6dof.cpp index 0d08467..5a82724 100644 --- a/User/component/arm_kinematics/arm6dof.cpp +++ b/User/component/arm_kinematics/arm6dof.cpp @@ -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 初始化机械臂运动学模型 @@ -91,15 +203,9 @@ int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* po if (!s_initialized || q == nullptr || pose == nullptr) { 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); @@ -181,17 +281,85 @@ int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAng q_guess[i][0] = q_init->q[i]; } } - - // 调用toolbox的逆运动学求解器（牛顿法） - Matrixf<6, 1> q_solved = s_robot->ikine(T_target, q_guess, tol, max_iter); - - // 将结果复制到输出 - for (int i = 0; i < 6; i++) { - q_result->q[i] = q_solved[i][0]; + + // 基于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 (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]; + } + + 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; + } + } } - - // 验证解的精度：位置误差 + 姿态误差双y重校验 - Matrixf<4, 4> T_verify = s_robot->fkine(q_solved); + + 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; diff --git a/User/device/motor_dm.c b/User/device/motor_dm.c index 0ab7520..be57d31 100644 --- a/User/device/motor_dm.c +++ b/User/device/motor_dm.c @@ -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; diff --git a/User/module/arm_oop.hpp b/User/module/arm_oop.hpp index 5a74643..06b4262 100644 --- a/User/module/arm_oop.hpp +++ b/User/module/arm_oop.hpp @@ -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; - // 提取的宏观参数 - 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高度 + // 以 q_seed 为优先初值，保证轨迹连续；无 seed 时读取当前关节角。 + Arm6dof_JointAngles_t seed; + if (q_seed) { + seed = *q_seed; + + } 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; } /** diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index a2f875e..0e5b980 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -5,7 +5,7 @@ #include "bsp/time.h" #include #include - + /* ========================================================================== */ /* 命令构建函数 */ /* ========================================================================== */ diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp index 3825a1c..76b471d 100644 --- a/User/task/arm_main.cpp +++ b/User/task/arm_main.cpp @@ -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}; @@ -83,15 +83,15 @@ float gravity_torques_dbg[6] = {0.0f}; struct { float x, y, z, roll, pitch, yaw; } end_pose_mm_dbg = {0}; // FK→IK验证：IK解算得到的关节角度 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, ¤t_angles, - &ik_test_result, 0.001f, 50); - ik_test_ret++; + ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose, + &ik_test_result, + ¤t_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, + ¤t_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); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/blink.c b/User/task/blink.c index 3fb08c7..92a9085 100644 --- a/User/task/blink.c +++ b/User/task/blink.c @@ -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 */ diff --git a/arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf b/arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf index d38f200..5cdd43e 100644 --- a/arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf +++ b/arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf @@ -384,7 +384,7 @@ xyz="0 0 1" /> diff --git a/ozone/arm.jdebug.user b/ozone/arm.jdebug.user index 49635e0..4efbc51 100644 --- a/ozone/arm.jdebug.user +++ b/ozone/arm.jdebug.user @@ -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 @@ -54,4 +56,13 @@ WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1 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 \ No newline at end of file +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 \ No newline at end of file