franka_ros

注解

Windows 不支持 franka_ros。

在继续本章之前，请先 安装或编译 franka_ros。

_images/ros-architecture.png

franka_ros 示意图概述

franka_ros 元包将 libfranka 集成到 ROS 和 ROS control中。在这里，我们不仅介绍了它的包，我们还给出了一个简短的操作方法，如何 编写自己的控制器。

本节中传递给 launch 文件的所有参数都带有默认值，因此如果使用默认网络地址和 ROS 命名空间，则可以省略它们。确保使用来自工作区的设置脚本调用 source 命令：

source /path/to/catkin_ws/devel/setup.sh

franka_description

该包包含运动学、关节限制、视觉表面和碰撞空间对我们的机器人和末端执行器的描述。碰撞空间是视觉描述的一个简化版本，用于提高碰撞检查的性能。该描述是基于按照 URDF XML 文档 的URDF格式。

如果想仿真 Panda 机器人，可以传递一个 gazebo 参数给 XACRO 文件。然后 URDF 将包含 估算的 惯量值，即基于 使用基于惩罚优化的可行参数检索的 Franka Emika Panda 机器人的动态辩识 (Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using Penalty-Based Optimization) 得到的连杆质量和惯性张量。

xacro $(rospack find franka_description)/robots/panda/panda.urdf.xacro gazebo:=true

同样适用于 FR3：

xacro $(rospack find franka_description)/robots/fr3/fr3.urdf.xacro gazebo:=true

碰撞体积

URDF 定义了两种碰撞类型：

  • Fine（精细）：这些碰撞体积由凸面网格构成，这些凸面网格是每个连杆的视觉网格 (.dae) 的近似和大幅简化的版本。精细体积应用于仿真 Gazebo 中的机器人碰撞。

  • Coarse（粗糙）: 这些碰撞体积是简单的胶囊体（一个有两个半球形端盖的圆柱体），连接到每个连杆，并按一定的安全距离膨胀。这些体积的计算效率更高，并在机器人内部使用以避免自碰撞。当规划运动时，例如使用 MoveIt 时，请使用这些几何图形。

可视化模型

碰撞模型（精细）

碰撞模型（粗糙）
_images/visual.png _images/collision-fine.png _images/collision-coarse.png

为了区分以上两种类型的碰撞模型，在 URDF 中插入了带有 *_sc 后缀的人为添加的连杆（用于自碰撞）：

panda_link0
├─ panda_link0_sc
└─ panda_link1
├─ panda_link1_sc
└─ panda_link2
├─ panda_link2_sc
└─ panda_link3
├─ panda_link3_sc
└─ panda_link4
├─ panda_link4_sc
└─ panda_link5
├─ panda_link5_sc
└─ panda_link6
├─ panda_link6_sc
└─ panda_link7
├─ panda_link7_sc
└─ panda_link8
├─ panda_link8_sc
└─ panda_hand
├─ panda_leftfinger
├─ panda_rightfinger
├─ panda_hand_sc
└─ panda_hand_tcp

可以通过 gazebo XACRO 参数控制哪个碰撞模型加载到 URDF 中:

  • xacro ... panda.urdf.xacro gazebo:=false: 这将 同时 使用精细和粗略碰撞模型。如果完全省略 arg 参数，这也是默认设置。如果要使用 MoveIt，请使用此选项。

  • xacro ... panda.urdf.xacro gazebo:=true: 这将 仅 使用精细碰撞模型。当想要一个可仿真的 URDF（即用于 Gazebo）时使用它。当使用粗略碰撞模型时，机器人当然会不断地与下一个连杆的胶囊体发生碰撞。

franka_gripper

这个包实现了 franka_gripper_node 节点，用于来自 ROS 的夹爪进行交互。该节点发布夹爪的状态并提供以下 actions servers：

  • franka_gripper::MoveAction(width, speed)：以定义的 speed 移动到目标 width。

  • franka_gripper::GraspAction(width, epsilon_inner, epsilon_outer, speed, force)：尝试在以给定 speed 闭合的同时以所需的 force 在所需的 width 处抓取。如果夹爪指间的距离 \(d\) 为以下范围之内时： \(\text{width} -\epsilon_\text{inner} < d < \text{width} + \epsilon_\text{outer}\)，则该操作是成功的。

  • franka_gripper::HomingAction(): 将夹爪归零并根据已安装的手指更新最大宽度。

  • franka_gripper::StopAction(): 中止正在运行的 action。这可以是用于在抓取后停止施加力。

  • control_msgs::GripperCommandAction(width, max_effort): MoveIt! 识别的一个标准夹爪 action。如果参数 max_effort 大于零，则夹爪将尝试抓取所期望 width 处的对象。另一方面，如果 max_effort 等于零 (\(\text{max_effort} < 1^{-4}\))，夹爪将尝试移动到所期望的 width 处。

注解

只在抓取对象时使用参数 max_effort，否则，夹爪将关闭并忽略 width 参数。

可以启动 franka_gripper_node 节点通过：

roslaunch franka_gripper franka_gripper.launch robot_ip:=<fci-ip>

提示

从 franka_ros 0.6.0 开始，为 roslaunch franka_control franka_control.launch 指定 load_gripper:=true,也将启动 franka_gripper_node 节点。

franka_hw

