[ROS 系列学习教程] 建模与仿真 - 使用 ros_control 控制六轴串联机械臂

ROS 系列学习教程(总目录)

ROS2 系列学习教程(总目录)

本文目录

前面介绍了用 diff_drive_controller 控制两轮差速机器人,本节介绍用 position_controllers/JointTrajectoryController(位置轨迹控制器)控制一台 六轴串联机械臂。与差速底盘只关心左右轮速度不同,机械臂的控制目标是六个关节的位姿,并进一步通过正/逆运动学让末端执行器到达期望位姿。以 UR20 机械臂为例。

一、六轴串联机械臂

六轴串联机械臂(6R 机械臂)由 6 个转动关节依次串联而成,具有 6 个自由度,理论上可让末端到达工作空间内任意位置与姿态。以下图 UR20 为例,6 个关节自基座到末端依次为:

关节 关节连接的连杆 说明(关系仅参考上图)
shoulder_pan_joint shoulder_link 和 base_link 绕 base_link 的Z轴旋转,决定整个机械臂的水平朝向
shoulder_lift_joint upper_arm_link 和 shoulder_link 绕 shoulder_link 的Y轴旋转,控制大臂前后摆动
elbow_joint upper_arm_link 和 forearm_link 绕 forearm_link 的X轴旋转,控制小臂俯仰
wrist_1_joint wrist_1_link 和 forearm_link 绕 wrist_1_link 的Z轴旋转,控制末端上下摆动
wrist_2_joint wrist_2_link 和 wrist_1_link 绕 wrist_2_link 的Z轴旋转,控制末端左右摆动
wrist_3_joint wrist_3_link 和 wrist_2_link 绕 wrist_3_link 的Z轴旋转,控制末端轴向旋转

前 3 个关节合称"位置关节",主要决定末端的位置;后 3 个关节构成球形手腕,主要决定末端的姿态。这种"位置 + 姿态"的分工是工业机械臂最常见的构型。UR20 的名义可达半径(肩关节到末端)约为 1.75 m,额定负载 20 kg。

在 URDF 中,这 6 个关节通过 transmission_interface/SimpleTransmission 声明为 hardware_interface/PositionJointInterface:

xml 复制代码
<transmission name="${prefix}elbow_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}elbow_joint">
    <hardwareInterface>${hw_interface}</hardwareInterface>
  </joint>
  <actuator name="${prefix}elbow_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

hw_interface 由 load_ur20.launch 传入,默认即为 hardware_interface/PositionJointInterface。这决定了硬件抽象层必须注册"关节状态接口 + 位置指令接口",控制器也只能下发位置量。

二、机械臂运动学模型

控制器直接操作的是关节角,而任务通常描述的是末端位姿,二者通过运动学联系。

2.1 正运动学

正运动学(Forward Kinematics)在已知各关节角 q = q 1 , ... , q 6 T q=q_1,\\dots,q_6^T q=q1,...,q6T 时求末端位姿。按 URDF 的连杆父子关系,把每个关节的齐次变换依次左乘:

0 T 6 = A 1 ( q 1 ) A 2 ( q 2 ) ⋯ A 6 ( q 6 ) {}^{0}T_{6} = A_1(q_1)A_2(q_2)\cdots A_6(q_6) 0T6=A1(q1)A2(q2)⋯A6(q6)

其中每个 A i A_i Ai 由关节的 <origin>(平移与姿态)和绕关节轴的旋转 R o t ( q i ) \mathrm{Rot}(q_i) Rot(qi) 组成。 0 T 6 {}^{0}T_6 0T6 的平移列即末端位置,旋转矩阵即末端姿态。本项目的 robot_kinematics.cpp 从 /robot_description 解析出连杆链,用四元数转旋转矩阵、齐次变换复合等方法自包含地实现了正运动学,不依赖 MoveIt/KDL。

2.2 逆运动学

逆运动学(Inverse Kinematics)由期望末端位姿反求关节角。6R 机械臂可以有解析解,但换构型就要重推公式;本项目采用通用数值解法------有限差分雅可比 + 阻尼最小二乘(Damped Least Squares):

