URDF笔记本：截取有用信息和可用信息

参考教程 cn urdf Tutorials

0、建立工作空间

1、用URDF从头开始构建可视化机器人模型

一个形状

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>
</robot>

将XML翻译成英语：这是一个名为myfirst的机器人，它只包含一个link（也称为part），其可视组件是一个0.6米长、半径为0.2米的圆柱体。

验证：roslaunch urdf_tutorial display.launch model:=urdf/01-myfirst.urdf

这个launch文件做三件事：
1、将指定的模型加载到参数服务器中
2、运行节点以发布传感器状态和转换
3、用配置文件启动Rviz

注意事项：
1、The fixed frame is transform frame where the center of the grid is located. Here, it’s a frame defined by our one link, base_link.
2、默认情况下，可视元素（圆柱体）的原点位于其几何图形的中心。因此，一半的圆柱体在网格之下。

多个形状

如果我们只是向urdf添加更多的link元素，解析器将不知道将它们放在哪里。所以，我们必须增加关节。

<?xml version="1.0"?>
<robot name="multipleshapes">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
  </joint>

</robot>

关节定义为父关节和子关节。URDF最终是一个具有一个根链接的树结构。这意味着腿的位置取决于base_link的位置。

起点(相对的概念)

1、关节的原点xyz是child link的原点将要摆放的位置（相对于parent link）
2、设置child link的原点xyz将会使该link相对于关节原点作偏置
3、设置child link的原点rpy将会使该link相对于关节原点作旋转
4、设置关节的原点rpy将会使该关节与其关联的link一起作旋转（相对于parent link）

<?xml version="1.0"?>
<robot name="origins">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>

</robot>

注意：

1、启动文件运行的包将基于您的URDF为模型中的每个link创建TF帧。Rviz使用这些信息来确定显示每个形状的位置。
2、如果一个给定的URDF link不存在TF帧，那么它将被默认以白色状态放置在原点。

材料

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>


  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>

颜色参考
也可以使用纹理指定用于为对象着色的图像文件

2、用URDF建立移动机器人模型

roslaunch urdf_tutorial display.launch model:=urdf/06-flexible.urdf

在以前的模型中，所有关节都是固定的。现在我们将探讨其他三种重要类型的关节：连续关节、旋转关节和棱柱关节（直线移动）。

连续关节（头部关节）

头部是一个连续的关节，意味着它可以从负无穷大到正无穷大的任意角度连续旋转
添加旋转轴：它指定了头部将围绕其旋转的矢量。我们希望它绕Z轴旋转，所以我们指定矢量“0 0 1”。

<joint name="head_swivel" type="continuous">
    <parent link="base_link"/>
    <child link="head"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 0.3"/>
  </joint>

旋转关节（夹持器）

有严格的limit。我们必须指定包含关节上下限（以弧度为单位）的限制标记。我们也必须为这个关节指定一个最大的速度和effort，实际值不会影响我们的期望值。

  <joint name="left_gripper_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
    <parent link="gripper_pole"/>
    <child link="left_gripper"/>
  </joint>

直线关节（夹持器手臂）

limit的单位是m

  <joint name="gripper_extension" type="prismatic">
    <parent link="base_link"/>
    <child link="gripper_pole"/>
    <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
  </joint>

其他类型的关节

直线关节只能沿一维移动，而平面关节可以在平面或二维中移动。此外，浮动关节是无约束的，可以在三维中任意一个方向上移动。

指定位姿

robot_state_publisher

This package allows you to publish the state of a robot to tf. Once the state gets published, it is available to all components in the system that also use tf. The package takes the joint angles of the robot as input and publishes the 3D poses of the robot links, using a kinematic tree model of the robot. The package can both be used as a library and as a ROS node. This package has been well tested and the code is stable. No major changes are planned in the near future.
此包允许您将robot的状态发布到tf。一旦状态被发布，系统中所有使用tf的组件都可以使用它。该软件包以机器人的关节角度为输入，使用机器人的运动树模型发布机器人连杆的三维姿态。这个包既可以用作库，也可以用作ROS节点。这个包已经过很好的测试，代码是稳定的。近期没有重大变化的计划。

joint_state_publisher

