MoveIt_Servo+ROS(melodic)+Franka_Panda机械臂-0.7.0跑通

原创已于 2025-10-06 10:42:32 修改 · 1.1k 阅读

25 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#机器人 #moveit servo #franka panda #ROS1

于 2025-10-05 15:29:40 首次发布

项目背景

最近要尝试实时控制机械臂，所以需要一个在线伺服去完成这么件事情。

环境

1.Ubuntu18.04

2.ROS-Melodic

3.libfranka内核:0.7.0 (2019年的机械臂)

主要思路

1. 给FrankaPanda换一个控制器，避免一些控制问题

2. 根据机械臂配置Moveit_servo的参数和启动文件。

3. 修改机械臂描述文件，避免自己咬自己的碰撞。

1.什么是控制器

ROS1 的“控制器”就三件东西：硬件接口、控制器插件、控制器管理器。我们先来聊聊控制器是怎么生效的?

1. 描述阶段——YAML 先“领证”

把控制器参数写进 xxx_control.yaml，这一步只是“登记信息”，还没实例化。

joint1_position_controller:
  type: effort_controllers/JointPositionController
  joint: joint1
  pid: {p: 100, i: 0.01, d: 10}

2. 加载阶段——controller_manager “插插座”

<node pkg="controller_manager" type="spawner" args="joint1_position_controller"/>

spawner 做三件事：

找到对应 .so 插件 → load
调用 init() 把句柄、参数、硬件接口指针塞给它 → construct
切到 running 状态 → start
此刻控制器线程实时循环 1 kHz，但输出力矩还是 0（安全默认）。

3. 实时阶段——update() “每毫秒跑一遍”

控制器拿到：

hardware_interface::RobotHW 里的当前 q/dq
自己的 pid_ 对象
计算 tau = pid.computeCommand(q_d - q, dt) → 写回 joint_handle.setCommand(tau)
RobotHW 再把 tau 通过 EtherCAT/串口发给伺服驱动，电机才真正出力。

4. 命令入口——rostopic “遥控器”

外部想让它动，就朝话题发：

rostopic pub /joint1_position_controller/command std_msgs/Float64 "data: 1.57"

控制器内部 command_ 变量被刷新，下一次 update() 就开始朝 1.57 rad 收敛。

2.在FrankaPanda的控制器

在 franka_ros/franka_example_controllers 包下，你可以查看到官方是怎么写控制器的，我们在 franka_ros/franka_example_controllers/config/franka_example_controllers.yaml 文件下也可以看到控制器参数.

joint_velocity_example_controller:
    type: franka_example_controllers/JointVelocityExampleController
    joint_names:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7

joint_position_example_controller:
    type: franka_example_controllers/JointPositionExampleController
    joint_names:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7

而在 franka_ros/franka_example_controllers/src/elbow_example_controller.cpp 就是控制器的源码。他将会被编译成，so供spawne使用。

给FrankaPanda换控制器(使用速度控制)

对于 franka_control来说，默认使用的是 position_joint_trajectory_controller 控制器，但是在使用该控制器的情况下，机械臂会发出异响。

原因可能是这样的：我拉取了Joint0的速度和位置曲线，当我给Joint0修改角度时，控制器貌似会给出一个瞬时的速度变化，后面我去看了一下effort(扭矩)，貌似会产生一个很大的扭矩，电机就会出现咔咔声。

也有人遇到过这个问题：

https://github.com/moveit/moveit/issues/3633

（MoveIt Servo 在真实 Franka Panda 机器人上运行时，机器人会出现抖动/震颤现象。）

我们现在来更换速度控制器：

1. 安装速度控制器

还记得吗，我们在 franka_example_controllers 的控制器是c++形式存在的，最后会被编译成.so，现在我们可以直接安装这个控制器。

sudo apt install ros-melodic-velocity-controllers

2. 注册控制器

在 /franka_ros/franka_control/config/default_controllers.yaml 下，加入velocity_joint_controller

velocity_joint_controller:
  type: velocity_controllers/JointGroupVelocityController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7

3.加载控制器

我的做法是，在 功能包/src/moveit_servo/launch/ 下新建了一个launch文件，专门拉起控制器。