Δ x = x d e s − x ( q ) Δ q = J T ( q ) J ( q ) J T ( q ) + λ 2 I − 1 Δ x q k + 1 = q k + Δ q \Delta x = x_{des}-x(q)\\ \Delta q = J^{T}(q)\leftJ(q)J\^{T}(q)+\\lambda\^{2}I\\right^{-1}\Delta x\\ q_{k+1} = q_k + \Delta q Δx=xdes−x(q)Δq=JT(q)J(q)JT(q)+λ2I−1Δxqk+1=qk+Δq

其中 J ( q ) = ∂ x / ∂ q J(q)=\partial x/\partial q J(q)=∂x/∂q 是末端位姿误差对关节角的雅可比矩阵,用中心差分近似:

J i j ≈ x i ( q + δ e j ) − x i ( q − δ e j ) 2 δ J_{ij} \approx \frac{x_i(q+\delta e_j)-x_i(q-\delta e_j)}{2\delta} Jij≈2δxi(q+δej)−xi(q−δej)

λ > 0 \lambda>0 λ>0 为阻尼系数(代码中 ik_damping 默认 0.03),保证在奇异位形附近 J J T JJ^T JJT 接近奇异时仍能稳定求逆。迭代直到位置误差小于 ik_epsilon(默认 10 − 4 10^{-4} 10−4 m)或达到最大迭代次数(默认 200)。求解 6 × 6 6\times6 6×6 线性方程组用的是带部分主元的高斯消元,整个求解器文件内自包含。

三、机械臂构型模型 D-H 参数法

串联机械臂由若干连杆经关节依次连接而成。要对它做运动学分析,第一步是用一套统一规则为每根连杆建立坐标系,并用最少的参数把相邻连杆坐标系的关系确定下来。常用的是D-H 参数法(Denavit-Hartenberg Parameterization),它用 4 个参数描述任意相邻两连杆之间的相对位姿。

3.1 D-H 参数的定义

给每个连杆固连一个坐标系 { i } \{i\} {i},相邻坐标系 { i − 1 } \{i-1\} {i−1} 与 { i } \{i\} {i} 之间的相对位姿可由 4 个参数确定:

  • 连杆长度 a i a_i ai:沿 x i x_i xi 轴,从 z i − 1 z_{i-1} zi−1 轴到 z i z_i zi 轴的距离;
  • 连杆扭角 α i \alpha_i αi:绕 x i x_i xi 轴,从 z i − 1 z_{i-1} zi−1 轴转到 z i z_i zi 轴的角度;
  • 连杆偏距 d i d_i di:沿 z i − 1 z_{i-1} zi−1 轴,从 x i − 1 x_{i-1} xi−1 轴到 x i x_i xi 轴的距离;
  • 关节角 θ i \theta_i θi:绕 z i − 1 z_{i-1} zi−1 轴,从 x i − 1 x_{i-1} xi−1 轴转到 x i x_i xi 轴的角度。

对于转动关节, θ i \theta_i θi 是关节变量,其余 3 个参数为常量;对于移动关节则相反。UR20 的 6 个关节全部为转动关节,因此每行只有 θ i \theta_i θi 随控制指令变化。

D-H 参数法有两种常见约定:

  • 标准 D-H(Classic D-H) :连杆 i i i 的坐标系固连在连杆 i i i 的远端, z i − 1 z_{i-1} zi−1 轴沿关节 i i i 的轴线;
  • 改进 D-H(Modified D-H,Craig 表示法) :连杆 i i i 的坐标系固连在连杆 i i i 的近端(即关节 i i i 处), z i z_i zi 轴沿关节 i i i 的轴线, a i − 1 a_{i-1} ai−1、 α i − 1 \alpha_{i-1} αi−1 的下标相应前移。

两者得到的末端位姿完全等价,只是坐标系原点的选取不同。UR 系列机器人官方文档采用改进 D-H 约定,本项目亦沿用该约定。

3.2 连杆坐标系与齐次变换

采用标准 D-H 时,从 { i − 1 } \{i-1\} {i−1} 到 { i } \{i\} {i} 的齐次变换按"绕 z z z 转、沿 z z z 移、沿 x x x 移、绕 x x x 转"的顺序复合:

i − 1 T i = Rot ⁡ ( z i − 1 , θ i ) Trans ⁡ ( z i − 1 , d i ) Trans ⁡ ( x i , a i ) Rot ⁡ ( x i , α i ) {}^{i-1}T_i = \operatorname{Rot}(z_{i-1},\theta_i)\ \operatorname{Trans}(z_{i-1},d_i)\ \operatorname{Trans}(x_i,a_i)\ \operatorname{Rot}(x_i,\alpha_i) i−1Ti=Rot(zi−1,θi) Trans(zi−1,di) Trans(xi,ai) Rot(xi,αi)

展开为矩阵:

i − 1 T i = cos ⁡ θ i − sin ⁡ θ i cos ⁡ α i sin ⁡ θ i sin ⁡ α i a i cos ⁡ θ i sin ⁡ θ i cos ⁡ θ i cos ⁡ α i − cos ⁡ θ i sin ⁡ α i a i sin ⁡ θ i 0 sin ⁡ α i cos ⁡ α i d i 0 0 0 1 {}^{i-1}T_i=\begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i\\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i\\ 0 & \sin\alpha_i & \cos\alpha_i & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix} i−1Ti= cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1

改进 D-H 把平移与旋转的顺序调整为"绕 x x x 转、沿 x x x 移、绕 z z z 转、沿 z z z 移":

i − 1 T i = Rot ⁡ ( x i − 1 , α i − 1 ) Trans ⁡ ( x i − 1 , a i − 1 ) Rot ⁡ ( z i , θ i ) Trans ⁡ ( z i , d i ) {}^{i-1}T_i = \operatorname{Rot}(x_{i-1},\alpha_{i-1})\ \operatorname{Trans}(x_{i-1},a_{i-1})\ \operatorname{Rot}(z_i,\theta_i)\ \operatorname{Trans}(z_i,d_i) i−1Ti=Rot(xi−1,αi−1) Trans(xi−1,ai−1) Rot(zi,θi) Trans(zi,di)

i − 1 T i = cos ⁡ θ i − sin ⁡ θ i 0 a i − 1 sin ⁡ θ i cos ⁡ α i − 1 cos ⁡ θ i cos ⁡ α i − 1 − sin ⁡ α i − 1 − d i sin ⁡ α i − 1 sin ⁡ θ i sin ⁡ α i − 1 cos ⁡ θ i sin ⁡ α i − 1 cos ⁡ α i − 1 d i cos ⁡ α i − 1 0 0 0 1 {}^{i-1}T_i=\begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1}\\ \sin\theta_i\cos\alpha_{i-1} & \cos\theta_i\cos\alpha_{i-1} & -\sin\alpha_{i-1} & -d_i\sin\alpha_{i-1}\\ \sin\theta_i\sin\alpha_{i-1} & \cos\theta_i\sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i\cos\alpha_{i-1}\\ 0 & 0 & 0 & 1 \end{bmatrix} i−1Ti= cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−disinαi−1dicosαi−11

把 6 个连杆变换依次左乘,即得末端坐标系相对基坐标系的位姿:

0 T 6 = 0 T 1   1 T 2   2 T 3   3 T 4   4 T 5   5 T 6 {}^{0}T_{6} = {}^{0}T_1\,{}^{1}T_2\,{}^{2}T_3\,{}^{3}T_4\,{}^{4}T_5\,{}^{5}T_6 0T6=0T11T22T33T44T55T6

这正是正运动学中 A 1 A 2 ⋯ A 6 A_1A_2\cdots A_6 A1A2⋯A6 的具体形式------每个 A i A_i Ai 就是由 D-H 参数构成的连杆变换矩阵。

3.3 UR20 的连杆参数

UR20 的连杆几何定义在 arm_description/config/ur20/default_kinematics.yaml 中。该文件用每根连杆相对父连杆的平移 ( x , y , z ) (x,y,z) (x,y,z) 和旋转 ( r o l l , p i t c h , y a w ) (roll,pitch,yaw) (roll,pitch,yaw) 描述构型,这些量正是 D-H 参数的具体数值。整理成改进 D-H 参数表如下(长度单位 m,角度单位 rad):