This package publishes sensor_msgs/JointState messages for a robot. The package reads the robot_description parameter, finds all of the non-fixed joints and publishes a JointState message with all those joints defined.
此包发布机器人的sensor-msgs/JointState消息。该包读取robot_description参数，查找所有非固定关节，并发布定义了所有这些关节的JointState消息。

Can be used in conjunction with the robot_state_publisher node to also publish transforms for all joint states.
可以与robot_state_publisher节点一起使用，也可以发布所有关节状态的变换。

当您在GUI中移动滑块时，模型在Rviz中移动，这是怎么做到的？
1、首先，GUI解析URDF并找到所有非固定的关节及其limit。
2、然后，它使用滑块的值发布sensor_msgs/JointState消息
3、然后，robot_state_publisher使用它们来计算不同部分之间的所有转换。
4、然后使用生成的变换树显示Rviz中的所有形状。

3、向URDF模型添加物理和碰撞

学习如何将碰撞和惯性特性添加到link，以及如何将关节动力学添加到关节。

碰撞单元

为了做碰撞测试或在gazebo等环境下进行仿真，我们需要定义碰撞单元

<link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>

注意事项：
1、碰撞元素是link对象的直接子元素，与visual标记处于同一级别
2、collision元素定义其形状的方式与visual元素相同，使用一个geometry标记。几何体标记的格式与可视化标记的格式完全相同。
3、您还可以使用与碰撞标记的子元素相同的方式指定原点（与visual相同）
有时碰撞的形状和visual的形状不一样：1、快速过程2、安全区域

物理属性

为了让你的模型能够正确模拟，你需要定义你的机器人的几个物理特性，也就是像gazebo这样的物理引擎所需要的特性。

惯性（具体见教程）

每个被模拟的link单元都需要一个惯性标签。

 <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="10"/>
      <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
    </inertial>
  </link>

注意事项：
1、此元素也是链接对象的子元素。
2、质量以千克为单位。
3、3x3转动惯量矩阵由惯量元素指定。因为这是对称的，所以它只由6个元素表示。

接触系数

还可以定义链接在彼此接触时的行为。这是通过名为contact_coefficients的碰撞标记的子元素完成的。有三个属性要指定：
mu - Friction coefficient
kp - Stiffness coefficient
kd - Dampening coefficient

关节动力学

关节的运动方式由关节的动力学标记定义。这里有两个属性：

摩擦-物理静态摩擦。对于直线关节，单位为牛顿。对于旋转关节，单位为牛顿米。
阻尼-物理阻尼值。对于直线关节，单位是牛顿秒每米。对于旋转关节，牛顿-米-秒/弧度。

如果未指定，则这些系数默认为零。

其他标签参考教程

4、使用Xacro清理URDF文件

学习一些使用Xacro减少URDF文件中代码量的技巧

现在，如果你在家里按照这些步骤来设计你自己的机器人，你可能已经厌倦了做各种各样的数学运算来得到非常简单的机器人描述以完成正确解析。幸运的是，您可以使用xacro包来简化您的生活。它做了三件非常有用的事情：

常数
简单数学
宏

使用Xacro

顾名思义，xacro是一种宏语言。
在URDF文件的顶部，必须指定一个名称空间，以便文件正确解析。例如，以下是有效xacro文件的前两行：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">

常数

<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<link name="base_link">
    <visual>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
        <material name="blue"/>
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
    </collision>
</link>

这两个值在前两行中指定。它们可以在任何地方（假设是有效的XML）定义，在任何级别，使用之前或之后。通常他们在最上面。
我们没有在geometry元素中指定实际的半径，而是使用美元符号和花括号来表示该值。
使用${}构造的内容值替换${}。这意味着您可以将其与属性中的其他文本组合。

<xacro:property name=”robotname” value=”marvin” />
<link name=”${robotname}s_leg” />

产生如下代码：

<link name=”marvins_leg” />

然而，${}中的内容不必只是一个属性，这就引出了我们的下一点。

数学

您可以使用四个基本操作（+、-、*、/）、一元减号和括号在${}构造中构建任意复杂的表达式。示例：

<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />

所有的计算都是使用浮点数完成的，因此

