学习笔记28 --常见活动关节以及控制器的urdf,控制器的yaml,以及对应的launch文件编写以及对应的仿真测试

环境:ubuntu16.04 ros-kinetic gazebo7.16 turtlebot_teleop包

注意,代码中使用到的中文注释,使用时候一定要删除,否则会报错的.

来源:
以前编写机器人urdf时候,关节控制器编写,控制器yaml编写,把控制器正常加载到ros里面的launch文件编写吃了不少功夫,这次总结一下以往的近一年来摸索的到的经验.下面以一个案例来展示.

案例:
制作一个差速轮式运动机器人,上面有一个顶升以及一个可以绕z轴一定范围内摇动的力臂

案例中测试用到turtlebot_teleop包对机器人手动控制差速测试,记得提前安装.

urdf里面包含了,活动关节类型有:
continuous / prismatic / revolute;以及使用到的控制器接口类型有, hardware_interface/VelocityJointInterface \ hardware_interface/VelocityJointInterface \ hardware_interface/PositionJointInterface,
test_joint_controller.urdf.xacro:

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

<!--base_link-->
     <link name="base_link">
        <visual>
           <geometry>
              <box size="0.01 0.01 0.01"/>
           </geometry>
       </visual>
       <collision>
            <geometry>
              <box size="0.01 0.01 0.01"/>
            </geometry>
       </collision>
        <inertial>
           <mass value="5" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
         </inertial>
     </link>

<!--left_wheel-->
     <joint name="left_wheel_joint" type="continuous">//关节类型
       <parent link="base_link"/>
       <child link="left_wheel"/>
       <origin xyz="0 0.2 0"/>
      <axis xyz="0 1 0" rpy="0 0 0" />//定义转轴是y轴
     </joint>
     <link name="left_wheel">
        <visual>
           <geometry>
            <cylinder length="0.05" radius="0.1"/>
           </geometry>
        <origin rpy="1.5707 0 0" xyz="0 0 0"/>
        </visual>
        <collision>
            <geometry>
               <cylinder length="0.05" radius="0.1"/>
            </geometry>
           <origin rpy="1.5707 0 0" xyz="0 0 0"/>
        </collision>
        <inertial>
           <mass value="3" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
         </inertial>
     </link>

      <gazebo reference="left_wheel">//定义轮子在gazebo里面物理属性
        <mu1 value="200.0"/>
        <mu2 value="100.0"/>
        <kp value="10000000.0" />
        <kd value="1.0" />
        <material>Gazebo/Grey</material>
      </gazebo>
<!--define transmission which has motor-->
      <transmission name="left_wheel_trans">
        <type>transmission_interface/SimpleTransmission</type>
        <actuator name="left_wheel_motor">//定义接口
          <mechanicalReduction>1</mechanicalReduction>//传动比
        </actuator>
        <joint name="left_wheel_joint">//马达连接的关节
          <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//控制器硬件接口类型为速度型
        </joint>
      </transmission>

<!--right_wheel-->
     <joint name="right_wheel_joint" type="continuous">
       <parent link="base_link"/>
       <child link="right_wheel"/>
       <origin xyz="0 -0.2 0"/>
      <axis xyz="0 1 0" rpy="0 0 0" />
     </joint>
     <link name="right_wheel">
        <visual>
           <geometry>
            <cylinder length="0.05" radius="0.1"/>
           </geometry>
        <origin rpy="1.5707 0 0" xyz="0 0 0"/>
       </visual>
        <collision>
           <geometry>
              <cylinder length="0.05" radius="0.1"/>
           </geometry>
           <origin rpy="1.5707 0 0" xyz="0 0 0"/>
        </collision>
        <inertial>
           <mass value="3" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
         </inertial>
     </link>

      <gazebo reference="right_wheel">
        <mu1 value="200.0"/>
        <mu2 value="100.0"/>
        <kp value="10000000.0" />
        <kd value="1.0" />
        <material>Gazebo/Grey</material>
      </gazebo>
<!--define transmission which has motor-->
      <transmission name="right_wheel_trans">
        <type>transmission_interface/SimpleTransmission</type>
        <actuator name="right_wheel_motor">
          <mechanicalReduction>1</mechanicalReduction>
        </actuator>
        <joint name="right_wheel_joint">
          <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
      </transmission>