连杆 i i i 对应关节 a i − 1 a_{i-1} ai−1 α i − 1 \alpha_{i-1} αi−1 d i d_i di 关节变量 θ i \theta_i θi
1 shoulder_pan 0 0 0.2363 q 1 q_1 q1
2 shoulder_lift 0 + π / 2 +\pi/2 +π/2 0 q 2 q_2 q2
3 elbow 0.862 0 0 q 3 q_3 q3
4 wrist_1 0.7287 0 0.201 q 4 q_4 q4
5 wrist_2 0 + π / 2 +\pi/2 +π/2 0.1593 q 5 q_5 q5
6 wrist_3 0 − π / 2 -\pi/2 −π/2 0.1543 q 6 q_6 q6

其中 0.2363 是肩部相对底座的高度(yaml 中 shoulder.z),0.862 与 0.7287 分别是大臂和小臂长度(对应 forearm.x、wrist_1.x 的绝对值),0.201/0.1593/0.1543 是腕部三段偏距。大臂与小臂长度之和加上腕部偏距,决定了 UR20 约 1.75 m 的名义可达半径,与业务节点中 max_reach 默认值一致。

需要说明:不同资料对 a a a 的符号和下标约定可能不同(有些资料把大臂、小臂长度写成负值),表中数值依据本项目 URDF 的坐标系方向给出,具体应以 default_kinematics.yaml 和 urdf/inc/ur20_macro.xacro 为准。

3.4 与 URDF 建模的对应关系

D-H 参数法是"手工"建立运动学模型的经典手段,而本项目采用 URDF 描述机械臂,两者本质相通但表达方式不同:

  • URDF 用连杆 <origin xyz="..." rpy="..."/> 标签直接写出相邻连杆的平移与旋转,相当于把 D-H 的齐次变换矩阵显式参数化,但不受"关节轴必须取作 z z z 轴"等 D-H 规则的约束,因而更通用;
  • robot_kinematics.cpp 从 /robot_description 解析出连杆链,把每个 <origin> 转成齐次变换(由四元数构造旋转矩阵),再按父子顺序复合得到 0 T 6 {}^{0}T_6 0T6,其效果与 D-H 链式相乘完全一致;
  • 逆运动学同样建立在这套连杆几何之上:数值解直接对正运动学求雅可比,无需像解析解那样依赖特定构型的 D-H 推导。这正是 选择阻尼最小二乘数值法的原因------换一款机械臂只需换 URDF,无需重写 IK 公式。

四、对外接口

position_controllers/JointTrajectoryController 提供 action 与话题两种下发方式,joint_state_controller 负责发布关节状态。

4.1 输入接口

  • ~/follow_joint_trajectory(control_msgs/FollowJointTrajectory)

    动作接口,接收带时间戳的多点关节轨迹,控制器按时间插值并跟踪。业务节点就是通过它下发轨迹,可对接 MoveIt、rqt_joint_trajectory_controller 或自定义 action client。

  • ~/command(trajectory_msgs/JointTrajectory)

    话题接口,直接发布一条关节轨迹,无需 action 客户端。

作为对比,备选控制器 arm_position_controller(position_controllers/JointGroupPositionController)的输入是 ~/command(std_msgs/Float64MultiArray),一次性下发 6 个关节角。

4.2 输出接口

  • ~/state(control_msgs/JointTrajectoryControllerState)

    控制器跟踪状态,包含期望/实际的关节位置、速度与误差。

  • /joint_states(sensor_msgs/JointState)

    由 joint_state_controller 以 50 Hz 发布,robot_state_publisher 据此计算并广播各连杆 TF,供 RViz 显示。

五、控制器参数

JointTrajectoryController 常用参数如下(完整列表见 wiki):

参数 数据类型 说明
type string 控制器类型,此处为 position_controllers/JointTrajectoryController
joints string\[\] 控制器管理的关节名列表,必须与 URDF 一致
constraints/goal_time double 轨迹结束后的宽限时间(s),在此期间仍可修正终点误差
constraints/stopped_velocity_tolerance double 判断关节已停止的速度阈值(rad/s)
constraints/<joint>/trajectory double 轨迹跟踪误差容差(rad)
constraints/<joint>/goal double 终点位置误差容差(rad)
stop_trajectory_duration double 紧急停止时轨迹收敛到停止点的时间(s,默认 0.5)
state_publish_rate double 状态发布频率(Hz,默认 50)
action_monitor_rate double action 目标监控频率(Hz,默认 20)