<link name="${5/6}"/>

等同于

<link name="0.833333333333"/>

宏

这是xacro包中最大最有用的组件。

简单的宏

<xacro:macro name="default_origin">
    <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>
<xacro:default_origin />

此代码将生成以下内容：

<origin rpy="0 0 0" xyz="0 0 0"/>

从技术上讲，名称不是必需的元素，但您需要指定它才能使用它。
<xacro:$NAME/>的每个实例都替换为xacro:macro标记的内容。
如果找不到具有指定名称的xacro，则不会展开它，也不会生成错误。

参数化宏

您还可以参数化宏，使它们每次都生成不同相同的文本。当与数学功能结合使用时，这将更加强大。

    <xacro:macro name="default_inertial" params="mass">
        <inertial>
                <mass value="${mass}" />
                <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                     iyy="1.0" iyz="0.0"
                     izz="1.0" />
        </inertial>
    </xacro:macro>

可以这样来使用

<xacro:default_inertial mass="10"/>

也可以将整个块用作参数。

<xacro:macro name="blue_shape" params="name *shape">
    <link name="${name}">
        <visual>
            <geometry>
                <xacro:insert_block name="shape" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <xacro:insert_block name="shape" />
            </geometry>
        </collision>
    </link>
</xacro:macro>

<xacro:blue_shape name="base_link">
    <cylinder radius=".42" length=".01" />
</xacro:blue_shape>

若要指定块参数，请在其参数名称前加 *。
可以使用“insert_block”命令插入块
插入块的次数根据您的需要而定，随便多少次。

实际使用

腿部宏

通常需要在不同的位置创建多个外观相似的对象。通常，位置会有一些对称性。您可以使用宏和一些简单的数学来减少必须编写的代码量，就像我们使用R2的两条腿一样。

<xacro:macro name="leg" params="prefix reflect">
    <link name="${prefix}_leg">
        <visual>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <joint name="base_to_${prefix}_leg" type="fixed">
        <parent link="base_link"/>
        <child link="${prefix}_leg"/>
        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
    </joint>
    <!-- A bunch of stuff cut -->
</xacro:macro>
<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />

常见技巧1：使用名称前缀获取两个同名对象
常见技巧2：使用数学计算关节原点。如果您更改了机器人的大小，则使用一些数学方法更改属性以计算关节偏移将节省许多麻烦。
常见技巧3：使用反射参数，并将其设置为1或-1。请看我们如何使用reflect参数将腿放在基础腿到腿原点的身体两侧。

5、在Gazebo中使用URDF

把Gazebo想成一个世界，而你的机器人就处于里面，所与你需要与你的机器人交互也就意味着和Gazebo交互。
ROS里面把解算程序写好，joint_state_publisher发布关节值给robot_state_publisher，解算出pose值发给tf，然后发给gazebo进行运动仿真。暂时的理解，后续补充！

Nonfunctional Gazebo Interface

我们可以使用Gazebo.launch将我们已经创建的模型派生到Gazebo中

 roslaunch urdf_sim_tutorial gazebo.launch 

这个launch做以下几件事清：

将宏教程中的urdf加载到参数描述中（如前所述）
展开一个空旷的gazebo世界
运行脚本从参数中读取urdf并在gazebo中生成它。
默认情况下，gazebo图形用户界面也将显示

然而，它什么也做不了，而且丢失了很多ROS使用这个机器人所需要的关键信息。以前我们一直使用joint_state_publisher来指定每个关节的姿势。然而，机器人本身应该在现实世界或露台上提供这些信息。然而，如果没有具体说明，Gazebo并不知道要发布这些信息。

这里我的理解是joint_state_publisher只是负责发布关节值的，但首先要有关节值才能发布对不对，所以这个关节值应该由我们指定(由joint_state_controler发布还是？)，可能就是之后写的一些解算程序啥的会和这里联系起来。而之前在rviz中通过滑块设置关节值应该也是一种发布关节值的方式。但是最终实际应用的话还是我们自己指定，也就是将逆解算出来的关节值发布到这里。暂时的理解，之后更新！

为了让机器人与你和ROS互动，我们需要指定两个东西：Plugins和Transmissions。

Gazebo Plugin