该包包含基于 libfranka API 的 ROS control 控制框架的机器人硬件抽象。硬件类 franka_hw::FrankaHW 在这个包中实现，为控制器提供以下接口：

接口

功能

hardware_interface::JointStateInterface

读取关节状态

hardware_interface::VelocityJointInterface

给关节速度指令并读取关节状态。

hardware_interface::PositionJointInterface

给关节角度指令并读取关节状态。

hardware_interface::EffortJointInterface

给关节级扭矩指令并读取关节状态。

franka_hw::FrankaStateInterface

读取整个机器人状态

franka_hw::FrankaPoseCartesianInterface

给笛卡尔位姿指令并读取整个机器人状态

franka_hw::FrankaVelocityCartesianInterface

给笛卡尔速度指令并读取整个机器人状态

franka_hw::FrankaModelInterface

读取机器人的动力学和运动学模型

要使用 ROS control 控制接口，必须通过名称检索资源句柄：

接口

资源句柄名称

hardware_interface::JointStateInterface

“<arm_id>_joint1” 到 “<arm_id>_joint7”

hardware_interface::VelocityJointInterface

“<arm_id>_joint1” 到 “<arm_id>_joint7”

hardware_interface::PositionJointInterface

“<arm_id>_joint1” 到 “<arm_id>_joint7”

hardware_interface::EffortJointInterface

“<arm_id>_joint1” 到 “<arm_id>_joint7”

franka_hw::FrankaStateInterface

“<arm_id>_robot”

franka_hw::FrankaPoseCartesianInterface

“<arm_id>_robot”

franka_hw::FrankaVelocityCartesianInterface

“<arm_id>_robot”

franka_hw::FrankaModelInterface

“<arm_id>_robot”

提示

默认情况下, <arm_id> 被设置为 “panda”.

franka_hw::FrankaHW 类还实现了控制器的启动、停止和切换。

FrankaHW 类还用作 FrankaCombinableHW 的基类，FrankaCombinableHW 是一个硬件类，可以与其他类结合以从单个控制器控制多个机器人。由任意数量的，基于为 ROS control 框架 https://github.com/ros-controls/ros_control 设计的 FrankaCombinableHW 类的 Panda 机器人的组合（数量由参数配置），在 FrankaCombinedHW 类中得到实现。 FrankaHW 和 FrankaCombinedHW 之间的主要区别在于后者仅支持扭矩控制。

重要

FrankaCombinableHW 类从 0.7.0 版开始提供，仅允许扭矩控制。

ROS 参数服务器用于在运行时确定在组合类中加载了哪些机器人。有关如何在相应硬件节点中配置 FrankaCombinedHW 的示例，请参阅 franka_control。

注解

FrankaHW 的方法最适合控制单个机器人。因此，我们建议仅将 FrankaCombinableHW/FrankaCombinedHW 类用于控制多个机器人。

FrankaCombinableHW/FrankaCombinedHW 类提供的接口如下：

接口

功能

hardware_interface::EffortJointInterface

给关节级扭矩指令并读取关节状态。

hardware_interface::JointStateInterface

读取关节状态

franka_hw::FrankaStateInterface

读取整个机器人状态

franka_hw::FrankaModelInterface

读取机器人的动力学和运动学模型

唯一可接受的命令接口声明是 EffortJointInterface，它可以与任何一组只读接口（FrankaModelInterface、JointStateInterface、FrankaStateInterface）结合使用。所有接口提供的资源句柄均按名称声明，并遵循与 FrankaHW 所述相同的命名约定。FrankaCombinableHW 的每个实例都提供完整的 service 和 action 接口集（请参阅 franka_control ）。

注解

FrankaCombinedHW 类在控制节点命名空间中提供了一个额外的动作服务器来恢复所有机器人。如果任何一个机器人发生反射 reflex 或错误 error，所有机器人的控制回路都会停止，直到它们恢复。

重要

FrankaHW 利用 ROS joint_limits_interface 来 执行位置、速度和力度的安全限制。下面列出了使用的接口：

  • joint_limits_interface::PositionJointSoftLimitsInterface

  • joint_limits_interface::VelocityJointSoftLimitsInterface

  • joint_limits_interface::EffortJointSoftLimitsInterface

接近极限将导致（未经宣布的）修改命令。

franka_control

ROS 节点 franka_control_node 和 franka_combined_control_node 是用于 ROS control 的硬件节点，它们使用 franka_hw 中的相应硬件类。他们提供了各种 ROS 服务，以在 ROS 生态系统中公开完整的 libfranka API。它提供以下服务：

  • franka_msgs::SetJointImpedance 指定内部控制器的关节刚度（阻尼自动从刚度导出）。

  • franka_msgs::SetCartesianImpedance 指定内部控制器的笛卡尔刚度（阻尼自动从刚度导出）。

  • franka_msgs::SetEEFrame 指定从 <arm_id>_EE（末端执行器）到 <arm_id>_NE（标称末端执行器）坐标系的变换。从法兰到末端执行器坐标系的变换分为两个变换：<arm_id>_EE 到 <arm_id>_NE 坐标系的变换和 <arm_id>_NE 到 <arm_id>_link8 坐标系的变换。 <arm_id>_NE 到 <arm_id>_link8 坐标系的变换只能通过管理员界面设置

  • franka_msgs::SetKFrame 指定从 <arm_id>_K 到 <arm_id>_EE 坐标系的变换。

  • franka_msgs::SetForceTorqueCollisionBehavior 为外部笛卡尔力旋量设置阈值以配置碰撞反射。

  • franka_msgs::SetFullCollisionBehavior 在笛卡尔和关节层级上设置外力阈值以配置碰撞反射。

  • franka_msgs::SetLoad 设置外部负载以进行补偿（例如一个抓取的物体）。

  • std_srvs::Trigger 服务允许连接和断开硬件节点（从 0.8.0 开始可用）。当没有活动（命令）控制器在运行时，可以断开硬件节点，为非 fci 应用程序释放相应的机器人，例如基于 Desk 桌面的操作。一旦想恢复 fci 操作，可以调用 connect 并再次启动基于 ros_control 的控制器

