探究--gazebo里 关节是如何动起来的____实现默认插件joint控制

先实现一个机器人joint的控制

创建一个机器人类似云台，实现航向和俯仰控制。

创建一个有joint的xacro文件

如何构建 及各标签含义及使用 参考：gazebo tutorial ：Using a URDF in Gazebo

给出xacro文件代码及注释

<robot name="swivel" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <material name="SwivelWhite">    <color rgba="1.0 1.0 1.0 1"/>material>
  <material name="SwivelLightGray"><color rgba="0.8 0.8 0.8 1"/>material>
  <material name="SwivelMedGray">  <color rgba="0.6 0.6 0.6 1"/>material>
  <material name="SwivelDarkGray"> <color rgba="0.4 0.4 0.4 1"/>material>
  <material name="SwivelRed">      <color rgba="0.5 0.4 0.4 1"/>material>
  <material name="SwivelGreen">    <color rgba="0.4 0.5 0.4 1"/>material>
  <material name="SwivelBlue">     <color rgba="0.4 0.4 0.5 1"/>material>

  
  
  
  <link name="base">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
				<box size="0.1 0.1 0.02" />
      geometry>
        <material name="SwivelLightGray" /> 
    visual>
        
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
				<box size="0.1 0.1 0.02" />
      geometry>
    collision>  
             
     <inertial>
        <mass value="9"/>
        <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/>
      inertial> 
  link>
    
  <gazebo reference="base">
       <material>Gazebo/Graymaterial>
  gazebo>


  
  <link name="link_1">
    <visual>
      <origin xyz="0 0 0.06" rpy="0 0 0" />
      <geometry >
	      <box size="0.05 0.05 0.1" />
      geometry>
      <material name="SwivelBlue" />
     visual>
     
    <collision>
      <origin xyz="0 0 0.06" rpy="0 0 0" />
      <geometry >
	      <box size="0.05 0.05 0.1" />
      geometry>
    collision> 
                 
     <inertial>
        <mass value="4"/>
        <inertia ixx="4.0" ixy="0.0" ixz="0.0" iyy="4.0" iyz="0.0" izz="4.0"/>
      inertial> 
  link>
  <gazebo reference="link_1">
       <material>Gazebo/Bluematerial>
  gazebo>

     
  <joint name="swivel_J0" type="revolute">
    <parent link="base"/>
    <child link="link_1"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="0" upper="6.28" effort="10"
	   velocity="1.0" />
    <joint_properties damping="0.0" friction="0.0"/>
  joint>

         
  <link name="link_2">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
      	<box size="0.1 0.2 0.1" />
      geometry>
      <material name="SwivelRed" />
    visual>
  <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
      	<box size="0.1 0.2 0.1" />
      geometry>
    collision> 
        <inertial>
        <mass value="2"/>
        <inertia ixx="2.0" ixy="0.0" ixz="0.0" iyy="2.0" iyz="0.0" izz="2.0"/>
      inertial> 
  link>
    <gazebo reference="link_2">
       <material>Gazebo/Redmaterial>
  gazebo>

       
  <joint name="swivel_J1" type="revolute">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin xyz="0 0 0.190" rpy="0 0 0" />
    <axis xyz="1 0 0" />
    <limit lower="-1.745" upper="2.356" effort="10"
	   velocity="1.0" />
    <joint_properties damping="0.0" friction="0.0"/>
  joint>

  <link name="camera_mount">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
  	<box size="0.05 0.05 0.05"/>
      geometry>
      <material name="SwivelGreen" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry >
  	     <box size="0.05 0.05 0.05"/>
      geometry>
    collision> 
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5"/>
      inertial> 
  link>

  <gazebo reference="camera_mount">
       <material>Gazebo/Greenmaterial>
  gazebo>



  <joint name="camera_J2" type="fixed">
    <parent link="link_2"/>
    <child link="camera_mount"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="1 0 0" />
    <limit lower="-2.094" upper="2.670" effort="10"
	   velocity="1.0" />
    <joint_properties damping="0.0" friction="0.0"/>
  joint>
    
robot>

显示模型如下所示

此时模型只是能在gazebo中显示出来，还不能做任何的动作

添加 transmission 标签

具体参考：gazebo Tutorial: ROS Control
将ros_control 用于自己的仿真机器人，需要在普通的URDF文件中加入其它标签。

< transmission >标签用于对应link的执行器到joint 。
其中主要的元素有：

