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_arm.urdf.xacro gazebo:=true

franka_gripper包

这个包实现了 franka_gripper_node 节点，用于交互连接来自 ROS 的夹具。该节点发布夹具的状态并提供以下 动作服务器：

  • 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(): 中止正在运行的动作服务器。这可以是用于在抓取后停止施加力。
  • control_msgs::GripperCommandAction(width, max_effort): MoveIt! 可识别的标准夹爪动作服务器。

你可以启动 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控制框架 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 的每个实例都提供完整的服务和操作接口集（请参阅 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 控制的硬件节点，它们使用 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 为外部笛卡尔扭力设置阈值以配置碰s撞反射。
  • 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。这可以从动作客户端完成，也可以通过简单地发布动作目标话题来完成：

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> load_gripper:=<true|false>

除了加载 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>

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

重要

只有一个 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>

其他单个 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 控制框架中。

重要

该软件包从 0.8.0 开始提供

拾放示例

让我们深入研究并模拟将石头从 A 运输到 B。运行以下命令以启动伴有Panda和示例世界的 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 动作发送一个目标，类似于真正的 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 动作，因为我们想在抓握后施加一个力，使物体不掉落。石头宽约 \(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）太小并且操作失败。现在将物体轻轻地移到红色的落点区域。

_images/franka-gazebo-example-grasp.png

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

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

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

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

定制化

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。然而，我们的机器人与这种架构完全不同！除了这些特定于关节的接口之外，它还支持特定于机器人的接口 robot-specific ，如 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 支持的一些非实时命令：

  服务 解释
✘ franka_msgs::SetJointImpedance Gazebo 不模拟内部阻抗控制器，而是直接设置扭矩命令
✘ franka_msgs::SetCartesianImpedance Gazebo 不模拟内部阻抗控制器，而是直接设置扭矩命令
✔ franka_msgs::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 所示相同），则假定为相同。您始终可以通过手动设置 ROS 参数 /<arm_id>/NE_T_EE 来覆盖此值。
✔ franka_msgs::SetKFrame 设置 \({}^{\mathrm{EE}}\mathbf{T}_{\mathrm{K}}\)，即从末端执行器到刚度坐标系的其次变换。
✔ franka_msgs::SetForceTorqueCollisionBehavior 设置阈值，高于该阈值外部扭力被视为接触和碰撞。
✘ franka_msgs::SetFullCollisionBehavior 尚未实现
✔ franka_msgs::SetLoad 设置外部负载以补偿其重力，例如被抓住的物体。您还可以通过分别设置 ROS 参数 /<arm_id>/{m_load,I_load,F_x_load} 对应质量、惯性张量和负载的质心来初始化它。

FrankaGripperSim插件

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

  • /<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 动作有一个bug，如果目标宽度让手指 open，它既不会成功也不会中止。这是因为缺少关节限制接口，这让手指在其极限处摆动。现在只使用 grasp 动作来 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\)): 当手指宽度低于此阈值时，移动动作成功
  • grasp/resting_threshold (double, default \(1\:\frac{mm}{s}\)): 低于该速度应检查目标宽度以中止或成功抓取动作
  • grasp/consecutive_samples (double, default: 3): 速度要连续低于阈值 resting_threshold 多少倍才会被评估为抓取
  • gripper_action/width_tolerance (double, default \(5\:mm\)): 当手指宽度低于此阈值时，抓取动作就（被认为）成功
  • gripper_action/speed (double, default \(10\:\frac{cm}{s}\)): 在抓取动作中使用的速度

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 来设置阈值
✔ O_F_ext_hat_K \(\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}\) 其中可以通过调用 set_force_torque_collision_behavior 设置阈值
✔ 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 控制中的 controller_spawner 节点或通过 hardware_manager 提供的服务调用来启动你的控制器。只要确保 controller_spawner 和 franka_control_node 在同一个命名空间中运行。有关更多详细信息，请查看 franka_example_controllers package 包或 ROS 控制教程。