六、配置控制器参数

arm_control/config/arm_control.yaml 同时给出了硬件接口参数、关节限位和两种控制器配置。

最小配置(仅轨迹控制器):

yaml 复制代码
arm_trajectory_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint

完整配置中,硬件接口参数与关节限位被控制器和硬件抽象层共用:

yaml 复制代码
hardware_interface:
  joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
  # 上电初始位姿,顺序与 joints 一致
  initial_positions: [0.0, -1.5708, 1.5708, -1.5708, 0.0, 0.0]

joint_limits:
  shoulder_pan_joint:
    has_position_limits: true
    min_position: -6.283185307
    max_position:  6.283185307
    has_velocity_limits: true
    max_velocity: 2.094395102
    has_effort_limits: true
    max_effort: 730.0
  # ... 其余 5 个关节类似

joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

arm_trajectory_controller:
  type: "position_controllers/JointTrajectoryController"
  joints: # 同上 6 个关节
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    shoulder_pan_joint: {trajectory: 0.20, goal: 0.01}
    # ... 其余关节
  stop_trajectory_duration: 0.5
  state_publish_rate: 50
  action_monitor_rate: 20

arm_position_controller:
  type: "position_controllers/JointGroupPositionController"
  joints: # 同上 6 个关节

注意 initial_positions 让机械臂上电停在"折起来"的姿态([0, -1.5708, 1.5708, -1.5708, 0, 0]),便于在 RViz 中观察;joint_limits 同时供控制器限位和硬件接口 write() 中的位置钳位使用,两者数值保持一致。两个控制器占用同一批关节,不能同时启动,由 launch 文件的 controller 参数二选一。

七、编写硬件抽象接口

与差速底盘使用 VelocityJointInterface 不同,机械臂使用 PositionJointInterface。头文件 arm_hardware_interface.h:

cpp 复制代码
class ArmHWInterface : public hardware_interface::RobotHW
{
public:
    // 单个关节的全部状态与指令,接口句柄直接持有这些成员的地址
    struct JointInfo
    {
        std::string name; // 关节名,必须与 URDF / 配置中的名字一致
        double cmd;       // 控制器写入的位置指令 (rad)
        double pos;       // 反馈位置 (rad)
        double vel;       // 反馈速度 (rad/s)
        double eff;       // 反馈力矩 (N*m)
        double min_pos;   // 位置下限 (rad)
        double max_pos;   // 位置上限 (rad)
    };

    explicit ArmHWInterface(ros::NodeHandle &nh);
    bool init();                                  // 读取参数并注册硬件接口
    void read(const ros::Duration &period);       // 从硬件读状态(仿真时为空)
    void write(const ros::Duration &period);      // 下发指令并推进仿真状态

private:
    ros::NodeHandle m_nh;
    hardware_interface::JointStateInterface m_jnt_state_interface;
    hardware_interface::PositionJointInterface m_jnt_pos_interface;

    // 注册句柄时保存了 JointInfo 成员的地址,此后不得再改变 vector 容量
    std::vector<JointInfo> m_joints;
};

源文件 arm_hardware_interface.cpp 中,init() 读取参数、注册接口,read()/write() 构成仿真闭环:

cpp 复制代码
bool ArmHWInterface::init()
{
    // 1. 关节列表:优先用参数服务器配置,缺失时回退到 UR20 默认 6 关节
    std::vector<std::string> joint_names;
    if (!m_nh.getParam("hardware_interface/joints", joint_names) || joint_names.empty())
        joint_names.assign(kDefaultJoints, kDefaultJoints + kDefaultJointCount);

    // 一次性填满 m_joints:注册句柄之后不能再扩容,否则句柄里的指针会失效
    m_joints.clear();
    m_joints.reserve(joint_names.size());
    for (std::size_t i = 0; i < joint_names.size(); ++i)
        m_joints.push_back(JointInfo(joint_names[i]));

    // 2. 初始位姿 + 3. 关节限位(write 中做钳位)
    std::vector<double> initial_positions;
    if (m_nh.getParam("hardware_interface/initial_positions", initial_positions) &&
        initial_positions.size() == m_joints.size())
    {
        for (std::size_t i = 0; i < m_joints.size(); ++i)
        {
            m_joints[i].pos = initial_positions[i];
            m_joints[i].cmd = initial_positions[i];
        }
    }
    for (std::size_t i = 0; i < m_joints.size(); ++i)
    {
        JointInfo &joint = m_joints[i];
        const std::string base = "joint_limits/" + joint.name;
        m_nh.getParam(base + "/min_position", joint.min_pos);
        m_nh.getParam(base + "/max_position", joint.max_pos);
        joint.pos = std::max(joint.min_pos, std::min(joint.pos, joint.max_pos));
        joint.cmd = joint.pos;
    }

    // 4. 注册 ros_control 接口:每个关节既要能读状态,也要能接受位置指令
    for (std::size_t i = 0; i < m_joints.size(); ++i)
    {
        JointInfo &joint = m_joints[i];
        hardware_interface::JointStateHandle state_handle(
            joint.name, &joint.pos, &joint.vel, &joint.eff);
        m_jnt_state_interface.registerHandle(state_handle);

        hardware_interface::JointHandle pos_handle(
            m_jnt_state_interface.getHandle(joint.name), &joint.cmd);
        m_jnt_pos_interface.registerHandle(pos_handle);
    }
    registerInterface(&m_jnt_state_interface);
    registerInterface(&m_jnt_pos_interface);
    return true;
}

void ArmHWInterface::read(const ros::Duration & /*period*/)
{
    // 接真实机器人时,在这里读编码器/驱动器,刷新 pos、vel、eff。
    // 本示例状态由 write() 同步推进,控制器读到的是上一周期的状态,
    // 这与真实系统存在一拍采样延迟的情况一致。
}

void ArmHWInterface::write(const ros::Duration &period)
{
    const double dt = period.toSec();
    for (std::size_t i = 0; i < m_joints.size(); ++i)
    {
        JointInfo &joint = m_joints[i];
        // 位置限幅,模拟驱动器对越界指令的保护
        const double target = std::max(joint.min_pos, std::min(joint.cmd, joint.max_pos));
        joint.cmd = target;
        // 理想位置执行器:指令即反馈,速度由位置差分得到,无负载时力矩为 0
        if (dt > 0.0)
            joint.vel = (target - joint.pos) / dt;
        joint.pos = target;
        joint.eff = 0.0;
    }
}

控制节点 arm_control_node.cpp 按 read() → cm.update() → write() 的顺序循环,频率可配(默认 100 Hz):

cpp 复制代码
int main(int argc, char **argv)
{
    ros::init(argc, argv, "arm_control_node");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    double control_rate = 100.0;
    private_nh.param("control_rate", control_rate, 100.0);

    arm_control::ArmHWInterface arm(nh);
    if (!arm.init())
    {
        ROS_FATAL("Failed to initialize the arm hardware interface.");
        return 1;
    }
    controller_manager::ControllerManager cm(&arm, nh);

    // 轨迹控制器的 action server、controller_manager 的 service 需要回调线程
    ros::AsyncSpinner spinner(2);
    spinner.start();

    ros::Rate rate(control_rate);
    ros::Time last_time = ros::Time::now();
    while (ros::ok())
    {
        const ros::Time now = ros::Time::now();
        ros::Duration period = now - last_time;
        last_time = now;
        // 首帧或使用仿真时间时 period 可能为 0,退回名义周期
        if (period.toSec() <= 0.0 || period.toSec() > 1.0)
            period = rate.expectedCycleTime();

        arm.read(period);
        cm.update(now, period);
        arm.write(period);
        rate.sleep();
    }
    return 0;
}

这里没有真实驱动器:write() 用"理想位置执行器"把控制器下发的位置指令直接当作编码器反馈,并由位置差分得到速度。接真实机器人时只需把 read() 改为读编码器、write() 改为下发总线指令,控制器侧无需改动。另一个细节是句柄持有 JointInfo 成员的地址,所以 init() 中先 reserve() 一次性填满 m_joints,注册句柄后不能再扩容。

八、控制机械臂运动

启动方式:

bash 复制代码
# 默认:轨迹控制器(action / 话题接口,可对接 MoveIt)
roslaunch arm_control arm_control.launch

