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

添加pose/joint控制

Browse Source
This commit is contained in:
xxxxm 2026-03-20 01:11:33 +08:00
parent bf2633c3de
commit edc200bdf0
4 changed files with 134 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
User/component/arm_kinematics/arm6dof.h
View File

@ -66,6 +66,8 @@ typedef enum {
typedef struct {
bool enable; /* 使能开关 */
bool set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
bool gripper_toggle; /* 夹爪状态切换置位（单帧脉冲） */
bool gripper_close; /* 夹爪目标状态：true=夹紧, false=松开 */
Arm_CtrlType_t ctrl_type; /* 控制类型 */
Arm6dof_Pose_t target_pose; /* 目标末端位姿（世界坐标系，供调试观察） */
Arm6dof_JointAngles_t target_joints; /* 目标六个关节角度（自定义控制器模式） */

18
User/module/arm_oop.hpp
View File

@ -592,7 +592,23 @@ public:
}
if(control_.mode == ControlMode::JOINT_POSITION) {
// JOINT_POSITION 模式由上层写入目标角，这里做边界校验与状态同步
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (!joints_[i]) {
continue;
}
const float target = joints_[i]->GetTargetAngle();
const auto& p = joints_[i]->GetParams();
if (!std::isfinite(target) || target < p.qmin || target > p.qmax) {
RelaxAllJoints();
control_.state = MotionState::ERROR;
return -1;
}
inverseKinematics_.angles.q[i] = target;
}
inverseKinematics_.valid = true;
traj_.valid = true;
traj_.active = false;
}
// GRAVITY_COMP 模式：保持当前位置 + 重力补偿

60
User/module/cmd/cmd.c
View File

