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{c m}{s}$ 移动到手指之间的宽度为 $8 c m$ :

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 c m$ ，重 $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	设置 $^{NE} T_{EE}$ ，即从标称末端效应器到末端效应器的齐次变换。也可以通过设置 ROS 参数 `/<arm_id>/NE_T_EE` 来初始化它。通常会在 Desk 中设置 $^{F} T_{NE}$ ，但在 `franka_gazebo` 中，如果没有指定夹爪或定义围绕 Z 旋转 $45 °$ 和偏移 $10.34 c m$ （与抓手的 Desk 所示相同），则假定为与 Desk 中相同。始终可以通过手动设置 ROS 参数 `/<arm_id>/NE_T_EE` 来覆盖此值。
✔	`set_K_frame` /SetKFrame	设置 $^{EE} T_{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 m m$ ): 当手指宽度低于此阈值时，move action 成功
grasp/resting_threshold (double, default $1 \frac{m m}{s}$ ): 低于该速度应检查目标宽度以中止或成功 grasp action
grasp/consecutive_samples (double, default: 3): 速度要连续低于阈值 resting_threshold 多少次才会被评估为抓取
gripper_action/width_tolerance (double, default $5 m m$ ): 当手指宽度低于此阈值时，夹爪 action 就（被认为）成功
gripper_action/speed (double, default $10 \frac{c m}{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 访问机器人状态 RobotState 时，它可以期望仿真出以下值：

	区域	注释
✔	`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`	$∣ {\hat{τ}}_{e x t} ∣> {thres}_{l o w e r}$ 其中可以通过调用 `set_force_torque_collision_behavior` 设置阈值
✔	`joint_collision`	$∣ {\hat{τ}}_{e x t} ∣> {thres}_{u p p e r}$ 其中可以通过调用 `set_force_torque_collision_behavior` 设置阈值
✔	`cartesian_contact`	$∣^{K} {\hat{F}}_{K, e x t} ∣> {thres}_{l o w e r}$ 其中可以通过调用 `set_force_torque_collision_behavior` 来设置阈值
✔	`cartesian_collision`	$∣^{K} {\hat{F}}_{K, e x t} ∣> {thres}_{u p p e r}$ 其中可以通过调用 `set_force_torque_collision_behavior` 来设置阈值
✔	`tau_ext_hat_filtered`	${\hat{τ}}_{e x t}$ 即估计的末端执行器的外部扭矩和力，用指数移动平均滤波器 (EMA) 过滤。其过滤的 $α$ 可以通过 ROS 参数进行配置。该字段不包含任何重力项，即 $τ_{e x t} = τ_{J} - τ_{J_{d}} - τ_{g r a v i t y}$
✔	`O_F_ext_hat_K`	$^{O} {\hat{F}}_{K, e x t} = J_{O}^{⊤ +} \cdot {\hat{τ}}_{e x t}$
✔	`K_F_ext_hat_K`	$^{K} {\hat{F}}_{K, e x t} = J_{K}^{⊤ +} \cdot {\hat{τ}}_{e x t}$
✘	`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 文档