重要

<arm_id>_EE 坐标系表示可配置的末端执行器坐标系的一部分，可以在运行时通过 franka_ros 进行调整。<arm_id>_K 坐标系标记内部笛卡尔阻抗的中心。它还可以作为外部力旋量的参考坐标系。<arm_id>_EE 和 <arm_id>_K 都不包含在 URDF 中，因为它们可以在运行时更改。 默认情况下，<arm_id> 设置为 “panda”。

_images/frames.svg

末端执行器坐标系概览。

要在机器人处于反射模式时从错误和反射中恢复，可以使用 Franka_msgs::ErrorRecoveryAction。这可以通过 action 客户端或通过在 action 目标主题上发布来实现。

rostopic pub -1 /franka_control/error_recovery/goal franka_msgs/ErrorRecoveryActionGoal "{}"

恢复后，franka_control_node 重新启动正在运行的控制器。这是可能的，因为当触发机器人反射或发生错误时，节点不会死亡。所有这些功能都由 franka_control_node 提供，其可以使用以下命令启动：

roslaunch franka_control franka_control.launch \
robot_ip:=<fci-ip> # mandatory \
load_gripper:=<true|false> # default: true \
robot:=<panda|fr3> # default: panda

除了加载 franka_control_node 之外，launch 文件还会启动 franka_control::FrankaStateController 来读取和发布机器人状态，包括外部力旋量、可配置的变换和使用 rivz 进行可视化所需的关节状态。出于可视化目的，一个 robot_state_publisher 节点已启动。

这个包还实现了 franka_combined_control_node，一个基于 franka_hw::FrankaCombinedHW 类的 ros_control 的硬件节点。加载的机器人组是通过 ROS 参数服务器配置的。这些参数必须在硬件节点的命名空间中（参见 franka_combined_control_node.yaml 作为参考），如下所示：

robot_hardware:
  - panda_1
  - panda_2
  # (...)

panda_1:
  type: franka_hw/FrankaCombinableHW
  arm_id: panda_1
  joint_names:
    - panda_1_joint1
    - panda_1_joint2
    - panda_1_joint3
    - panda_1_joint4
    - panda_1_joint5
    - panda_1_joint6
    - panda_1_joint7
  # Configure the threshold angle for printing joint limit warnings.
  joint_limit_warning_threshold: 0.1 # [rad]
  # Activate rate limiter? [true|false]
  rate_limiting: true
  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
  cutoff_frequency: 1000
  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
  internal_controller: joint_impedance
  # Configure the initial defaults for the collision behavior reflexes.
  collision_config:
    lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]

panda_2:
  type: franka_hw/FrankaCombinableHW
  arm_id: panda_2
  joint_names:
    - panda_2_joint1
    - panda_2_joint2
    - panda_2_joint3
    - panda_2_joint4
    - panda_2_joint5
    - panda_2_joint6
    - panda_2_joint7
  # Configure the threshold angle for printing joint limit warnings.
  joint_limit_warning_threshold: 0.1 # [rad]
  # Activate rate limiter? [true|false]
  rate_limiting: true
  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
  cutoff_frequency: 1000
  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
  internal_controller: joint_impedance
  # Configure the initial defaults for the collision behavior reflexes.
  collision_config:
    lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
    lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
    upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]

# (+ more robots ...)

注解

请务必选择唯一且一致的 arm_id 参数。 ID 必须与关节名称中的前缀匹配，并且应该根据加载到控制节点命名空间的机器人描述。

关于基于参数加载硬件类的更多信息，请参考 https://github.com/ros-controls/ros_control 中 combined_robot_hw::CombinedRobotHW 的官方文档。

第二个重要的参数文件（参见 franka_ros/franka_control/config/default_combined_controllers.yaml作为参考）配置了一组默认控制器，可以通过硬件节点启动。控制器必须与启动的硬件相匹配。提供的默认参数化（此处为 2 个机器人）如下所示：

panda_1_state_controller:
  type: franka_control/FrankaStateController
  arm_id: panda_1
  joint_names:
    - panda_1_joint1
    - panda_1_joint2
    - panda_1_joint3
    - panda_1_joint4
    - panda_1_joint5
    - panda_1_joint6
    - panda_1_joint7
  publish_rate: 30  # [Hz]

panda_2_state_controller:
  type: franka_control/FrankaStateController
  arm_id: panda_2
  joint_names:
    - panda_2_joint1
    - panda_2_joint2
    - panda_2_joint3
    - panda_2_joint4
    - panda_2_joint5
    - panda_2_joint6
    - panda_2_joint7
  publish_rate: 30  # [Hz]

# (+ more controllers ...)