@ -3,6 +3,7 @@
*/
#include "cmd.h"
#include "bsp/time.h"
#include <math.h>
#include <stdint.h>
#include <string.h>
@ -256,15 +257,16 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
/*
* 遥控器控制机械臂末端位姿 —— 笛卡尔增量方案
* 遥控器控制机械臂：拨杆选择 pose 或 joint
*
* 左拨杆 SW_L (sw[0]) —— 整体使能:
* 左拨杆 SW_L (sw[0]) —— 使能与控制类型:
* UP → 失能（电机松弛/重力补偿）
* MID → 使能，笛卡尔增量控制
* DOWN → 使能（保留备用）
* MID → 使能，POSE 笛卡尔增量控制
* DOWN → 使能，JOINT 关节角增量控制
*
* 右拨杆 SW_R (sw[1]) —— 自由度分组:
* 右拨杆 SW_R (sw[1]) —— 子模式/分组 + 夹爪状态切换:
*
* POSE(ctrl_type=ARM_CTRL_REMOTE_CARTESIAN):
* UP 【位置模式】3轴平移 + 偏航
* 右摇杆X (RX) → X 平移
* 右摇杆Y (RY) → Y 平移
@ -277,34 +279,44 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
* 左摇杆X (LX) → Roll 横滚旋转
* 左摇杆Y (LY) → Z 平移（升降）
*
* DOWN → set_target_as_current（目标吸附当前实际位姿，消除累积漂移）
* JOINT(ctrl_type=ARM_CTRL_CUSTOM_JOINT):
* UP → J1/J2/J3 由 RX/RY/LY 增量控制
* MID → J4/J5/J6 由 RX/RY/LY 增量控制
*
* 通用:
* SW_R 进入 DOWN 的边沿触发一次 gripper_toggle，
* 并翻转 gripper_close（true=夹紧, false=松开）。
*/
ctx->output.arm.cmd.set_target_as_current = false;
if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
ctx->output.arm.cmd.set_target_as_current = true;
ctx->output.arm.cmd.gripper_toggle = false;
/* 右拨杆 DOWN 边沿：夹爪状态切换置位（仅输出控制量，不在此执行夹爪动作） */
if (ctx->input.rc.sw[1] == CMD_SW_DOWN && ctx->last_input.rc.sw[1] != CMD_SW_DOWN) {
ctx->output.arm.cmd.gripper_close = !ctx->output.arm.cmd.gripper_close;
ctx->output.arm.cmd.gripper_toggle = true;
}
switch (ctx->input.rc.sw[0]) {
case CMD_SW_MID:
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
break;
case CMD_SW_DOWN:
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
break;
default:
ctx->output.arm.cmd.enable = false;
goto end;
}
/* 遥控器模式使用笛卡尔位姿累积控制 */
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
/* set_target_as_current 置位时不叠加摇杆增量，由上层负责同步位姿基准 */
if (ctx->output.arm.cmd.set_target_as_current) return;
/* 摇杆速度直接积分到 target_pose（坐标系语义由 arm_main 的 control_frame 决定） */
/* 按控制类型分别处理增量输入 */
const float pos_scale = 0.3f; /* 末端线速度上限 (m/s) */
const float rot_scale = 1.0f; /* 末端角速度上限 (rad/s) */
const float joint_scale = 1.0f; /* 关节角速度上限 (rad/s) */
const float dt = ctx->timer.dt;
if (ctx->output.arm.cmd.ctrl_type == ARM_CTRL_REMOTE_CARTESIAN) {
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
/* 位置模式：3轴平移 + 偏航 */
@ -321,8 +333,26 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_scale * dt;
break;
default:
/* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
break;
}
} else {
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_right.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[1] += ctx->input.rc.joy_right.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_left.y * joint_scale * dt;
break;
case CMD_SW_MID:
ctx->output.arm.cmd.target_joints.q[3] += ctx->input.rc.joy_right.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[4] += ctx->input.rc.joy_right.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[5] += ctx->input.rc.joy_left.y * joint_scale * dt;
break;
default:
/* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
break;
}
}
end:
return;
}

53
User/task/arm_main.cpp
View File

@ -154,10 +154,26 @@ static Arm6dof_Pose_t SyncTargetToFrame(uint8_t frame, const Arm6dof_Pose_t& cur
}
static void SyncCommandTargetFromCurrent() {
Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
if (!robot_arm) {
return;
}
const Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
Arm6dof_JointAngles_t sync_joints = arm_cmd.target_joints;
for (int i = 0; i < 6; ++i) {
if (joints[i]) {
sync_joints.q[i] = joints[i]->GetCurrentAngle();
}
}
arm_cmd.target_pose = sync;
arm_cmd.target_joints = sync_joints;
// 同步上游CMD中的目标值，避免下一帧被旧目标覆盖导致跳变。
// 注意：这里只同步目标量，不修改夹爪toggle等一次性控制位。
if (cmd_for_arm) {
cmd_for_arm->target_pose = sync;
cmd_for_arm->target_joints = sync_joints;
}
}
@ -219,6 +235,23 @@ static void RunCartesianControl(ControlMode mode) {
traj_progress_dbg = robot_arm->GetTrajProgress();
}
static void RunJointControlFromCmd() {
float target_joints[6];
for (int i = 0; i < 6; ++i) {
target_joints[i] = arm_cmd.target_joints.q[i];
}
robot_arm->SetMode(ControlMode::JOINT_POSITION);
if (robot_arm->MoveJoint(target_joints) != 0) {
// 超限时回收错误并把命令吸附到当前角，避免持续触发错误
robot_arm->ResetError();
SyncCommandTargetFromCurrent();
}
robot_arm->Control();
ik_from_fk_result = robot_arm->GetIkAngles();
traj_progress_dbg = 0.0f;
}
extern "C" {
void Task_arm_main(void* argument) {
@ -290,7 +323,12 @@ void Task_arm_main(void* argument) {
// 更新机械臂状态（读取关节角度、FK、重力补偿计算）
robot_arm->Update();
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
const osStatus_t arm_cmd_rx = osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
if (arm_cmd_rx != osOK) {
// 未收到新命令时清理单帧脉冲，避免重复消费上一帧toggle类信号
arm_cmd.gripper_toggle = false;
arm_cmd.set_target_as_current = false;
}
robot_arm->Enable(arm_cmd.enable);
@ -306,6 +344,13 @@ void Task_arm_main(void* argument) {
SyncCommandTargetFromCurrent();
}
// 控制类型切换沿：同步目标，避免 pose/joint 切换时命令继承旧目标导致突变
static Arm_CtrlType_t last_ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
if (arm_cmd.enable && arm_cmd.ctrl_type != last_ctrl_type) {
SyncCommandTargetFromCurrent();
}
last_ctrl_type = arm_cmd.ctrl_type;
// 每帧同步调试观察变量
for (int i = 0; i < 6; ++i) {
@ -369,7 +414,11 @@ void Task_arm_main(void* argument) {
// ★ 2.5D 解析降维控制算法 ★
// 已经集成到底层 arm_oop 的 CARTESIAN_ANALYTICAL 模式中，完全与原轨迹规划流程接轨
// =================================================================================
if (arm_cmd.ctrl_type == ARM_CTRL_CUSTOM_JOINT) {
RunJointControlFromCmd();
} else {
RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
}
break;
}
}