保存
This commit is contained in:
parent
7c078bdb54
commit
e0e6b33791
@ -211,12 +211,11 @@ class Serial_Link {
|
|||||||
return J_;
|
return J_;
|
||||||
}
|
}
|
||||||
|
|
||||||
// inverse kinematic, numerical solution(Newton method)
|
// inverse kinematic, Levenberg-Marquardt method
|
||||||
// param[in] T: homogeneous transformation matrix of end effector
|
// param[in] Td: target homogeneous transformation matrix of end effector
|
||||||
// param[in] q: initial joint variable vector(q0) for Newton method's
|
// param[in] q: initial joint variable vector(q0) for iteration
|
||||||
// iteration
|
// param[in] tol: tolerance of error (norm of twist error vector)
|
||||||
// param[in] tol: tolerance of error (norm(error of twist vector))
|
// param[in] max_iter: maximum iterations, default 50
|
||||||
// param[in] max_iter: maximum iterations, default 30
|
|
||||||
// param[out] q: joint variable vector
|
// param[out] q: joint variable vector
|
||||||
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
|
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
|
||||||
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
|
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
|
||||||
@ -225,56 +224,64 @@ class Serial_Link {
|
|||||||
Matrixf<3, 1> pe, we;
|
Matrixf<3, 1> pe, we;
|
||||||
Matrixf<6, 1> err, new_err;
|
Matrixf<6, 1> err, new_err;
|
||||||
Matrixf<_n, 1> dq;
|
Matrixf<_n, 1> dq;
|
||||||
float step = 1;
|
float lambda = 1.0f; // LM 阻尼因子
|
||||||
for (int i = 0; i < max_iter; i++) {
|
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 iter = 0; iter < max_iter; iter++) {
|
||||||
T = fkine(q);
|
T = fkine(q);
|
||||||
pe = t2p(Td) - t2p(T);
|
pe = t2p(Td) - t2p(T);
|
||||||
// angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
|
|
||||||
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int k = 0; k < 3; k++) {
|
||||||
err[i][0] = pe[i][0];
|
err[k][0] = pe[k][0];
|
||||||
err[i + 3][0] = we[i][0];
|
err[k + 3][0] = we[k][0];
|
||||||
}
|
}
|
||||||
if (err.norm() < tol)
|
float err_norm = err.norm();
|
||||||
|
if (err_norm < tol)
|
||||||
return q;
|
return q;
|
||||||
// adjust iteration step
|
|
||||||
Matrixf<6, _n> J = jacob(q);
|
Matrixf<6, _n> J = jacob(q);
|
||||||
for (int j = 0; j < 5; j++) {
|
Matrixf<_n, _n> JtJ = J.trans() * J;
|
||||||
dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step;
|
Matrixf<_n, 1> Jte = J.trans() * err;
|
||||||
if (dq[0][0] == INFINITY) // J'*J singular
|
|
||||||
{
|
// LM 步：dq = (J'J + λI)^{-1} J'e
|
||||||
dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) *
|
dq = matrixf::inv(JtJ + lambda * matrixf::eye<_n, _n>()) * Jte;
|
||||||
J.trans() * err * step;
|
|
||||||
// SVD<6, _n> JTJ_svd(J.trans() * J);
|
// 检查求逆是否成功
|
||||||
// dq = JTJ_svd.solve(err) * step * 5e-2f;
|
if (dq[0][0] == INFINITY || dq[0][0] != dq[0][0]) {
|
||||||
q += dq;
|
lambda *= lambda_up;
|
||||||
for (int i = 0; i < _n; i++) {
|
if (lambda > lambda_max) return q;
|
||||||
if (links_[i].type() == R)
|
continue;
|
||||||
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
|
}
|
||||||
}
|
|
||||||
break;
|
// 评估新误差
|
||||||
}
|
Matrixf<_n, 1> q_new = q + dq;
|
||||||
T = fkine(q + dq);
|
// 关节角折叠到有效区间
|
||||||
pe = t2p(Td) - t2p(T);
|
for (int i = 0; i < _n; i++) {
|
||||||
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
if (links_[i].type() == R)
|
||||||
for (int i = 0; i < 3; i++) {
|
q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_);
|
||||||
new_err[i][0] = pe[i][0];
|
}
|
||||||
new_err[i + 3][0] = we[i][0];
|
T = fkine(q_new);
|
||||||
}
|
pe = t2p(Td) - t2p(T);
|
||||||
if (new_err.norm() < err.norm()) {
|
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
||||||
q += dq;
|
for (int k = 0; k < 3; k++) {
|
||||||
for (int i = 0; i < _n; i++) {
|
new_err[k][0] = pe[k][0];
|
||||||
if (links_[i].type() == robotics::Joint_Type_e::R) {
|
new_err[k + 3][0] = we[k][0];
|
||||||
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
|
}
|
||||||
}
|
float new_err_norm = new_err.norm();
|
||||||
}
|
|
||||||
break;
|
if (new_err_norm < err_norm) {
|
||||||
} else {
|
// 误差减小：接受步长，减小阻尼（趋近 Gauss-Newton，加速收敛）
|
||||||
step /= 2.0f;
|
q = q_new;
|
||||||
}
|
lambda *= lambda_down;
|
||||||
|
if (lambda < lambda_min) lambda = lambda_min;
|
||||||
|
} else {
|
||||||
|
// 误差增大：拒绝步长，增大阻尼（趋近梯度下降，缩小步幅）
|
||||||
|
lambda *= lambda_up;
|
||||||
|
if (lambda > lambda_max) return q; // 阻尼过大，无法继续
|
||||||
}
|
}
|
||||||
if (step < 1e-3f)
|
|
||||||
return q;
|
|
||||||
}
|
}
|
||||||
return q;
|
return q;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -196,94 +196,94 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 轨迹规划核心：每帧插值一小步并对插值点求IK
|
* @brief 轨迹规划核心：基于固定的 start→goal 路径，由 traj_.t 驱动进度
|
||||||
|
*
|
||||||
|
* 设计原则：轨迹由 traj_.start / traj_.goal 固定，traj_.t 按速度限制逐帧递增。
|
||||||
|
* - IK目标点由 traj_.t 插值得到，与当前末端位姿无关，手动扰动不影响轨迹目标
|
||||||
|
* - IK初始猜测优先使用上一帧轨迹解（连续性好），首帧用当前关节角
|
||||||
*
|
*
|
||||||
* 每帧执行流程：
|
* 每帧执行流程：
|
||||||
* 1. 按速度限制计算本帧 t 步进量 Δt = v_max * dt / 总距离
|
* 1. 计算 start→goal 的总位移/角位移向量（固定）
|
||||||
* 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标
|
* 2. dt = min(v_max*dt/total, 1.0) 按速度限制计算本帧进度增量
|
||||||
* 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛
|
* 3. traj_.t += dt，clamp 到 [0, 1]
|
||||||
* 4. 验证 + 写入 inverseKinematics_.angles
|
* 4. interp = start + t * (goal - start)
|
||||||
|
* 5. IK(interp, q_init=上一帧解 或 当前关节角) → 写入 traj_.angles
|
||||||
*/
|
*/
|
||||||
void StepTrajectory() {
|
void StepTrajectory() {
|
||||||
if (!traj_.active) return;
|
if (!traj_.active) return;
|
||||||
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
||||||
|
|
||||||
// 计算总距离（用于将速度限制转换为 t 步进量）
|
// 计算 start→goal 的固定总位移向量（不随手动扰动变化）
|
||||||
float dx = traj_.goal.x - traj_.start.x;
|
float tdx = traj_.goal.x - traj_.start.x;
|
||||||
float dy = traj_.goal.y - traj_.start.y;
|
float tdy = traj_.goal.y - traj_.start.y;
|
||||||
float dz = traj_.goal.z - traj_.start.z;
|
float tdz = traj_.goal.z - traj_.start.z;
|
||||||
float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz);
|
float total_dist = sqrtf(tdx*tdx + tdy*tdy + tdz*tdz);
|
||||||
|
|
||||||
float dr = traj_.goal.roll - traj_.start.roll;
|
float tdr = traj_.goal.roll - traj_.start.roll;
|
||||||
float dp = traj_.goal.pitch - traj_.start.pitch;
|
float tdp = traj_.goal.pitch - traj_.start.pitch;
|
||||||
float dyw = traj_.goal.yaw - traj_.start.yaw;
|
float tdyw = traj_.goal.yaw - traj_.start.yaw;
|
||||||
// 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯
|
// 姿态差 wrap 到 (-π, π]：走最短路径
|
||||||
dr -= roundf(dr / TWO_PI) * TWO_PI;
|
tdr -= roundf(tdr / TWO_PI) * TWO_PI;
|
||||||
dp -= roundf(dp / TWO_PI) * TWO_PI;
|
tdp -= roundf(tdp / TWO_PI) * TWO_PI;
|
||||||
dyw -= roundf(dyw / TWO_PI) * TWO_PI;
|
tdyw -= roundf(tdyw / TWO_PI) * TWO_PI;
|
||||||
float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw);
|
float total_ang = sqrtf(tdr*tdr + tdp*tdp + tdyw*tdyw);
|
||||||
|
|
||||||
// Δt = v_max * dt / 总距离
|
// 按速度限制计算本帧进度增量，位置/姿态约束取严格者
|
||||||
// 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进
|
float dt_p = (total_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / total_dist) : 1.0f;
|
||||||
// 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完
|
float dt_a = (total_ang > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / total_ang) : 1.0f;
|
||||||
float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f;
|
float dt = dt_p < dt_a ? dt_p : dt_a;
|
||||||
float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f;
|
if (dt > 1.0f) dt = 1.0f;
|
||||||
float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束）
|
|
||||||
|
|
||||||
traj_.t += dt_step;
|
traj_.t += dt;
|
||||||
if (traj_.t >= 1.0f) {
|
if (traj_.t >= 1.0f) {
|
||||||
traj_.t = 1.0f;
|
traj_.t = 1.0f;
|
||||||
traj_.active = false; // 到达终点
|
traj_.active = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 线性插值：P(t) = start + t*(goal-start)
|
// 线性插值：start + t * (goal - start)，轨迹固定，不受手动扰动影响
|
||||||
Arm6dof_Pose_t interp;
|
Arm6dof_Pose_t interp;
|
||||||
interp.x = traj_.start.x + traj_.t * dx;
|
interp.x = traj_.start.x + traj_.t * tdx;
|
||||||
interp.y = traj_.start.y + traj_.t * dy;
|
interp.y = traj_.start.y + traj_.t * tdy;
|
||||||
interp.z = traj_.start.z + traj_.t * dz;
|
interp.z = traj_.start.z + traj_.t * tdz;
|
||||||
interp.roll = traj_.start.roll + traj_.t * dr;
|
interp.roll = traj_.start.roll + traj_.t * tdr;
|
||||||
interp.pitch = traj_.start.pitch + traj_.t * dp;
|
interp.pitch = traj_.start.pitch + traj_.t * tdp;
|
||||||
interp.yaw = traj_.start.yaw + traj_.t * dyw;
|
interp.yaw = traj_.start.yaw + traj_.t * tdyw;
|
||||||
|
|
||||||
// IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支）
|
// IK初始猜测：优先用上一帧轨迹IK解（连续性好，不受手动扰动影响）
|
||||||
|
// traj_.valid=false 表示首帧或上帧IK失败，改用当前关节角
|
||||||
Arm6dof_JointAngles_t q_init;
|
Arm6dof_JointAngles_t q_init;
|
||||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
if (traj_.valid) {
|
||||||
q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
q_init = traj_.angles;
|
||||||
|
} else {
|
||||||
|
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.01f, 50) != 0) {
|
if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
|
||||||
traj_.valid = false;
|
traj_.valid = false;
|
||||||
traj_.active = false;
|
traj_.active = false;
|
||||||
control_.state = MotionState::ERROR;
|
control_.state = MotionState::ERROR;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 角度连续性修正：将IK解调整到与当前关节角最近的等效角
|
// 角度连续性修正：仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）
|
||||||
// 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义：
|
// IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支
|
||||||
// IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。
|
|
||||||
// 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。
|
|
||||||
// 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）：
|
|
||||||
// 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。
|
|
||||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||||
if (!joints_[i]) continue;
|
if (!joints_[i]) continue;
|
||||||
const auto& dh = joints_[i]->GetDHParams();
|
const auto& dh = joints_[i]->GetDHParams();
|
||||||
// 关节范围跨度不足 2π——无等效分支，无需修正
|
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; // 范围 ≤ 2π，无等效分支
|
||||||
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue;
|
float diff = traj_.angles.q[i] - q_init.q[i];
|
||||||
|
|
||||||
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;
|
diff -= roundf(diff / TWO_PI) * TWO_PI;
|
||||||
traj_.angles.q[i] = q_cur + diff;
|
traj_.angles.q[i] = q_init.q[i] + diff;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 限位检查
|
// 限位检查
|
||||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||||
if (joints_[i]) {
|
if (joints_[i]) {
|
||||||
const auto& dh = joints_[i]->GetDHParams();
|
const auto& dh = joints_[i]->GetDHParams();
|
||||||
if (traj_.angles.q[i] < (dh.qmin-qLimitTolerance) ||
|
if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) ||
|
||||||
traj_.angles.q[i] > (dh.qmax+qLimitTolerance)) {
|
traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) {
|
||||||
traj_.valid = false;
|
traj_.valid = false;
|
||||||
traj_.active = false;
|
traj_.active = false;
|
||||||
control_.state = MotionState::ERROR;
|
control_.state = MotionState::ERROR;
|
||||||
@ -292,19 +292,15 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常
|
// 突变检查：以上一帧轨迹解（q_init）为参考，检查轨迹自身连续性
|
||||||
// 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
|
|
||||||
static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
|
static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
|
||||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||||
if (joints_[i]) {
|
float delta = fabsf(traj_.angles.q[i] - q_init.q[i]);
|
||||||
float delta = fabsf(traj_.angles.q[i] -
|
if (delta > TRAJ_MAX_JOINT_DELTA) {
|
||||||
joints_[i]->GetCurrentAngle());
|
traj_.valid = false;
|
||||||
if (delta > TRAJ_MAX_JOINT_DELTA) {
|
traj_.active = false;
|
||||||
traj_.valid = false;
|
control_.state = MotionState::ERROR;
|
||||||
traj_.active = false;
|
return;
|
||||||
control_.state = MotionState::ERROR;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -546,14 +542,13 @@ public:
|
|||||||
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
|
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
|
||||||
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
|
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
|
||||||
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
|
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
|
||||||
|
|
||||||
if (goal_changed) {
|
if (goal_changed) {
|
||||||
// 从当前末端位姿出发重新规划轨迹
|
// 从当前末端位姿出发重新规划轨迹
|
||||||
traj_.start = end_effector_.current;
|
traj_.start = end_effector_.current;
|
||||||
traj_.goal = goal;
|
traj_.goal = goal;
|
||||||
traj_.t = 0.0f;
|
traj_.t = 0.0f;
|
||||||
traj_.active = true;
|
traj_.active = true;
|
||||||
traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory
|
traj_.valid = false; // 首帧由 StepTrajectory 用当前关节角作IK初值
|
||||||
control_.state = MotionState::MOVING;
|
control_.state = MotionState::MOVING;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
@ -262,7 +262,11 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
|
|||||||
* DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿）
|
* DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿）
|
||||||
*/
|
*/
|
||||||
ctx->output.arm.cmd.set_target_as_current = false;
|
ctx->output.arm.cmd.set_target_as_current = false;
|
||||||
switch (ctx->input.rc.sw[0]) {
|
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
|
||||||
|
ctx->output.arm.cmd.set_target_as_current = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (ctx->input.rc.sw[0]) {
|
||||||
case CMD_SW_MID:
|
case CMD_SW_MID:
|
||||||
ctx->output.arm.cmd.enable = true;
|
ctx->output.arm.cmd.enable = true;
|
||||||
break;
|
break;
|
||||||
@ -274,9 +278,6 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
|
|||||||
goto end;
|
goto end;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
|
|
||||||
ctx->output.arm.cmd.set_target_as_current = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 遥控器模式使用笛卡尔位姿控制 */
|
/* 遥控器模式使用笛卡尔位姿控制 */
|
||||||
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
|
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
|
||||||
|
|||||||
@ -65,8 +65,7 @@ MotorLZ* motors_lz[3] = {nullptr}; // 用于调试查看
|
|||||||
MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看
|
MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看
|
||||||
Joint* joints[6] = {nullptr};
|
Joint* joints[6] = {nullptr};
|
||||||
RoboticArm* robot_arm = nullptr;
|
RoboticArm* robot_arm = nullptr;
|
||||||
Arm_CMD_t arm_cmd; // 当前机械臂控制命令
|
Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_target_as_current）
|
||||||
Arm6dof_Pose_t target_pose = {0}; // 控制输入：目标末端位姿，单位 m/rad（内部计算全链路SI单位）
|
|
||||||
|
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
// 测试用调试变量（可在调试器Watch窗口直接观察和修改）
|
// 测试用调试变量（可在调试器Watch窗口直接观察和修改）
|
||||||
@ -91,15 +90,10 @@ float gravity_comp_scale = 1.0f;
|
|||||||
// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
|
// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
|
||||||
float traj_progress_dbg = 0.0f;
|
float traj_progress_dbg = 0.0f;
|
||||||
// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
|
// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
|
||||||
float traj_lin_vel = 0.05f; // 末端线速度 (m/s)，默认 150mm/s
|
float traj_lin_vel = 0.15f; // 末端线速度 (m/s)，默认 150mm/s
|
||||||
float traj_ang_vel = 0.3f; // 末端角速度 (rad/s)，默认 ~57°/s
|
float traj_ang_vel = 1.0f; // 末端角速度 (rad/s)，默认 ~57°/s
|
||||||
// 同步控制：设置为1时将target_pose同步为当前位姿（世界系，供MoveCartesian使用），同步后自动清零
|
|
||||||
int sync_target_to_current = 0;
|
|
||||||
// 航向系同步：设置为1时将target_pose同步为当前位姿的航向系表示（供MoveCartesianHeading使用），同步后自动清零
|
|
||||||
// 位置：p_h = Rz(-yaw)*p_w
|
|
||||||
// 姿态：R_h = Rz(-yaw)*R_cur 提取RPY，同步后直接修改 yaw/pitch/roll 即可直观控制末端各轴旋转
|
|
||||||
int sync_heading_to_current = 0;
|
|
||||||
|
|
||||||
|
uint8_t control_frame = 1; // 选择在何种坐标系下控制 1= 世界系，2=工具系，3=航向系
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
// IK测试调试变量
|
// IK测试调试变量
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
@ -109,10 +103,9 @@ int sync_heading_to_current = 0;
|
|||||||
int ik_test_enable = 0;
|
int ik_test_enable = 0;
|
||||||
Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察
|
Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察
|
||||||
int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化
|
int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化
|
||||||
// 工具系同步：设置为1时将target_pose同步为当前位姿的工具系表示（供MoveCartesianTool使用），同步后自动清零
|
|
||||||
// 位置：p_tool = R_cur^T * p_world
|
// cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿
|
||||||
// 姿态：roll=pitch=yaw=0 即 R_target=I，机械臂保持当前末端姿态不变
|
extern Arm_CMD_t *cmd_for_arm;
|
||||||
int sync_tool_to_current = 0;
|
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
||||||
@ -167,12 +160,14 @@ void Task_arm_main(void* argument) {
|
|||||||
// JOINT_POSITION: 关节位置控制 + 重力补偿前馈
|
// JOINT_POSITION: 关节位置控制 + 重力补偿前馈
|
||||||
robot_arm->SetMode(ControlMode::GRAVITY_COMP);
|
robot_arm->SetMode(ControlMode::GRAVITY_COMP);
|
||||||
|
|
||||||
|
// 读取当前末端位姿，同步到 arm_cmd 和 cmd 侧累积 target_pose，防止上电时大范围跳变
|
||||||
// 将 target_pose 初始化为当前末端位姿，防止 stage4 首次进入时进行大范围跳变
|
|
||||||
osDelay(100);
|
osDelay(100);
|
||||||
robot_arm->Update(); // 先跨一帧读取当前状态
|
robot_arm->Update();
|
||||||
target_pose = robot_arm->GetEndPose();
|
arm_cmd.target_pose = robot_arm->GetEndPose();
|
||||||
|
if (cmd_for_arm) {
|
||||||
|
cmd_for_arm->target_pose = arm_cmd.target_pose;
|
||||||
|
}
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
/*---------------------------零点校准---------------------------*/
|
/*---------------------------零点校准---------------------------*/
|
||||||
@ -198,55 +193,64 @@ void Task_arm_main(void* argument) {
|
|||||||
|
|
||||||
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
|
||||||
|
|
||||||
if (!arm_cmd.enable) {
|
robot_arm->Enable(arm_cmd.enable);
|
||||||
robot_arm->Enable(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
if (sync_target_to_current) {
|
// set_target_as_current：将目标位姿同步为当前实际末端位姿
|
||||||
target_pose = robot_arm->GetEndPose();
|
// 同时写回 cmd_for_arm->target_pose，使 cmd 侧增量累积从正确基准继续
|
||||||
sync_target_to_current = 0; // 自动清零
|
if (arm_cmd.set_target_as_current) {
|
||||||
|
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
|
||||||
|
Arm6dof_Pose_t pose;
|
||||||
|
switch (control_frame) {
|
||||||
|
case 1: { // 世界系
|
||||||
|
pose = ep;
|
||||||
|
arm_cmd.target_pose = pose;
|
||||||
|
if (cmd_for_arm) {
|
||||||
|
cmd_for_arm->target_pose = pose;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 2: { // 工具系（航向系：Rz(-yaw)旋转水平分量）
|
||||||
|
float cy = cosf(ep.yaw), sy = sinf(ep.yaw);
|
||||||
|
pose.x = cy * ep.x + sy * ep.y;
|
||||||
|
pose.y = -sy * ep.x + cy * ep.y;
|
||||||
|
pose.z = ep.z;
|
||||||
|
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
||||||
|
float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw)
|
||||||
|
Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr))
|
||||||
|
* robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||||
|
Matrixf<3,1> rpy_h = robotics::r2rpy(R_h);
|
||||||
|
pose.yaw = rpy_h[0][0];
|
||||||
|
pose.pitch = rpy_h[1][0];
|
||||||
|
pose.roll = rpy_h[2][0];
|
||||||
|
arm_cmd.target_pose = pose;
|
||||||
|
if (cmd_for_arm) {
|
||||||
|
cmd_for_arm->target_pose = pose;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 3: { // 工具系（R_cur^T * p_world，姿态置零）
|
||||||
|
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
||||||
|
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||||
|
float p_arr[3] = {ep.x, ep.y, ep.z};
|
||||||
|
Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr);
|
||||||
|
pose.x = p_tool[0][0];
|
||||||
|
pose.y = p_tool[1][0];
|
||||||
|
pose.z = p_tool[2][0];
|
||||||
|
pose.yaw = 0.0f;
|
||||||
|
pose.pitch = 0.0f;
|
||||||
|
pose.roll = 0.0f;
|
||||||
|
arm_cmd.target_pose = pose;
|
||||||
|
if (cmd_for_arm) {
|
||||||
|
cmd_for_arm->target_pose = pose;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
// 航向系同步：将target_pose设为当前位姿的航向系表示，供MoveCartesianHeading使用
|
}
|
||||||
// 位置：Rz(-yaw) * p_world → 航向系坐标
|
|
||||||
// 姿态：R_heading = Rz(-yaw) * R_cur，提取RPY后写入target_pose
|
|
||||||
// 同步后 yaw/pitch/roll 即为末端在航向系下的当前姿态，修改哪个轴就转哪个轴
|
|
||||||
if (sync_heading_to_current) {
|
|
||||||
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
|
|
||||||
float cy = cosf(ep.yaw), sy = sinf(ep.yaw);
|
|
||||||
// 位置逆变换
|
|
||||||
target_pose.x = cy * ep.x + sy * ep.y;
|
|
||||||
target_pose.y = -sy * ep.x + cy * ep.y;
|
|
||||||
target_pose.z = ep.z;
|
|
||||||
// 姿态逆变换：R_h = Rz(-yaw) * R_cur，再提取RPY
|
|
||||||
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
|
||||||
float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw)
|
|
||||||
Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr))
|
|
||||||
* robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
|
||||||
Matrixf<3,1> rpy_h = robotics::r2rpy(R_h);
|
|
||||||
target_pose.yaw = rpy_h[0][0];
|
|
||||||
target_pose.pitch = rpy_h[1][0];
|
|
||||||
target_pose.roll = rpy_h[2][0];
|
|
||||||
sync_heading_to_current = 0; // 自动清零
|
|
||||||
}
|
|
||||||
// 工具系同步：将target_pose设为当前位姿的工具系表示，供MoveCartesianTool使用
|
|
||||||
// 位置：p_tool = R_cur^T * p_world
|
|
||||||
// 姿态：roll=pitch=yaw=0 即 R_target=I → R_world=R_cur，保持当前末端姿态不变
|
|
||||||
if (sync_tool_to_current) {
|
|
||||||
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
|
|
||||||
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
|
||||||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
|
||||||
float p_arr[3] = {ep.x, ep.y, ep.z};
|
|
||||||
Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr); // R^T * p_world
|
|
||||||
target_pose.x = p_tool[0][0];
|
|
||||||
target_pose.y = p_tool[1][0];
|
|
||||||
target_pose.z = p_tool[2][0];
|
|
||||||
target_pose.yaw = 0.0f; // R_target = I，保持姿态
|
|
||||||
target_pose.pitch = 0.0f;
|
|
||||||
target_pose.roll = 0.0f;
|
|
||||||
sync_tool_to_current = 0; // 自动清零
|
|
||||||
}
|
|
||||||
|
|
||||||
// 每帧同步调试观察变量
|
// 每帧同步调试观察变量
|
||||||
for (int i = 0; i < 6; ++i) {
|
for (int i = 0; i < 6; ++i) {
|
||||||
current_angles.q[i] = joints[i]->GetCurrentAngle();
|
current_angles.q[i] = joints[i]->GetCurrentAngle();
|
||||||
@ -299,7 +303,7 @@ void Task_arm_main(void* argument) {
|
|||||||
robot_arm->SetMode(ControlMode::CARTESIAN_POSITION);
|
robot_arm->SetMode(ControlMode::CARTESIAN_POSITION);
|
||||||
// robot_arm->MoveCartesianTool(target_pose);
|
// robot_arm->MoveCartesianTool(target_pose);
|
||||||
// robot_arm->MoveCartesianHeading(target_pose);
|
// robot_arm->MoveCartesianHeading(target_pose);
|
||||||
robot_arm->MoveCartesian(target_pose);
|
robot_arm->MoveCartesian(arm_cmd.target_pose);
|
||||||
|
|
||||||
|
|
||||||
// robot_arm->StepTrajectory();
|
// robot_arm->StepTrajectory();
|
||||||
@ -314,7 +318,7 @@ void Task_arm_main(void* argument) {
|
|||||||
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
|
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
|
||||||
// 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
|
// 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
|
||||||
if (ik_test_enable) {
|
if (ik_test_enable) {
|
||||||
ik_test_ret = Arm6dof_InverseKinematics(&target_pose, ¤t_angles,
|
ik_test_ret = Arm6dof_InverseKinematics(&arm_cmd.target_pose, ¤t_angles,
|
||||||
&ik_test_result, 0.001f, 50);
|
&ik_test_result, 0.001f, 50);
|
||||||
ik_test_ret++;
|
ik_test_ret++;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -62,7 +62,7 @@ void Task_cmd(void *argument) {
|
|||||||
/* 获取命令发送到各模块 */
|
/* 获取命令发送到各模块 */
|
||||||
cmd_for_chassis = CMD_GetChassisCmd(&cmd);
|
cmd_for_chassis = CMD_GetChassisCmd(&cmd);
|
||||||
cmd_for_arm = CMD_GetArmCmd(&cmd);
|
cmd_for_arm = CMD_GetArmCmd(&cmd);
|
||||||
osMessageQueueReset(task_runtime.msgq.chassis.cmd);
|
osMessageQueueReset(task_runtime.msgq.chassis.cmd);
|
||||||
osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
|
osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
|
||||||
|
|
||||||
osMessageQueuePut(task_runtime.msgq.arm.cmd, cmd_for_arm, 0, 0);
|
osMessageQueuePut(task_runtime.msgq.arm.cmd, cmd_for_arm, 0, 0);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user