我们提供了一个 launch 启动文件来运行 franka_combined_control_node，其中包含用户指定的硬件和控制器配置文件，默认配置为 2 个机器人。用以下命令启动它：

roslaunch franka_control franka_combined_control.launch \
    robot_ips:=<your_robot_ips_as_a_map>                 # mandatory
    robot:=<path_to_your_robot_description> \
    args:=<xacro_args_passed_to_the_robot_description> \ # if needed
    robot_id:=<name_of_your_multi_robot_setup> \
    hw_config_file:=<path_to_your_hw_config_file>\       # includes the robot ips!
    controllers_file:=<path_to_your_default_controller_parameterization>\
    controllers_to_start:=<list_of_default_controllers_to_start>\
    joint_states_source_list:=<list_of_sources_to_fuse_a_complete_joint_states_topic>

这个 launch 启动文件可以被参数化以运行任意数量的机器人。为此，只需以 franka_control/config/franka_combined_control_node.yaml 和 franka_ros/franka_control/config/default_combined_controllers.yaml 的样式编写自己的配置文件。

重要

请务必将机器人的正确 IP 作为映射传递给 franka_combined_control.launch。这看起来像：{<arm_id_1>/robot_ip: <my_ip_1>, <arm_id_2>/robot_ip: <my_ip_2>, …}

franka_visualization

该包包含连接到机器人并发布机器人和夹爪关节状态以在 RViz 中进行可视化的发布者。要运行此包启动：

roslaunch franka_visualization franka_visualization.launch robot_ip:=<fci-ip> \
  load_gripper:=<true|false> robot:=<panda|fr3>

这纯粹是为了可视化 - 没有向机器人发送命令。它对检查与机器人的连接情况很有用。

重要

只有一个 franka::Robot 实例可以连接到机器人。这意味着，例如 franka_joint_state_publisher 不能与 franka_control_node 并行运行。这也意味着不能在运行控制器的单独程序时同时执行可视化示例。

franka_example_controllers

在这个包中，实现了一组用于通过 ROS 控制机器人的示例控制器。控制器描述了 franka_hw::FrankaHW 类提供的各种接口以及相应的用法。每个示例都带有一个独立的启动文件，用于启动机器人上的控制器并对其进行可视化。

要启动关节阻抗示例，请执行以下命令：

roslaunch franka_example_controllers joint_impedance_example_controller.launch \
  robot_ip:=<fci-ip> load_gripper:=<true|false> robot:=<panda|fr3>

其他单个 Panda 示例以相同方式启动。

dual_arm_cartesian_impedance_example_controller 展示了对两个基于 FrankaCombinedHW 类的 Panda 机器人的控制，使用一个实时控制器通过基于阻抗的控制方法完成笛卡尔任务。示例控制器能以以下命令启动

roslaunch franka_example_controllers \
    dual_arm_cartesian_impedance_example_controller.launch \
    robot_id:=<name_of_the_2_arm_setup> \
    robot_ips:=<your_robot_ips_as_a_map> \
    rviz:=<true/false> rqt:=<true/false>

该示例假设机器人配置符合 dual_panda_example.urdf.xacro，其中两个 Panda 以 1 米的距离安装在一个盒子顶部。随意用与实际设置相匹配的描述替换此机器人描述。 rviz 选项允许选择是否应启动可视化。使用 rqt，用户可以选择启动 rqt-gui，它允许在运行时通过动态参数重新调整来在线适配渲染的末端效应器阻抗。

franka_gazebo

该包允许在 Gazebo 中仿真我们的机器人。这是可能的，因为 Gazebo 能够通过 gazebo_ros 包集成到 ROS control 框架中。

重要

该包从 0.8.0 开始提供

拾放示例

让我们深入研究并仿真将石头从 A 运输到 B。运行以下命令以启动伴有 Panda 和示例 world 的 Gazebo。

roslaunch franka_gazebo panda.launch x:=-0.5 \
    world:=$(rospack find franka_gazebo)/world/stone.sdf \
    controller:=cartesian_impedance_example_controller \
    rviz:=true

这将打开 Gazebo GUI，可以在其中看到带有石头和 RViz 的环境，可以用它来控制机器人的末端执行器位姿。

_images/franka-gazebo-example.png

取放示例的 Gazebo GUI（左）和 RViz（右）

要打开夹爪，只需向 move action 发送一个目标，类似于真正的 franka_gripper 的工作方式。让我们将夹爪以 \(10\:\frac{cm}{s}\) 移动到手指之间的宽度为 \(8\:cm\):

rostopic pub --once /franka_gripper/move/goal franka_gripper/MoveActionGoal "goal: { width: 0.08, speed: 0.1 }"

由于我们使用 franka_example_controllers 的笛卡尔阻抗控制器启动了我们的机器人，因此可以使用 RViz 中的交互式标记 Gizmo 移动末端执行器，就像在现实中一样。移动机器人，使白色石头位于夹爪的手指之间，准备好被捡起。

注解

如果机器人的肘部移动很奇怪，这是因为笛卡尔阻抗示例控制器的默认零空间刚度设置为低。如有必要，启动 Dynamic Reconfigure 并调整 panda > cartesian_impedance_example_controller > nullspace_stiffness。

为了拾取物体，这次我们使用 grasp action，因为我们想在抓握后施加一个力，使物体不掉落。石头宽约 \(3\:cm\)，重 \(50\:g\) 。让我们用 \(5\:N\) 来抓取它：