• < joint name=" " > name必须关联到一个joint ，并且只能关联到一个 代码后面有解释
• < type > 传动器的种类，目前仅transmission_interface/SimpleTransmission可以实现
• < hardwareInterface > 在 < actuator >和 < joint >标签内部，gazebo_ros_control plugin 加载什么类型的硬件接口 （position, velocity or effort interfaces）

在link1与base的joint （swivel_J0） 后面加入

  
 <transmission name="tran1"> 
    <type>transmission_interface/SimpleTransmissiontype>
    <joint name="swivel_J0"> 
      <hardwareInterface>EffortJointInterfacehardwareInterface>
    joint>
    <actuator name="motor1"> 
      <hardwareInterface>EffortJointInterfacehardwareInterface>
      <mechanicalReduction>1mechanicalReduction>
    actuator>
  transmission>

在link1与ling2的joint（swivel_J1）

 
<transmission name="tran2"> 
    <type>transmission_interface/SimpleTransmissiontype>
    <joint name="swivel_J1">  
      <hardwareInterface>EffortJointInterfacehardwareInterface>
    joint>
    <actuator name="motor2">  
      <hardwareInterface>EffortJointInterfacehardwareInterface>
      <mechanicalReduction>1mechanicalReduction>
    actuator>
  transmission>

加入 gazebo_ros_control plugin 的标签

该插件标签实际上会 解析 transmission 并 加载适当的硬件接口和控制器管理器。
具体参考：gazebo Tutorial: ROS Control

默认情况下，gazebo_ros_control插件非常简单，尽管它还可以通过其他插件体系结构进行扩展，以允许高级用户在ros_control和Gazebo之间创建自己的自定义机器人硬件接口。

gazebo_ros_control < plugin >有如下子标签：

• < robotNamespace > 标签用于的ros的命名空间 ，默认是 robot name
• < controlPeriod > 控制器的更新周期（对应源码的update函数 调用频率 后面再对应）
• 对应源码
• < robotParam > 默认是’/robot_description’
• < robotSimType > 重点 用户可以扩展的机器人仿真接口 可以自己做插件，然后在此地方加载。 默认是已经写好的 DefaultRobotHWSim

后面
先用默认方式控制下
再用自己写的插件控制

在文件中加入如下代码

    <gazebo>
      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
         <robotNamespace>/swivel</robotNamespace>
        <controlPeriod>0.01</controlPeriod>
        <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      </plugin>
    </gazebo>

创建一个 .yaml配置文件（配置控制器）

控制器可以自己写，然后通过launch文件加载
也可以使用写好的控制器 直接写一个配置文件（.yaml）然后通过launch文件 将配置参数加载到参数服务器，在launch 文件里 启动控制器（controller_manager）该源码会根据参数加载对应的控制器（配置文件里的）。

swivel:
    # Publish all joint states -----------------------------------
  swiveljoint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  swiveljoint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: swivel_J0
    pid: {p: 100.0, i: 0.01, d: 10.0}
  swiveljoint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: swivel_J1
    pid: {p: 100.0, i: 0.01, d: 10.0}

写launch启动文件

<launch>

    
    <include file="$(find hector_gazebo_worlds)/launch/start.launch"/>

  
  <param name="robot_description"
         command="$(find xacro)/xacro.py '$(find swivel_description)/model/swivel_test.urdf.xacro'" />

  
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model rrbot -param robot_description"/>
 
  
  <rosparam file="$(find swivel_description)/config/swivel_gimbal_control.yaml" command="load"/>

  
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/swivel" args="swiveljoint_state_controller swiveljoint1_position_controller swiveljoint2_position_controller  --shutdown-timeout 3"/>

launch>

执行该launch文件

roslaunch swivel_world.launch 

可以看到.yaml 里的控制器已经加载 并且启动了

发布控制命令

因为是调用ros里面写好的控制器，所以不需要知道源码里是怎么接收消息，然后怎么处理的。也不用纠结为什么要发布这个topic。因为源码接收的就是这个topic。。。

rostopic pub -1 /swivel/swiveljoint1_position_controller/command std_msgs/Float64 "data: 1.5" 

然后会发现 竖轴转动了。已经成功控制了机器人的关节。

参考底下有rqt工具如何使用
可以发布topic
可以画曲线

探究怎么实现的

撸源码探究ros gazebo里的 硬件抽象构建

• ros里的控制器

写自己的插件实现控制

• 接下来要撸ros相关部分的源码了。探究是怎么实现的
• 然后是仿照源码写自己的插件 机器人接口插件 和 控制器插件