为了让ROS与Gazebo进行互动，我们必须动态链接到ROS库，告诉Gazebo该做什么。
To link Gazebo and ROS, we specify the plugin in the URDF, right before the closing tag:

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
    </plugin>
  </gazebo>

不过，这还没有什么新的进展。为此，我们需要在URDF之外指定更多信息。

Spawning Controllers

现在我们已经连接了ROS和Gazebo，我们需要指定一些ROS代码，这些代码是我们希望在Gazebo中运行的，我们通常称之为控制器。它们最初被加载到ROS参数空间中。我们的第一个控制器，yaml文件joints.yaml（一般情况下位于MYROBOT_control/config目录下）。

type: "joint_state_controller/JointStateController"
publish_rate: 50

这个控制器可以在joint_state_controller包中找到，并直接从Gazebo将机器人关节的状态发布到ROS中。（和joint_state_publisher的关系？，这个是从Gazebo2ROS？可能之后会发布到rviz）

在09-joints.launch中，您可以看到我们应该如何将这个yaml文件加载到r2d2_joint_state_controller名称空间中。然后我们调用controller_manager/spawner`脚本，并将其加载到Gazebo中。

你可以启动这个，但还不完全，头部不正常。roslaunch urdf_sim_tutorial 09-joints.launch
这将运行控制器，并实际发布/joint_states话题（joint_state_publisher也对应这个话题）……但其中没有任何内容。

header:
  seq: 652
  stamp:
    secs: 13
    nsecs: 331000000
  frame_id: ''
name: []
position: []
velocity: []
effort: []

你还想要Gazebo做什么吗？好吧，你想它知道要发布哪些关节的信息。

Transmissions

对于每一个非固定节点，我们需要指定一个Transmissions，它告诉gazebo如何处理关节。我们从头关节开始。在URDF中添加以下内容：

  <transmission name="head_swivel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="$head_swivel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_swivel">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

出于介绍的目的，只需将这段代码中的大部分作为样板。
首先要注意的是关节元素。名称应该与前面声明的关节匹配。
在我们探索插件时，硬件接口将非常重要。

现在，头部在RViz中正确显示，因为头部关节在关节状态消息中列出。

header:
  seq: 220
  stamp:
    secs: 4
    nsecs: 707000000
  frame_id: ''
name: ['head_swivel']
position: [-2.9051283156888985e-08]
velocity: [7.575990694887896e-06]
effort: [0.0]

我们可以继续为所有非固定接头添加变速器（我们会这样做的）
所有关节都已正确发布。但是，生活不仅仅是看机器人。我们想控制他们，所以，让我们再找一个控制器。

Joint Control

type: "position_controllers/JointPositionController"
joint: head_swivel

这指定使用position_controllers包中的JointPositionController来控制head_swivel transmission。请注意，此关节的URDF中的硬件接口与控制器类型匹配。

roslaunch urdf_sim_tutorial 10-head.launch 

现在Gazebo订阅了一个新的话题，然后你可以通过在ROS中发布一个值来控制头部的位置
rostopic pub /r2d2_head_controller/command std_msgs/Float64 “data: -0.707”

发布此命令后，位置将立即更改为指定值。这是因为我们在urdf中没有规定接头的任何限制。但是，如果我们改变关节，它会逐渐移动。

  <joint name="head_swivel" type="continuous">
    <parent link="base_link"/>
    <child link="head"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 ${bodylen/2}"/>
    <limit effort="30" velocity="1.0"/>
  </joint>

Another Controller

我们可以用类似的方法改变夹持接头的URDF。然而，我们可能不想用它自己的ROS主题单独控制抓取器的每个关节，而是将它们组合在一起。为此，我们只需要在ROS参数中指定一个不同的控制器。

type: "position_controllers/JointGroupPositionController"
joints:
  - gripper_extension
  - left_gripper_joint
  - right_gripper_joint

机器人的轮子转来转去(具体看教程)

Understanding the PR2 Robot Description

本教程介绍了复杂机器人（如PR2）的顶层URDF Xacro文件的布局。

后续

Tutorial: Using a URDF in Gazebo
Learning URDF (including C++ API)
TF(Transmissoin frame)