<!--idler_wheel-->
     <joint name="idler_wheel_joint" type="continuous">
       <parent link="base_link"/>
       <child link="idler_wheel"/>
       <origin xyz="0.2 0 0"/>
      <axis xyz="0 1 0" rpy="0 0 0" />
     </joint>
     <link name="idler_wheel">
        <visual>
           <geometry>
            <cylinder length="0.05" radius="0.1"/>
           </geometry>
        <origin rpy="1.5707 0 0" xyz="0 0 0"/>
       </visual>
        <collision>
           <geometry>
              <cylinder length="0.05" radius="0.1"/>
           </geometry>
           <origin rpy="1.5707 0 0" xyz="0 0 0"/>
        </collision>
        <inertial>
           <mass value="3" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
         </inertial>
     </link>

      <gazebo reference="idler_wheel">
        <mu1 value="0"/>
        <mu2 value="0"/>
        <kp value="10000000.0" />
        <kd value="1.0" />
        <material>Gazebo/Grey</material>
      </gazebo>

<!--climbing_mechanism-->
     <joint name="climbing_mechanism_to_base_link" type="prismatic">//定义一个prismatic类型的关节,主要是应用定轴移动的关节,案例中采用顶升来展示
       <parent link="base_link"/>
       <child link="climbing_mechanism"/>
       <origin xyz="0 0 0.5"/>
       <axis xyz="0 0 1" rpy="0 0 0" />//表示关节移动方向是z方向
     <limit effort="100.0" lower="0" upper="0.1" velocity="0.3"/>//这里定义了关节所具有的力大小,移动的范围以及对应移动过程的速度
     </joint>
     <link name="climbing_mechanism">
        <visual>
           <geometry>
              <cylinder length="0.1" radius="0.03"/>
           </geometry>
       </visual>
       <collision>
            <geometry>
              <cylinder length="0.1" radius="0.03"/>
            </geometry>
       </collision>
        <inertial>
           <mass value="1" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
         </inertial>
     </link>
    <gazebo reference="climbing_mechanism">
        <material>Gazebo/Orange</material>    
    </gazebo>
<!--set a VelocityJointInterface-->
  <transmission name="climbing_mechanism_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="climbing_mechanism_motor">
      <mechanicalReduction>16</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//定义为速度硬件接口,这里一定要记住prismatic关键对应的硬件接口为此
    </actuator>
    <joint name="climbing_mechanism_to_base_link">//定义硬件接口对应的关节或者表示马达链接的关节
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//速度类型硬件接口
    </joint>
  </transmission>

 <!--arm_axis-->
    <joint name="arm_axis_to_base_link" type="revolute">//定义关节类型为revolute,主要应用是一定幅度范围的部件
        <parent link="base_link"/>
        <child link="arm_axis"/>
        <origin xyz="0.1 0 0.2" rpy="0 0 0"/>
        <axis xyz="0 0 1" rpy="0 0 0"/>//定义该关节可以绕z轴转动
        <limit lower="-1.57" upper="1.57" effort="1090" velocity="1"/>//定义关节可活动弧度范围,力大小以及转动速度
    </joint>
    <link name="arm_axis">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.01 0.01 0.01"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="0.01"/>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1"/>
        </inertial>
    </link>
    <!--set a PositionJointInterface-->
    <transmission name="arm_axis_trans">
        <type>transmission_interface/SimpleTransmission</type>
        <actuator name="arm_axis_motor">
            <mechanicalReduction>10</mechanicalReduction>
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>//硬件接口类型是位置型
        </actuator>
        <joint name="arm_axis_to_base_link">//马达链接的关节名称
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>//关节对应的硬件接口是位置类型
        </joint>
    </transmission>
    <!--arm-->
    <joint name="arm_to_arm_axis" type="fixed">
        <parent link="arm_axis"/>
        <child link="arm"/>
        <origin xyz="0.3 0 0" rpy="0 1.57 0"/>
    </joint>
    <link name="arm">
        <visual>
            <geometry>
                <cylinder length="0.6" radius="0.025"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.6" radius="0.025"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
        </collision>
        <inertial>
            <mass value="2"/>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1"/>
        </inertial>
    </link>



    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
            <legacyModeNS>true</legacyModeNS>
        </plugin>
    </gazebo>

</robot>

这个yaml文件是对各个活动关节pid进行定义(提供给gazebo的),注意,yaml文件编写时候一定要按照对应的格式要求,否则是读不进去ros里面,从而导致控制器加载失败,下面是对各个控制器进行定义,test_joint_controller.yaml:

//滑动类型关节控制器yaml编写样式
climbing_mechanism_controller: //控制器名称自定义
  type: "velocity_controllers/JointPositionController"//接口类型一定要对应urdf里面对应的关节定义的
  joint: climbing_mechanism_to_base_link//控制器对应的关节
  pid: {p: 100.0, i: 0.01, d: 1.0}//这里定义该控制器的pid

//一定范围内转动的关节控制器定义样式
arm_position_controller:
 type: "position_controllers/JointGroupPositionController"
 joints:
  - arm_axis_to_base_link//对应urdf里面定义的关节

//差速运动控制器编写,(这里面也放了一些后面发odom啥的yaml,方便后面做的是建图导航用的,可以忽视的)
mobile_base_controller:
  base_frame_id: base_link
  enable_odom_tf: false
  odom_frame_id: odom
  publish_cmd: true
  wheel_separation: 0.393
  type: diff_drive_controller/DiffDriveController//控制器类型为差素类型
  left_wheel: left_wheel_joint//差速的左关节
  right_wheel: right_wheel_joint//差素的右关节
  publish_rate: 35
  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

//下面是给活动关节提供gazebo仿真所需要的pid(缺少会报错并且机器人可能会在gazebo里面失重,表现异常,如飞天)
gazebo_ros_control:
  pid_gains:
    # these gains are used by the gazebo_ros_control plugin
    climbing_mechanism_to_base_link:
      p: 10000
      i: 0.00000001
      d: 0.00000001

    left_wheel_joint:
      p: 100
      i: 0.001
      d: 0.001

    right_wheel_joint:
      p: 100
      i: 0.001
      d: 0.001

    arm_axis_to_base_link:
      p: 500.0
      i: 0.01
      d: 300.0

这个launch是把urdf以及定义的控制器加载到ros里面,test_joint_controller.launch:

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

<!-- Spawn a robot into Gazebo -->
<param name="robot_description" command="$(find xacro)/xacro '$(find rpy)/urdf/test_joint_controller.urdf.xacro' use_nominal_extrinsics:=true --inorder"/>

<node name="urdf_apawner" pkg="gazebo_ros" type="spawn_model" args="-z 1.0 -unparam -urdf -model robot -param robot_description" respawn="false" output="screen"/>

<!--load controller,need to define controller's yaml and define the pid yaml-->
<rosparam command="load" file="$(find rpy)/config/test_joint_controller.yaml"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="mobile_base_controller climbing_mechanism_controller arm_position_controller"/>//这里面的控制器名称需要根据yaml定义的一致

<include file="$(find gazebo_ros)/launch/empty_world.launch">
<param name="debug" value="true"/>
<param name="verbose" value="true"/>
</include>

<!--basic node-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>

<!--node name="rviz" pkg="rviz" type="rviz"/-->
</launch>

这个launch作用是启动键盘的,
test_joint_controller_teleop.launch:

<?xml version="1.0"?>
<launch>
  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
  <node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard"  output="screen">
    <param name="scale_linear" value="5.0" type="double"/>
    <param name="scale_angular" value="1.5" type="double"/>
    <remap from="turtlebot_teleop_keyboard/cmd_vel" to="mobile_base_controller/cmd_vel"/>
  </node>
</launch>

在终端里面加载机器人,如果有如下信息表示定义的控制器已正常加载到ros里面:

[INFO] [1595420563.704004, 0.500000]: Started controllers: mobile_base_controller, climbing_mechanism_controller, arm_position_controller

下面是把机器人加载到gazebo的图片,方便对比,这里面插入了一个正方体,

然后,这里测试一下climbing_mechanism_controller以及arm_position_controller的效果,分别在新建的终端里面输入

rostopic pub /climbing_mechanism_controller/command std_msgs/Float64 "data: 0.1" 

和

rostopic pub /arm_position_controller/command std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 0
    stride: 0
  data_offset: 0
data:
- -1" 

机器人效果如图:

差速运动的就不截图了,自己安装一下上面提及的键盘控制包,把那个上面另外一个launch文件在新的终端启动就可以玩了.

个人体会:
控制器正确定义需要urdf里面写好关节以及对应的硬件接口类型,
yaml文件一定要对设置的控制器进行初始化(属于哪个关节的,然后相应的参数有哪些等等),仿真时候一定要在yaml对应活动关节定义好gazebo的对应pid(大小自己测试来写ok了,如果不是很严谨的话),还有yaml文一定一定要注意格式(什么时候空格等等)

个人感觉,搞明白这个案例,基本上轮式移动机器人的urdf以及控制器yaml编写是没问题了.
#######################
好记性不如烂笔头
–20200722