<launch>
  <!-- 真实Panda：速度控制 + 夹爪一体 -->
  <node name="franka_control" pkg="franka_control" type="franka_control_node" required="true">
    <rosparam file="$(find franka_control)/config/franka_control_node.yaml" command="load"/>
    <param name="robot_ip" value="192.168.1.50"/>
    <remap from="franka_state_controller/joint_states" to="/joint_states"/>
  </node>

  <!-- 夹爪 -->
  <include file="$(find franka_gripper)/launch/franka_gripper.launch">
    <arg name="robot_ip" value="192.168.1.50"/>
  </include>

  <!-- 速度控制器 + 状态发布 -->
  <rosparam file="$(find franka_control)/config/default_controllers.yaml" command="load"/>
  <node name="ctrl_spawner" pkg="controller_manager" type="spawner"
        args="franka_state_controller velocity_joint_controller"/>

  <!-- 合并夹爪状态 -->
  <node name="gripper_js_relay" pkg="topic_tools" type="relay"
        args="/franka_gripper/joint_states /joint_states"/>
</launch>

4.启动：roslaunch moveit_servo 控制器.launch

我们就可以在命令行看来，franka_state_controller， velocity_joint_controller 两个控制器已经被加载。

franka_state_controller是机械臂状态汇报机，velocity_joint_controller 就是速度伺服。

process[franka_control-1]: started with pid [3586]
process[franka_gripper-2]: started with pid [3587]
[ INFO] [1759644369.141634243]: franka_gripper_node: Found default_speed 0.1
[ INFO] [1759644369.142336477]: franka_gripper_node: Found default_grasp_epsilon inner: 0.005, outer: 0.005
WARNING: Package name "wj_716N_lidar" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[ctrl_spawner-3]: started with pid [3608]
process[gripper_js_relay-4]: started with pid [3609]
process[joint_monitor-5]: started with pid [3610]
[INFO] [1759644369.650621]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1759644369.652650]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1759644369.654409]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1759644369.656375]: Loading controller: franka_state_controller
[INFO] [1759644369.664136]: Loading controller: velocity_joint_controller
[INFO] [1759644369.670137]: Controller Spawner: Loaded controllers: franka_state_controller, velocity_joint_controller

第一步的工作已经完成。

3.配置Moveit_Servo

请不要使用sudo apt install ros-melodic-moveit-servo预安装，请将moveit_servo源码拷贝到功能包的src目录下。

由于moveit servo是针对ur机械臂做的适配，我们可以直接修改moveit_servo/config/ur_simulated_config.yaml 文件对FrankaPanda做出适配。以下仅供参考

来源：https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/panda_simulated_config.yamlhttps://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/panda_simulated_config.yaml 配置文件主要对：command_out_topic，move_group_name，ee_frame_name，publish_joint_velocities 参数做出修改，已适配速度控制器。

###############################################
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment

## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
  # Scale parameters are only used if command_in_type=="unitless"
  linear:  0.2  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
  joint: 0.5
low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.

## Properties of outgoing commands
publish_period: 0.01  # 1/Nominal publish rate [seconds]
low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
max_expected_latency: 0.1 


# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: false
publish_joint_velocities: true
publish_joint_accelerations: false

## MoveIt properties
move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame_name: panda_hand  # The name of the end effector link, used to return the EE pose
robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector

## Stopping behaviour
incoming_command_timeout:  0.2  # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4

## Configure handling of singularities and joint limits
lower_singularity_threshold:  10.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

## Topic names
cartesian_command_in_topic: delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: joint_states
status_topic: status # Publish status to this topic

# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)
# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
command_out_topic: /velocity_joint_controller/command # Publish outgoing commands here
command_out_type: std_msgs/Float64MultiArray

## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available:
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

4. 配置Moveit_Servo的launch

我们以 cpp_interface_example.launch 为例，首先，我们删掉原本的控制器（因为在步骤2我们已经启动了控制器），然后对 launch 中机器人的描述文件做出修改。

主要就是给servo指定robot_description_semantic和robot_description

<launch>
  <!-- Launch an example that sends commands via C++ API. -->
  
  <node name="servo_server" pkg="moveit_servo" type="cpp_interface_example" output="screen" >

    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
    <rosparam ns="optional_parameter_namespace" command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />

    <param name="robot_description"
       command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>

      <param name="robot_description_semantic" textfile="$(find franka_description)/robots/panda_arm_hand.srdf"/>
      <remap from="/robot_description_semantic" to="servo_server/robot_description_semantic"/>

  </node>
