ROS moveit轨迹规划通信问题

：

moveit轨迹规划好后是通过什么节点或话题将关节信息发送给下位机的？想得到moveit轨迹关键点的关节数据，并向下位机发送，请问move_group是通过什么方式将关节信息发送给下位机的，我要如何输出这些数据呢？

小鱼

@577478126

一、开始之前搞清楚几个问题？

1.MoveIt为什么不能直接控制真实机械臂？

原因是真实机械臂的类型实在是太多了，今天你做一个机械臂，明天他做个机械臂.Moveit只是一个机械臂运动规划的框架，并不负责驱动真实的机械臂。

2.Moveit通过什么控制机械臂？

moveit既然不能直接驱动机械臂，那么必须通过一个媒介来控制它，这个媒介就是我们今天的重点—— 轨迹执行器 。

二、Moveit如何控制rviz和gazebo中虚拟机械臂的？

细心的同学可能已经发现了，moveit每次启动都会加载xx_controller的东西，如果在运行moveit的时候使用

rostopic list

就会看到有一个这样的话题

/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status

如果是用的gazebo可能会名字不一样，但都是相同的前缀，后面都是挂着cancel、feedback、goal、result、status这五个。

这个话题就是轨迹执行服务所订阅的话题，这个轨迹执行者是谁，谁就要提供这五个话题服务（其实这个不是普通的topic，而是ros中的Action通信机制）。

1. 我们可以使用下面的命令来查一查，话题发布者和订阅者分别是谁？

rostopic info /execute_trajectory/goal

Type: moveit_msgs/ExecuteTrajectoryActionGoal

