如何将Moveit2官网教程中panda机械臂的命令接口，从'position'正确地为'velocity'？

龍魂

我跟随Moveit2官网创建了panda机械臂，并成功运行了 demo.launch.py：

MoveIt Setup Assistant — MoveIt Documentation: Rolling documentation

我想将教程熊猫机械臂命令接口从'position'更改为'velocity'。

我更改了两个文件：

ros2_controllers.yaml
panda.ros2_control.xacro

更改后，运行以下命令启动程序：
colcon build
ros2 launch panda_moveit_config demo.launch.py

这时RViz 展示了正确的计划轨迹

但是完成计划后并单击“ Execute”按钮时，rviz中机械臂没有相应的动作（只有之前点击“Plan”后出现的计划轨迹）

我的更改内容是否有问题？或者说正确的更改是什么？

以下是控制台输出，以及我更改的两个文件：

控制台输出：

[rviz2-4] [INFO] [1713256529.569964067] [move_group_interface]: MoveGroup action client/server ready
[move_group-3] [INFO] [1713256529.570737967] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-3] [INFO] [1713256529.570926767] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-4] [INFO] [1713256529.571154567] [move_group_interface]: Planning request accepted
[move_group-3] [INFO] [1713256529.579226867] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-3] [INFO] [1713256529.579303767] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'ompl'
[move_group-3] [INFO] [1713256529.579782367] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'robot_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-3] [INFO] [1713256529.634432567] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[rviz2-4] [INFO] [1713256529.635581867] [move_group_interface]: Planning request complete!
[rviz2-4] [INFO] [1713256529.635675367] [move_group_interface]: time taken to generate plan: 0.0163399 seconds
[move_group-3] [INFO] [1713256535.657329254] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-3] [INFO] [1713256535.657736354] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-3] [INFO] [1713256535.657824254] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-3] [INFO] [1713256535.657872854] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-3] [INFO] [1713256535.658506454] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[rviz2-4] [INFO] [1713256535.658094154] [move_group_interface]: Execute request accepted
[move_group-3] [INFO] [1713256535.660050654] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-3] [INFO] [1713256535.660163154] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-3] [INFO] [1713256535.660240354] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-3] [INFO] [1713256535.660452454] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to robot_arm_controller
[ros2_control_node-5] [INFO] [1713256535.660962554] [robot_arm_controller]: Received new action goal
[ros2_control_node-5] [INFO] [1713256535.661051554] [robot_arm_controller]: Accepted new action goal
[move_group-3] [INFO] [1713256535.661379754] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: robot_arm_controller started execution
[move_group-3] [INFO] [1713256535.661414754] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-5] [INFO] [1713256545.758803767] [robot_arm_controller]: Goal reached, success!
[move_group-3] [INFO] [1713256545.761655467] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'robot_arm_controller' successfully finished
[move_group-3] [INFO] [1713256545.789580068] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...
[move_group-3] [INFO] [1713256545.789703768] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: SUCCEEDED
[rviz2-4] [INFO] [1713256545.790211768] [move_group_interface]: Execute request success!

更改的文件：

ros2_controllers.yaml:

# This config file is used by ros2_control
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    robot_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    robot_hand_controller:
      type: position_controllers/GripperActionController


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

robot_arm_controller:
  ros__parameters:
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7
    command_interfaces:
      # - position
      # changed: 
      - velocity
    state_interfaces:
      - position
      - velocity
robot_hand_controller:
  ros__parameters:
    joint: panda_finger_joint1

panda.ros2_control.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="panda_ros2_control" params="name initial_positions_file">
    <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
      <hardware>
        <!-- By default, set up controllers for simulation. This won't work on real hardware -->
        <plugin>mock_components/GenericSystem</plugin>
      </hardware>
      <joint name="panda_joint1">
        <!-- changed -->
        <command_interface name="velocity"/> <!-- position -->
        <state_interface name="position">
          <param name="initial_value">${initial_positions['panda_joint1']}</param>
    </state_interface>
    <state_interface name="velocity"/>
  </joint>
  <joint name="panda_joint2">
    <!-- changed -->
    <command_interface name="velocity"/> <!-- position -->
    <state_interface name="position">
      <param name="initial_value">${initial_positions['panda_joint2']}</param>
        </state_interface>
        <state_interface name="velocity"/>
      </joint>
      <joint name="panda_joint3">
        <!-- changed -->
        <command_interface name="velocity"/> <!-- position -->
        <state_interface name="position">
          <param name="initial_value">${initial_positions['panda_joint3']}</param>
    </state_interface>
    <state_interface name="velocity"/>
  </joint>
  <joint name="panda_joint4">
    <!-- changed -->
    <command_interface name="velocity"/> <!-- position -->
    <state_interface name="position">
      <param name="initial_value">${initial_positions['panda_joint4']}</param>
        </state_interface>
        <state_interface name="velocity"/>
      </joint>
      <joint name="panda_joint5">
        <!-- changed -->
        <command_interface name="velocity"/> <!-- position -->
        <state_interface name="position">
          <param name="initial_value">${initial_positions['panda_joint5']}</param>
    </state_interface>
    <state_interface name="velocity"/>
  </joint>
  <joint name="panda_joint6">
    <!-- changed -->
    <command_interface name="velocity"/> <!-- position -->
    <state_interface name="position">
      <param name="initial_value">${initial_positions['panda_joint6']}</param>
        </state_interface>
        <state_interface name="velocity"/>
      </joint>
      <joint name="panda_joint7">
        <!-- changed -->
        <command_interface name="velocity"/> <!-- position -->
        <state_interface name="position">
          <param name="initial_value">${initial_positions['panda_joint7']}</param>
    </state_interface>
    <state_interface name="velocity"/>
  </joint>
  <joint name="panda_finger_joint1">
    <command_interface name="position"/>
    <state_interface name="position">
      <param name="initial_value">${initial_positions['panda_finger_joint1']}</param>
        </state_interface>
        <state_interface name="velocity"/>
      </joint>
    </ros2_control>
  </xacro:macro>
</robot>

小鱼

@437985437 1.感觉是执行部分问题 2.查看下action的客户端和服务端，看看是否连接在一起

龍魂

@小鱼
补充：
rqt_graph的结果如下：

action的情况：
命令：
ros2 action info /robot_arm_controller/follow_joint_trajectory
响应：

Action: /robot_arm_controller/follow_joint_trajectory
Action clients: 1
    /moveit_simple_controller_manager
Action servers: 1
    /robot_arm_controller

龍魂

@437985437
图片有些模糊，再补充一下：

龍魂

问题解决了，是ros2 control官方提供的硬件组件“mock_components/GenericSystem”的问题。

我实现了一个新的硬件组件，仅将速度命令接口的值写入到速度状态接口，将位置命令接口的值写入为：position += velocity * period。

然后可以正常通过速度命令接口来接入到 Moveit2 中，并规划机械臂位姿。

还有疑惑的地方是，这个组件真的不支持速度命令接口吗？我没有在官网找到相关的内容。鱼大佬@小鱼对这个有研究吗？

官网文档：
Mock Components — ROS2_Control: Humble Apr 2024 documentation

小鱼

@437985437 暂时没有研究哈，蹲下一位大佬