</launch>

5.修改机械臂的描述文件（0.7.0）

SRDF 就是 MoveIt 的“碰撞白名单”，告诉规划器/伺服哪些连杆永远不必做碰撞检测，没有它机器人会“自己跟自己撞”而刹停。但是我们现在缺少panda_arm_hand.srdf，在这里可以找到

https://gitlab.uwaterloo.ca/vrajendr/franka_ros_custom/-/blob/0.2.2/panda_moveit_config/config/panda_arm_hand.srdf?ref_type=tagshttps://gitlab.uwaterloo.ca/vrajendr/franka_ros_custom/-/blob/0.2.2/panda_moveit_config/config/panda_arm_hand.srdf?ref_type=tags 将这个文件放到franka_description/robots/下，就可以美滋滋的启动moveit_servo的cpp_interface_example演示啦！

roslaunch moveit_servo cpp_interface_example.launch

如果你此刻启动launch，可以会遇到一下问题（针对0.7.0）

[ INFO] [1759644384.566025926]: Listening to '/collision_object'
[ INFO] [1759644384.567117317]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1759644384.569697993]: Listening to '/attached_collision_object' for attached collision objects
[ WARN] [1759644384.785700972]: An acceleration limit is not defined for this joint; minimum stop distance should not be used for collision checking
[ INFO] [1759644384.795729624]: Servo status: No warnings
[ WARN] [1759644384.805909118]: Stale command. Try a larger 'incoming_command_timeout' parameter?
[ WARN] [1759644384.908015256]: Objects in collision (printing 1st of 1 pairs): panda_hand, panda_link8
[ WARN] [1759644384.915903983]: Halting for collision!
[ INFO] [1759644384.925719619]: Servo status: Collision detected, emergency stop
[ WARN] [1759644388.795783046]: Halting for collision!

或者

[ WARN] [1759644614.729275347]: Objects in collision (printing 1st of 1 pairs): panda_link7, panda_link8

主要问题就是：SRDF 里缺一行关键配置，直接导致 MoveIt 认为 panda_link8 和 panda_hand 之间必须做碰撞检查，而它们在实际装配里其实是刚性连接（法兰-手爪），于是每次 Servo 一动就报警。

所以我们只要在 panda_arm_hand.srdf 插入两行描述即可。

    <disable_collisions link1="panda_link8" link2="panda_hand" reason="Adjacent" />
    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/>

同时打开 /franka_ros/franka_description/robots/panda_arm.xacro 文件，将 <link name="${arm_id}_link8"> 改为以下描述。（目的就是简化这个link的碰撞）。

原本的三段 <collision> 正是 0.7.0 里 “过度保守” 的罪魁祸首——它们在 panda_link8 的末端用 两个大球 + 一根粗圆柱 包了一个 L 形，结果把 panda_hand 的根部 也包进去了，于是 MoveIt 永远认为“self collision”。

    <link name="${arm_id}_link8">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.04" length="0.01"/>
        </geometry>
      </collision>
    </link>

随后就可以美美桑内的运行啦！

roslaunch moveit_servo cpp_interface_example.launch

6.题外话：Franka_ros 0.7.0

参考的帖子：

https://github.com/frankarobotics/franka_ros/issues/97 https://github.com/moveit/panda_moveit_config/pull/66 https://github.com/moveit/panda_moveit_config/pull/35

貌似在Franka Panda机械臂的发展史上，越来越多的工程师们发现建模的问题，所以在0.8.0之后，官方把 panda_link8 碰撞体改成 1 段短圆柱，同时描述文件也发生了比较大的变化。（特别是如果去看一下大家在issue做出的srdf更改，区别还是很大的）

0.10.0 URDF 里终于把 panda_hand 质量/惯性 填成实测值，Gazebo 不再“抽搐”。

0.10.1新增 self-collision matrix 自动生成脚本，SRDF 零手工。

......

但是因为很多项目和环境都基于Franka 0.7.0的内核展开，所以也没办法升级环境。他也许就静静的呆在那，等待下一个青年工程师的控制和成长。