rostopic pub --once /franka_gripper/grasp/goal \
             franka_gripper/GraspActionGoal \
             "goal: { width: 0.03, epsilon:{ inner: 0.005, outer: 0.005 }, speed: 0.1, force: 5.0}"

注解

在 Gazebo 的顶部菜单中，转到 View > Contacts 以可视化接触点和力

如果抓取成功，手指现在将把石头固定到位。如果不是，则可能是目标公差（内部和外部 epsilon）太小并且 action 失败。现在将物体轻轻地移到红色的落点区域。

_images/franka-gazebo-example-grasp.png

把石头从蓝色运到红色区域

将其轻轻放在红色垫子上后，通过夹爪的 stop action 停止抓握：

rostopic pub --once /franka_gripper/stop/goal franka_gripper/StopActionGoal {}

请注意，接触力现在消失了，因为不再施加力了。或者，也可以使用 move action。

定制化

franka_gazebo 的启动文件包含许多参数，可以使用这些参数自定义仿真的行为。例如，要在一次仿真中生成两只 panda 臂，可以使用以下命令：

<?xml version="1.0"?>
<launch>

  <include file="$(find gazebo_ros)/launch/empty_world.launch" >
    <!-- Start paused, simulation will be started, when Pandas were loaded -->
    <arg name="paused" value="true"/>
    <arg name="use_sim_time" value="true"/>
  </include>

  <group ns="panda_1">
    <include file="$(find franka_gazebo)/launch/panda.launch">
      <arg name="arm_id"     value="panda_1" />
      <arg name="y"          value="-0.5" />
      <arg name="controller" value="cartesian_impedance_example_controller" />
      <arg name="rviz"       value="false" />
      <arg name="gazebo"     value="false" />
      <arg name="paused"     value="true" />
    </include>
  </group>

  <group ns="panda_2">
    <include file="$(find franka_gazebo)/launch/panda.launch">
      <arg name="arm_id"     value="panda_2" />
      <arg name="y"          value="0.5" />
      <arg name="controller" value="force_example_controller" />
      <arg name="rviz"       value="false" />
      <arg name="gazebo"     value="false" />
      <arg name="paused"     value="false" />
    </include>
  </group>

</launch>

注解

要查看支持哪些参数，请使用： roslaunch franka_gazebo panda.launch --ros-args

FrankaHWSim

默认情况下，Gazebo ROS 只能仿真具有 “标准” 硬件接口的关节，如关节状态接口 Joint State Interfaces 和关节命令接口 Joint Command Interfaces。然而，我们的机器人与这种架构完全不同！除了这些特定于关节的接口之外，它还支持 特定于机器人 的接口，如 FrankaModelInterface （参见 franka_hw）。自然 gazebo 默认不理解这些自定义硬件接口。这就是 FrankaHWSim 插件的作用。

为了让机器人能够支持 Franka 接口，只需在 URDF 中声明一个自定义的 robotsSimType：

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>${arm_id}</robotNamespace>
    <controlPeriod>0.001</controlPeriod>
    <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
  </plugin>
  <self_collide>true</self_collide>
</gazebo>

当使用 模型生成器 生成此机器人时，此插件将加载到 gazebo 节点中。它将扫描 URDF 并尝试查找支持的硬件接口。目前只支持 franka_hw 提供的部分接口：

接口

功能

✔

hardware_interface::JointStateInterface

读取关节状态

✔

hardware_interface::EffortJointInterface

给关节级扭矩指令并读取关节状态。

✔

hardware_interface::VelocityJointInterface

给关节速度指令并读取关节状态。

✔

hardware_interface::PositionJointInterface

给关节角度指令并读取关节状态。

✔

franka_hw::FrankaStateInterface

读取整个机器人状态

✔

franka_hw::FrankaModelInterface

读取机器人的动力学和运动学模型

✘

franka_hw::FrankaPoseCartesianInterface

给笛卡尔位姿指令并读取整个机器人状态

✘

franka_hw::FrankaVelocityCartesianInterface

给笛卡尔速度指令并读取整个机器人状态

重要

这意味着只能仿真那些要求这些支持的接口的控制器，而不是其他的！例如，笛卡尔阻抗示例控制器可以被仿真，因为它只需要 EffortJoint-、FrankaState- 和 FrankaModelInterface。然而，关节阻抗示例控制器不能被仿真，因为它需要尚不支持的 FrankaPoseCartesianInterface。

除了实时硬件接口外，FrankaHWSim 插件支持 franka_control 支持的一些非实时命令：

服务 / 类型

解释

✘

set_joint_impedance / SetJointImpedance

Gazebo 不仿真内部阻抗控制器，而是直接设置扭矩命令

✘

set_cartesian_impedance / SetCartesianImpedance

Gazebo 不仿真内部阻抗控制器，而是直接设置扭矩命令

✔

set_EE_frame / SetEEFrame

设置 \({}^{\mathrm{NE}}\mathbf{T}_{\mathrm{EE}}\)，即从标称末端效应器到末端效应器的齐次变换。也可以通过设置 ROS 参数 /<arm_id>/NE_T_EE 来初始化它。通常会在 Desk 中设置 \({}^{\mathrm{F}}\mathbf{T}_{\mathrm{NE}}\)，但在 franka_gazebo 中，如果没有指定夹爪或定义围绕 Z 旋转 \(45\:°\) 和偏移 \(10.34\:cm\) （与抓手的 Desk 所示相同），则假定为与 Desk 中相同。始终可以通过手动设置 ROS 参数 /<arm_id>/NE_T_EE 来覆盖此值。

