如何在ROS中通过rosbag和Dynamixel获取机器人关节位置并驱动关节运动

你播放的/dynamixel_workbench/joint_states是电机状态反馈的只读输出话题，仅用来上报当前关节的位置、速度、力矩数据，控制器不会监听这个话题来执行运动指令，因此播放bag时机械臂不会响应。

• 第一步：确认控制指令话题
  运行dynamixel_workbench_controllers.launch后，执行rosnode info /dynamixel_workbench查看控制器节点订阅的输入话题，常规情况下位置控制模式对应的指令话题为/dynamixel_workbench/joint_position/command（单关节位置指令）或者/dynamixel_workbench/follow_joint_trajectory/goal（轨迹控制指令）。

• 第二步：转换bag数据为控制指令
  你已经录制的joint_states数据需要经过格式转换才能作为控制指令下发，可运行以下简单Python节点完成转换：

  #!/usr/bin/env python3
  import rospy
  from sensor_msgs.msg import JointState
  from std_msgs.msg import Float64MultiArray
  
  # 替换为你自己的控制指令话题
  cmd_pub = rospy.Publisher('/dynamixel_workbench/joint_position/command', Float64MultiArray, queue_size=10)
  
  def states_cb(msg):
      cmd_msg = Float64MultiArray()
      # 直接提取录制的位置数据作为控制指令
      cmd_msg.data = msg.position
      cmd_pub.publish(cmd_msg)
  
  if __name__ == "__main__":
      rospy.init_node('states_to_cmd_converter')
      # 替换为你自己录制的joint_states话题名
      rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, states_cb)
      rospy.spin()
  

  给节点添加可执行权限后即可运行。

• 第三步：开启电机扭矩
  你之前手动拖动机械臂时电机处于扭矩释放状态，驱动前必须开启扭矩：可以通过dynamixel_workbench的GUI工具批量开启，也可以在launch文件中将torque_enable参数设置为true，启动时自动开启。

• 第四步：执行bag回放
  按顺序执行以下操作即可驱动机械臂复现录制的运动：

  1. 启动dynamixel_workbench_controllers.launch
  2. 启动上述格式转换节点
  3. 执行rosbag play <你的bag文件路径>，可添加--rate 0.5参数降低播放速度，避免运动超出发力/转速限制

• 确保转换节点的关节顺序和控制器要求的关节顺序完全一致，避免关节错位运动
• AX12的有效位置范围为0~300度，不要超出该范围避免触发硬件限位报错

内容的提问来源于stack exchange，提问作者shakiba