# 备选:一组关节角度控制器(命令行调试最方便)
roslaunch arm_control arm_control.launch controller:=arm_position_controller
rostopic pub -1 /arm_position_controller/command std_msgs/Float64MultiArray \
  "data: [0.0, -1.57, 1.57, -1.57, 0.0, 0.0]"

轨迹模式下,最简单的是直接用话题下发一条单点轨迹:

bash 复制代码
rostopic pub -1 /arm_trajectory_controller/command trajectory_msgs/JointTrajectory \
  "{joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint], points: [{positions: [0.5, -1.2, 1.2, -1.0, 0.0, 0.0], time_from_start: {secs: 2, nsecs: 0}}]}"

业务节点 arm_business 演示了完整的"任务空间 → 关节空间"流程:从 /robot_description 构建运动链,以启动时末端位姿为参考,在可达球内随机生成 8 个到肩关节距离互不相同的目标点,逐点数值逆解并检查整条臂是否触地,最后通过 follow_joint_trajectory action 依次下发轨迹,让末端遍历这些点。关键代码如下:

cpp 复制代码
// 两段式轨迹:起点取当前位形,保证与上一条轨迹连续
control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory.joint_names = joint_names_;

trajectory_msgs::JointTrajectoryPoint start_point;
start_point.positions = q_prev;                 // 上一条轨迹的终点
start_point.velocities.assign(dof_, 0.0);
start_point.time_from_start = ros::Duration(0.0);
goal.trajectory.points.push_back(start_point);

trajectory_msgs::JointTrajectoryPoint end_point;
end_point.positions = q_goal;                   // 逆解得到的关节角
end_point.velocities.assign(dof_, 0.0);
end_point.time_from_start = ros::Duration(traverse_time_);
goal.trajectory.points.push_back(end_point);

随机点与逆解的核心逻辑:

cpp 复制代码
// 分层半径:第 i 个半径落在互不相交的区间,天然两两不同
const double r = max_radius * (frac_lo + band * (static_cast<double>(i) + unit(rng)));
...
if (!kinematics_.solveInverseKinematics(cur_ref, trial[i], q_trial,
                                        ik_epsilon_, ik_max_iterations_, ik_damping_, ...))
    all_ok = false;
// 地面碰撞检查:整条臂各连杆端点必须高于 ground_clearance_
if (kinematics_.armMinZ(q_trial) < ground_clearance_)
    all_ok = false;

运行:

bash 复制代码
roslaunch business_test arm_business.launch
# 常用可调参数
roslaunch business_test arm_business.launch \
  max_reach:=1.75 traverse_time:=1.0 hold_time:=0.2 loops:=1 rviz:=false

traverse_time 控制相邻点的运动时间(越小越快),hold_time 为到点停留时间,loops:=0 表示无限循环,orientation_mode 可选保持启动姿态或让末端 Z 轴沿可达球半径指向外侧。RViz 中以黄色圆点标出这 8 个遍历目标点。

相关推荐
IT_陈寒18 分钟前
Vue开发者必知:5个让你效率翻倍的隐藏技巧
前端·人工智能·后端
效率工作实验室1 小时前
企业级AI Agent平台选型指南:主流厂商能力对比与落地场景解析
ai·企业级ai·agent平台选型·agent平台能力对比·企业智能体落地场景
深蓝AI4 小时前
12000 名工程师的高盛,只用四个月就收回了交易员的 AI 编程权限
人工智能·程序员
怕浪猫8 小时前
这个十几个agent 协作项目值得关注
人工智能·面试·github
starzy199010 小时前
深入理解Scala隐式类:扩展方法机制
开发语言·人工智能·scala
深蓝AI10 小时前
为什么 agent 的 demo 都很惊艳,上了生产就不靠谱
人工智能·程序员
数字新视界11 小时前
信创动环监控在机房环境管理中的智能化应用与实践分析
大数据·人工智能·物联网·信息与通信·动环监控系统
岁月宁静11 小时前
DeepSeek Harness 实战与架构解析:让 Agent 从“会聊”到“能交付”
人工智能·python·agent
深蓝AI11 小时前
一个项目五个 AI 同时干活,Claude Code 怎么让它们不互相打架
人工智能·程序员