✔

set_K_frame / SetKFrame

设置 \({}^{\mathrm{EE}}\mathbf{T}_{\mathrm{K}}\)，即从末端执行器到刚度坐标系的齐次变换。

✔

set_force_torque_collision_behavior / SetForceTorqueCollisionBehavior

设置阈值，高于该阈值外部力旋量被视为接触和碰撞。

✘

set_full_collision_behavior / SetFullCollisionBehavior

尚未实现

✔

set_load / SetLoad

设置外部负载以补偿其重力，例如抓取的物体。还可以通过分别设置对应质量、惯性张量和负载质心的 ROS 参数 /<arm_id>/{m_load,I_load,F_x_load} 来初始化它。

✔

set_user_stop / std_srvs::SetBool (since 0.9.1)

这是一项仅在 franka_gazebo 中可用的特殊服务，用于仿真用户停止。按下用户停止（也就是通过此服务发布一个 true）将断开 disconnect 来自 ROS 控制器的馈送到关节的所有命令信号。要再次连接它们，请调用 error_recovery action。

FrankaGripperSim

这个插件仿真 Gazebo 中的 franka_gripper_node。这是为两个手指关节做的 ROS 控制器，带有位置和力控制器。它提供了与真正的夹爪节点相同的五个动作 action：

  • /<arm_id>/franka_gripper/homing

  • /<arm_id>/franka_gripper/stop

  • /<arm_id>/franka_gripper/move

  • /<arm_id>/franka_gripper/grasp

  • /<arm_id>/franka_gripper/gripper_action

重要

grasp action 有一个bug，如果让手指 open 到目标宽度，它既不会成功也不会中止。这是因为缺少关节限制接口，这让手指在其极限处摆动。现在只使用 grasp action 来 close 手指！

假设 URDF 包含两个可以力控制的手指关节，即有一个相应的 EffortJointInterface 传动声明。该控制器在其命名空间中需要以下参数：

  • type (string, required):应该是 franka_gazebo/FrankaGripperSim

  • arm_id (string, required):panda 的手臂 id，以推断出手指关节的名称

  • finger1/gains/p (double, required): 第一指位置-力控制器的比例增益

  • finger1/gains/i (double, default: 0): 第一指位置-力控制器的积分增益

  • finger1/gains/d (double, default: 0):第一指位置-力控制器的微分增益

  • finger2/gains/p (double, required): 第二指位置-力控制器的比例增益

  • finger2/gains/i (double, default: 0): 第二指位置-力控制器的积分增益

  • finger2/gains/d (double, default: 0): 第二指位置-力控制器的微分增益

  • move/width_tolerance (double, default \(5\:mm\)): 当手指宽度低于此阈值时，move action 成功

  • grasp/resting_threshold (double, default \(1\:\frac{mm}{s}\)): 低于该速度应检查目标宽度以中止或成功 grasp action

  • grasp/consecutive_samples (double, default: 3): 速度要连续低于阈值 resting_threshold 多少次才会被评估为抓取

  • gripper_action/width_tolerance (double, default \(5\:mm\)): 当手指宽度低于此阈值时，夹爪 action 就（被认为）成功

  • gripper_action/speed (double, default \(10\:\frac{cm}{s}\)): 在夹爪 action 中使用的速度

JointStateInterface

为了能够从 ROS 控制器访问关节状态接口，只需在 URDF 的任何传动标签中声明相应的关节。然后一个关节状态接口将自动可用。通常，为 EffortJointInterface 等命令接口声明传动标签。

注解

对于任何名为 <arm_id>_jointN （N 为整数）的关节，FrankaHWSim 将自动补偿其重力以模仿 libfranka 的行为。

EffortJointInterface

为了能够从控制器向一个关节发送扭矩命令，只需在 URDF 中为关节声明一个具有相应硬件接口类型的传动标签。

<transmission name="${joint}_transmission">
 <type>transmission_interface/SimpleTransmission</type>
 <joint name="${joint}">
   <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
 </joint>
 <actuator name="${joint}_motor">
   <hardwareInterface>${transmission}</hardwareInterface>
 </actuator>
</transmission>

<gazebo reference="${joint}">
  <!-- Needed for ODE to output external wrenches on joints -->
  <provideFeedback>true</provideFeedback>
</gazebo>

注解

如果希望能够读取外力或扭矩，例如从碰撞中，确保将 <provideFeedback> 标记设置为 true。

FrankaStateInterface

这是一个 机器人专用 接口，因此与普通硬件接口略有不同。为了能够从控制器访问 franka 状态接口，请在 URDF 中声明以下带有所有七个关节的传动标签：

<transmission name="${arm_id}_franka_state">
  <type>franka_hw/FrankaStateInterface</type>
  <joint name="${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
  <joint name="${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>

  <actuator name="${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
  <actuator name="${arm_id}_motor7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
</transmission>

当控制器通过 FrankaStateInterface 访问 机器人状态 robot state 时，它可以期望仿真出以下值：

区域

注释

✔

O_T_EE

✘

O_T_EE_d

尚不支持运动生成，字段将仅包含零

✔

F_T_EE