Publishers: 
 * /rviz_monster_4900_3400225440813710105 (http://monster:33953/)

Subscribers: 
 * /move_group (http://monster:34519/)

大家可以跟着小鱼一起尝试哈，大家没有看错，这个话题就是连接moveit和rviz的，好家伙，到这里，moveit和rviz之间的不正当关系终于被机智的我们发现了。

2. moveit如何控制gazebo中的机械臂呢？

很多教程告诉大家，首先要demo.launch中的fake_execution为fasle;

<include file="$(find ur5_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>  
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

这句话是啥意思呢，其实就是告诉rviz我不要你来帮我控制rviz中虚拟的机械臂了，我在外面已经有人了，不需要你了！

那rviz就伤心了，就算你不要我（执行）了，那你总要告诉那个人（gazebo）的(执行)情况，也就是告诉rviz当前机械臂的关节角度，毕竟还要原配，要执行moveit指令的。

3. gazebo和moveit到底谁爱上了谁？

所以接下来我们运行一下gazebo，看一看gazebo到底拿了什么好处从moveit那里，然后又给了什么东西给moveit。

讲true改成false后，不运行gazebo，直接运行moveit，你就会发现，moveit的启动加载指令中会出现这样一个WARN和ERROR：

[ WARN] [1626355641.007776896, 8.232000000]: Waiting for /follow_joint_trajectory to come up
[ WARN] [1626355647.034715552, 14.232000000]: Waiting for /follow_joint_trajectory to come up
[ERROR] [1626355653.060493182, 20.233000000]: Action client not connected: /follow_joint_trajectory
[ INFO] [1626355653.085575856, 20.258000000]: Returned 0 controllers in list

如果运行gazebo，再启动moveit就变成了下面的INFO了

[ INFO] [1626355494.355057943, 322.100000000]: Added FollowJointTrajectory controller for 
[ INFO] [1626355494.355212191, 322.100000000]: Returned 1 controllers in list

就像是小蝌蚪找妈妈一样，moveit一启动就会找对应名字的Action Server，一旦找到就会将其加载起来，留着后面调用。

而gazebo向外提供什么东西呢？

![数据流图](01 Moveit如何控制真实机械臂？.assets/image-20210715221757439-1626358679296.png)

我们再次使用

rostopic list

查看一下话题列表，就会发现多了这个，这个其实是gazebo和moveit的move_group的通信组件。

rostopic list
/arm_controller/follow_joint_trajectory/cancel
/arm_controller/follow_joint_trajectory/feedback
/arm_controller/follow_joint_trajectory/goal
/arm_controller/follow_joint_trajectory/result
/arm_controller/follow_joint_trajectory/status

再具体一些看下图就会明白，rqt_graph是这样的。gazebo订阅arm_controller的goal，发布joint_state话题就这样实现了仿真。

![rqt_graph](01 Moveit如何控制真实机械臂？.assets/image-20210715213207249.png)

4. moveit发送给gazebo的轨迹数据长啥样？

这个大家可以尝试一下，很简单，因为moveit会把自己的请求内容通过一个话题给暴露出来，这个话题就是：

/move_group/display_planned_path

使用

rostopic echo /move_group/display_planned_path

然后随便点击一下rviz中的plan和execute，可以看到这个话题的打印

model_id: "ur5"
trajectory: 
  - 
    joint_trajectory: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: "world"
      joint_names: 
        - shoulder_pan_joint
        - shoulder_lift_joint
        - elbow_joint
        - wrist_1_joint
        - wrist_2_joint
        - wrist_3_joint
      points: 
        - 
          positions: [-0.0005475973198105777, 0.01301509545699897, -4.790678318222774e-05, 0.0006401223916085996, -1.1133751010916626e-05, 5.540891806798953e-05]
          velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
          accelerations: [4.964547491296066e-06, 0.0, 0.0, 0.0, 0.0, 0.0]
          effort: []
          time_from_start: 
            secs: 0
            nsecs:         0
        - 
          positions: [-0.0005122762372628266, 0.012990523657837615, -6.632561628428418e-05, 0.0006650964481281676, 2.001062241064062e-05, -0.6034930544710522]
          velocities: [9.363578155606405e-06, -6.513955555019406e-06, -4.882811364940933e-06, 6.620593515710222e-06, 8.256337394129777e-06, -0.16000000000000003]
          accelerations: [-3.5927592836451216e-21, 0.0, -8.981898209112804e-22, 0.0, 0.0, 0.0]
          effort: []
          time_from_start: 
            secs: 3
            nsecs: 772177896
        - 
          positions: [-0.0004769551547150755, 0.012965951858676258, -8.474444938634064e-05, 0.0006900705046477356, 5.1154995832197863e-05, -1.2070415178601723]
          velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
          accelerations: [-4.964547491296059e-06, 3.4536841762486802e-06, 2.588855297563277e-06, -3.5102233764803286e-06, -4.377490999290573e-06, 0.08483163011052242]
          effort: []
          time_from_start: 
            secs: 7
            nsecs: 544355792
    multi_dof_joint_trajectory: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: ''
      joint_names: []
      points: []
trajectory_start: 
  joint_state: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: "world"
    name: 
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    position: [-0.0005475973198105777, 0.01301509545699897, -4.790678318222774e-05, 0.0006401223916085996, -1.1133751010916626e-05, 5.540891806798953e-05]
    velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    effort: []
  multi_dof_joint_state: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: "world"
    joint_names: []
    transforms: []
    twist: []
    wrench: []
  attached_collision_objects: []
  is_diff: False
---

这个数据其实是机械臂关节空间随时间变化的轨迹点，并且包含时间、速度、加速度信息。gazebo既然都可是使用那这个点该怎么用呢？请你继续往下看：

三、拿到数据，发给真实机械臂

上面说完了如何使用moveit控制虚拟的机械臂，并且也知道了moveit最终控制机械臂的数据长什么样子。

那我们想要控制真实机械臂是不是也就很简单了，我们只需要像gazebo一样，拿到启动一个action，拿到轨迹数据并反实时馈真实机械臂的位姿。

接下来我们一步步的开始。

1. 第一步就是拿到moveit的输出——轨迹数据

自定义Action的Server来拿到数据

上面说了，moveit启动的时候会自动加载对应名称的actionServer，所以如果我们自己写一个actionServer且名字和moveit要的那个保持相同，是不是moveit就会把我们当成执行器了。

我们使用Python来实现：

def on_goal(goal_handle):
    global goal
    goal_handle.set_accepted()
    goal = goal_handle.get_goal()
    goal = goal.trajectory.points
    goal_handle.set_succeeded()

def on_cancel(goal_handle):
    goal_handle.set_aborted()
    goal_handle.set_canceled()

rospy.init_node('calculate_py', disable_signals=True)
server = actionlib.ActionServer("jaka_controller/follow_joint_trajectory",
FollowJointTrajectoryAction,on_goal,on_cancel,auto_start=False)

完成上面的代码，我们其实就能收到对应controller的轨迹数据了。

2. 第二步实时反馈机械臂的关节数据

这一点其实也很简单，无论大家机械臂是什么品牌的，只要能通过串口/网络等连接到我们的电脑，我们使用对应厂家的SDK即可读取机械臂关节角度数据。

读取到数据后，把它通过/joint_state话题发布出去即可。

我们这么做其实就是模仿gazebo，输入轨迹数据，输出实时的真实机械臂的关节数据。

3. 发给真实机械臂

这一步也是最需要注意的一步，因为可能涉及到不同厂家不同机械臂的不同功能。

想要控制真实机械臂之前，要确定真实的机械臂要什么数据，我们从moveit拿到的轨迹数据可以直接发给六个关节去执行吗？答案肯定是可以。但是你会发现，最终机器人执行的效果可能和你想象的不太一样(因为轨迹插值原因)，这里小鱼总结一下。

可以将大家的机械臂分为三类：

3.1 第一类：大厂出的工业臂/协作臂

这类机械臂一般提供轨迹插值算法，可以直接讲moveit生成的关节角度随时间变化的信息通过这类机械臂的控制接口发送给它，这类机械臂会自动进行插值，然后执行。

比如AUBO的机械臂，可以通过起始点和路点和结束点来执行轨迹。

3.2 第二类：小厂出的用于教育类的机械臂

这类机械臂也有对应的操作SDK，只需要认真仔细的阅读它，确定它是 否支持发轨迹进行执行 ，如果不行，那就走另外一条路， 确定其电机的控制周期 ，对moveit生成的关节空间姿态数据 进行三次项或者五次项插值 ，如果不知道怎么做的，可以关注小鱼的微信公众号联系小鱼 来帮忙 ，小鱼后面也会写如何进行关节和迪卡尔空间的轨迹插值算法。

3.3 第三类：自己diy的机械臂

这类机械臂小鱼在大学的时候做过，如果不考虑实时性，其实也比较简单，一般是直接写嵌入式程序驱动伺服电机即可。

小鱼大学时候用的是STM32驱动舵机进行的，大概是在大一的寒假，现在想象觉得很遥远了，不过还挺有意思的。

这类同学呢，小鱼给的建议是通过串口直接连接到电脑上，在电脑上用python写一个节点进行串口通信，来发指令给电机，同时这个节点接收moveit的控制数据，进行轨迹插值后然后执行即可， 同时也要发布每个电机的角度哦 。

空白

@小鱼顶小鱼我也打算尝试在寒假自己做一个机械臂，请问小鱼有啥教程或者参考学习的建议吗？目前考虑的是用3D打印机打印模型，然后通过stm32控制下位机，ros考虑用nano或者直接电脑与stm32通信，对于stm32玩的比较多，底层控制应该没啥问题，主要问题在ros2方面，一方面没怎么学过python和c++，一方面对ros2的认识还在理论方面，所以想请教一下

布鲁斯杰

@小鱼道理都懂了，没有具体的代码，我还是不行呢，鱼哥。拿到moveit规划的数据过后，怎么传呢，那么长一窜的数据，又要对应到不同的舵机上去，真的不知道怎么写了。