可以通过参数 F_T_NE、NE_T_EE 和/或对 set_EE_frame 的服务调用进行配置

✔

NE_T_EE

可以通过参数 EE_T_EE 和/或对 setEE_frame 的服务调用进行配置

✔

EE_T_K

可以通过参数 EE_T_K 和/或对 set_K_frame 的服务调用进行配置

✔

m_ee

如果可以找到抓手，将可以从 URDF 惯性标签中的质量来设置，否则为零。可以被参数 m_ee 覆盖

✔

I_ee

如果可以找到抓手，将可以从 URDF 惯性标签中的惯量来设置，否则为零。可以被参数 I_ee 覆盖

✔

F_x_Cee

如果可以找到抓手，将可以从 URDF 的惯性标签中的原点来设置，否则为零。可以被参数 F_x_Cee 覆盖

✔

m_load

可以通过参数 m_load 和/或对 set_load 的服务调用进行配置

✔

I_load

可以通过参数 I_load 和/或对 set_load 的服务调用进行配置

✔

F_x_Cload

可以通过参数 F_x_Cload 和/或对 set_load 的服务调用进行配置

✔

m_total

✔

I_total

✔

F_x_Ctotal

✘

elbow

✘

elbow_d

✘

elbow_c

✘

delbow_d

✘

delbow_c

✔

tau_J

直接来自 Gazebo

✔

tau_J_d

扭矩控制器发送的值，否则为0

✔

dtau_J

tau_J 的数值导数

✔

q

直接来自 Gazebo

✔

q_d

使用位置接口时的最后一个命令关节位置。当使用速度接口时与 q 相同。但是，使用扭矩接口时，该值不会更新。

✔

dq

直接来自 Gazebo

✔

dq_d

使用速度接口时的最后命令关节速度。当使用位置接口时与 dq 相同。但是，当使用扭矩接口时，该值将为零。

✔

ddq_d

使用位置或速度接口时的当前加速度。但是，当使用扭矩接口时，该值将为零。

✔

joint_contact

\(\mid \hat{\tau}_{ext} \mid > \mathrm{thres}_{lower}\) 其中可以通过调用 set_force_torque_collision_behavior 设置阈值

✔

joint_collision

\(\mid \hat{\tau}_{ext} \mid > \mathrm{thres}_{upper}\) 其中可以通过调用 set_force_torque_collision_behavior 设置阈值

✔

cartesian_contact

\(\mid {}^K \hat{F}_{K,ext} \mid > \mathrm{thres}_{lower}\) 其中可以通过调用 set_force_torque_collision_behavior 来设置阈值

✔

cartesian_collision

\(\mid {}^K \hat{F}_{K,ext} \mid > \mathrm{thres}_{upper}\) 其中可以通过调用 set_force_torque_collision_behavior 来设置阈值

✔

tau_ext_hat_filtered

\(\hat{\tau}_{ext}\) 即估计的末端执行器的外部扭矩和力，用指数移动平均滤波器 (EMA) 过滤。其过滤的 \(\alpha\) 可以通过 ROS 参数进行配置。该字段不包含任何重力项，即 \(\tau_{ext} = \tau_J - \tau_{J_d} - \tau_{gravity}\)

✔

O_F_ext_hat_K

\({}^O\hat{F}_{K,ext} = J_O^{\top +} \cdot \hat{\tau}_{ext}\)

✔

K_F_ext_hat_K

\({}^K\hat{F}_{K,ext} = J_K^{\top +} \cdot \hat{\tau}_{ext}\)

✘

O_dP_EE_d

✔

ddq_d

将与 gravity_vector ROS 参数相同。默认为 {0,0,-9.8}

✘

O_T_EE_c

✘

O_dP_EE_c

✘

O_ddP_EE_c

✔

theta

与 q 相同，因为我们不在 Gazebo 中仿真软关节

✔

dtheta

与 dq 相同，因为我们不在 Gazebo 中仿真软关节

✘

current_errors

完全是假的，反射系统尚未实现

✘

last_motion_errors

完全是假的，反射系统尚未实现

✔

control_command_success_rate

总是 1.0

✘

robot_mode

机器人模式切换和反射系统尚未实现

✔

time

仿真中的当前 ROS 时间，来自 Gazebo

FrankaModelInterface

这是一个机器人专用 robot-specific 接口，因此与普通硬件接口略有不同。为了能够从控制器访问 franka 模型接口，请在 URDF 中声明以下带有传动链的根部（例如 panda_joint1）和尖端（例如 panda_joint8）的传动标签：

<transmission name="${arm_id}_franka_model">
  <type>franka_hw/FrankaModelInterface</type>
  <joint name="${root}">
    <role>root</role>
    <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
  </joint>
  <joint name="${tip}">
    <role>tip</role>
    <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
  </joint>

  <actuator name="${root}_motor_root"><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
  <actuator name="${tip}_motor_tip"  ><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
</transmission>

模型函数本身是用 KDL 实现的。这从 URDF 中获取运动学结构和惯性属性来计算模型属性，如雅可比矩阵或质量矩阵。

摩擦

为了使对象（例如手指和对象）之间具有适当的摩擦力，需要在 URDF 中调整一些 Gazebo 参数。对于关节 panda_finger_joint1 和 panda_finger_joint2 我们建议设置以下参数：

<gazebo reference="${link}">
  <collision>
    <max_contacts>10</max_contacts>
    <surface>
      <contact>
        <ode>
          <!-- These two parameter need application specific tuning. -->
          <!-- Usually you want no "snap out" velocity and a generous -->
          <!-- penetration depth to keep the grasping stable -->
          <max_vel>0</max_vel>
          <min_depth>0.003</min_depth>
        </ode>
      </contact>
      <friction>
        <ode>
          <!-- Rubber/Rubber contact -->
          <mu>1.16</mu>
          <mu2>1.16</mu2>
        </ode>
      </friction>
      <bounce/>
    </surface>
  </collision>
</gazebo>

注解

请参阅 Gazebo Friction 文档

franka_msgs

该包包含消息、服务和动作类型，主要用于包 franka_hw 和 franka_control 来发布机器人状态或在 ROS 生态系统中公开 libfranka API。有关此包中提供的服务和动作的更多信息，请参阅 franka_control。

panda_moveit_config

注解

此包已移至存储库 ros_planning。

有关更多详细信息、文档和教程，请查看 MoveIt！教程网站。

编写自己的控制器

示例控制器包 中的所有示例控制器都派生自 controller_interface::MultiInterfaceController 类，该类允许在一个控制器实例中声明多达四个接口。类的声明看起来如下所示：

class NameOfYourControllerClass : controller_interface::MultiInterfaceController <
                              my_mandatory_first_interface,
                              my_possible_second_interface,
                              my_possible_third_interface,
                              my_possible_fourth_interface> {
  bool init (hardware_interface::RobotHW* hw, ros::NodeHandle& nh);  // mandatory
  void update (const ros::Time& time, const ros::Duration& period);  // mandatory
  void starting (const ros::Time& time)   // optional
  void stopping (const ros::Time& time);  // optional
  ...
}

franka_hw 小节描述了可用的接口。

重要

请注意，命令接口的可声明组合受到限制，因为例如同时命令关节位置和笛卡尔位姿等没有意义。只读接口，如 JointStateInterface、FrankaStateInterface 或 FrankaModelInterface 始终可以声明并且不受限制。

命令接口的可能声明是：

franka_hw::FrankaHW

franka_combinable_hw::FrankaCombinableHW

  • 所有可能的单一接口声明

  • EffortJointInterface + PositionJointInterface

  • EffortJointInterface + VelocityJointInterface

  • EffortJointInterface + FrankaCartesianPoseInterface

  • EffortJointInterface + FrankaCartesianVelocityInterface

  • EffortJointInterface

  • EffortJointInterface + FrankaCartesianPoseInterface

  • EffortJointInterface + FrankaCartesianVelocityInterface

结合运动生成器接口提供 EffortJointInterface 背后的想法是向用户公开内部运动生成器。计算出的与运动生成器命令相对应的期望关节位姿在一个时间步后的机器人状态中可用。这种组合的一个用例是使用自己的关节层级扭矩控制器跟随笛卡尔轨迹。在这种情况下，将声明 EffortJointInterface + FrankaCartesianPoseInterface 的组合，将轨迹传输到 FrankaCartesianPoseInterface，并根据从机器人状态得到的期望关节位姿 (q_d) 计算关节层级扭矩命令。这允许使用机器人的内置逆运动学，而不必自己解决。

要实现功能齐全的控制器，必须至少实现继承的虚函数 init 和 update。初始化 - 例如启动位姿 - 应该在 starting 函数中完成，因为在重新启动控制器时会调用 starting，而在加载控制器时只调用一次 init。stopping 方法应包含与关闭相关的功能（如果需要）。

重要

在关闭控制器之前，始终给命令来温和地减速。使用速度接口时，不要在 stopping 时简单地命令速度为零。因为它可能在机器人仍在移动时被调用，所以它相当于命令速度跳跃导致非常高的关节层级扭矩。在这种情况下，保持相同的速度并停止控制器，比发送零值并让机器人处理减速更好。

控制器类必须通过 pluginlib 正确导出，这需要添加：

#include <pluginlib/class_list_macros.h>
// Implementation ..
PLUGINLIB_EXPORT_CLASS(name_of_your_controller_package::NameOfYourControllerClass,
                       controller_interface::ControllerBase)

在 .cpp 文件的末尾。此外，需要定义一个包含以下内容的 plugin.xml 文件：

<library path="lib/lib<name_of_your_controller_library>">
  <class name="name_of_your_controller_package/NameOfYourControllerClass"
         type="name_of_your_controller_package::NameOfYourControllerClass"
         base_class_type="controller_interface::ControllerBase">
    <description>
      Some text to describe what your controller is doing
    </description>
  </class>
</library>

通过添加导出：

<export>
  <controller_interface plugin="${prefix}/plugin.xml"/>
</export>

到 package.xml。此外，需要至少将控制器名称与控制器类型结合加载到 ROS 参数服务器。此外，可以包括需要的其他参数。一个示例 configuration.yaml 文件可能看起来如下所示：

your_custom_controller_name:
  type: name_of_your_controller_package/NameOfYourControllerClass
  additional_example_parameter: 0.0
  # ..

现在，可以使用 ROS control 中的 controller_spawner 节点或通过 hardware_manager 提供的服务调用来启动控制器。只要确保 controller_spawner 和 franka_control_node 在同一个命名空间中运行。有关更多详细信息，请查看 franka_example_controllers package 包或 ROS control 教程。