为了方便入门和理解,我们这里建立一个最简单的单关节机器人,以此来入门机器人的gazebo仿真。

1.编辑urdf文件。

首先,我们在solidworks里面新建好机器人模型,然后导出urdf文件。

导出了urdf文件之后呢,我们就需要给urdf文件重新修改了, 首先直接修改文件名,在后面添加上.xacro后缀,改成了xacro文件。

 里面有solidworks软件里面的插件(转urdf插件)自己生成的两个连杆(base_link和link_1)和一个关节(joint_1)。然后我们就需要给其添加gazebo仿真所需要的标签了。

(1)dynamic标签

这个标签主要涉及动力学。

改标签是link标签的子标签,因此这个标签添加在link标签下面,每个link都需要一个dynamic标签。这个标签我们设置两个属性,第一个属性damping是阻尼系数,我们设置为0.7,第二个标签是摩擦系数,我们设置为0.5.

<dynamic damping="0.7" friction="0.5"/>

 (2)gazebo标签

这个标签主要设置连杆的颜色等属性。

  <gazebo reference="link_1">
    <material>Gazebo/Black</material>
  </gazebo>

(3)transmission标签

这个标签主要是设置关节驱动器,关节驱动器一般是电机,舵机这些,用于驱动关节,改变关节变量用的,每个活动关节都需要设置一个关节驱动电机,设置的每个关节驱动电机都需要和对应的关节绑定,以此来说明改驱动器驱动该关节。

  <transmission name="joint_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="joint_1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

(4)ros_control插件

此外我们还需要用gazebo标签来载入ros_control插件,由于我们整个驱动控制是基于ros_control做的,因此需要载入该插件,这个插件会给我们自动处理其余的任务,我们无需自己配置。

需要注意的是下面的<robotNamespace>标签,这个标签声明命名空间,这个需要留意一下。

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

添加了这些之后,我们的urdf文件就修改好了。

修改好之后的urdf文件如下:

<?xml version="1.0" encoding="utf-8"?>
 
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 
<robot
 
  name="gazebo_demo">
 
  <link
 
    name="base_link">
 
    <inertial>
 
      <origin
 
        xyz="0 9.74542958129844E-19 -4.31390337655099E-18"
 
        rpy="0 0 0" />
 
      <mass
 
        value="6.99004365423729" />
 
      <inertia
 
        ixx="0.0101938136624294"
 
        ixy="0"
 
        ixz="-3.37700910261971E-35"
 
        iyy="0.0101938136624294"
 
        iyz="-8.16962303997948E-35"
 
        izz="0.00873755456779662" />
 
    </inertial>
 
    <visual>
 
      <origin
 
        xyz="0 0 0"
 
        rpy="0 0 0" />
 
      <geometry>
 
        <mesh
 
          filename="package://gazebo_demo/meshes/base_link.STL" />
 
      </geometry>
 
      <material
 
        name="">
 
        <color
 
          rgba="0.537254901960784 0.349019607843137 0.337254901960784 1" />
 
      </material>
 
    </visual>
 
    <collision>
 
      <origin
 
        xyz="0 0 0"
 
        rpy="0 0 0" />
 
      <geometry>
 
        <mesh
 
          filename="package://gazebo_demo/meshes/base_link.STL" />
 
      </geometry>
 
    </collision>
  <dynamic damping="0.7" friction="0.5"/>
 
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Black</material>
  </gazebo>
 
  <link
 
    name="link_1">
 
    <inertial>
 
      <origin
 
        xyz="0.1 -1.86377227087894E-19 -1.38777878078145E-17"
 
        rpy="0 0 0" />
 
      <mass
 
        value="1.12492033114729" />
 
      <inertia
 
        ixx="0.000798459868114599"
 
        ixy="1.67727625611222E-20"
 
        ixz="8.85083255431749E-21"
 
        iyy="0.00679752984625413"
 
        iyz="9.66746937132908E-21"
 
        izz="0.00712727290972403" />
 
    </inertial>
 
    <visual>
 
      <origin
 
        xyz="0 0 0"
 
        rpy="0 0 0" />
 
      <geometry>
 
        <mesh
 
          filename="package://gazebo_demo/meshes/link_1.STL" />
 
      </geometry>
 
      <material
 
        name="">
 
        <color
 
          rgba="1 1 1 0.32" />
 
      </material>
 
    </visual>
 
    <collision>
 
      <origin
 
        xyz="0 0 0"
 
        rpy="0 0 0" />
 
      <geometry>
 
        <mesh
 
          filename="package://gazebo_demo/meshes/link_1.STL" />
 
      </geometry>
 
    </collision>
  <dynamic damping="0.7" friction="0.5"/>
 
  </link>
  <gazebo reference="link_1">
    <material>Gazebo/Black</material>
  </gazebo>
 
  <joint
 
    name="joint_1"
 
    type="revolute">
 
    <origin
 
      xyz="0 0 0.075"
 
      rpy="0 0 0" />
 
    <parent
 
      link="base_link" />
 
    <child
 
      link="link_1" />
 
    <axis
 
      xyz="0 0 -1" />
 
    <limit
 
      lower="-1.57"
 
      upper="1.57"
 
      effort="1"
 
      velocity="10" />
 
  </joint>
 
  <transmission name="joint_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="joint_1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
 
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/gazebo_demo</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
 
 
</robot>

2.编辑yaml文件

yaml文件为配置文件,主要配置关节控制器的物理参数,例如控制器类型,和pid等参数。

控制器类型一共有三种,分别是:位置控制,速度控制和力控制。

我这里配置好的文件如下。

gazebo_demo:
 
  # Publish all joint states -----------------------------------
 
  joint_state_controller:
 
    type: joint_state_controller/JointStateController
 
    publish_rate: 50  
 
 
 
  # Position Controllers ---------------------------------------
 
  joint1_position_controller:
 
    type: position_controllers/JointPositionController
 
    joint: joint_1
 
    #pid: {p: 100.0, i: 0.01, d: 10.0}
 
 
 
gazebo_demo/gazebo_ros_control/pid_gains:
 
  joint_1: {p: 100.0, i: 0.0, d: 10.0}

3.编辑launch文件启动gazebo仿真环境

这里我也编辑好了,如下:

<launch>
 
  <param name="robot_description" command="$(find xacro)/xacro $(find gazebo_demo)/urdf/gazebo_demo.urdf.xacro" />
 
  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
 
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find gazebo_demo)/worlds/gazebo_demo.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>
 
 
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model myrobot -param robot_description"/>
 
 
  <rosparam file="$(find gazebo_demo)/config/joint_names_gazebo_demo.yaml" command="load"/>
 
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
	output="screen" ns="/gazebo_demo" args="joint1_position_controller"/>
 
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
	respawn="false" output="screen">
    <remap from="/joint_states" to="/gazebo_demo/joint_states" />
  </node>
 
 
 
</launch>

启动之后就可以看到这个机器人出现在了地面上。

4.编写程序控制机器人关节往复运动

这里编写了一个简单的节点进行控制,程序如下:

#include "ros/ros.h"
#include "std_msgs/Float64.h" 
#include <sstream>
 
int main(int argc, char  *argv[])
{   
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"control_gazebo_demo");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<std_msgs::Float64>("/gazebo_demo/joint1_position_controller/command",10);
    std_msgs::Float64 control_gazebo_demo;
    control_gazebo_demo.data=0.0;
    bool flag=0;//0weizeng 1weijian
    ros::Rate r(50);
    while (ros::ok())
    {
        
        for(control_gazebo_demo.data=0.0;control_gazebo_demo.data<=1.5;control_gazebo_demo.data+=0.01)
        {
		pub.publish(control_gazebo_demo);
 
		ROS_INFO("发送的消息:%f",control_gazebo_demo);
		r.sleep();
        }
 
        for(control_gazebo_demo.data=1.5;control_gazebo_demo.data>=0.0;control_gazebo_demo.data-=0.01)
        {
		pub.publish(control_gazebo_demo);
 
		ROS_INFO("发送的消息:%f",control_gazebo_demo);
		r.sleep();
        }
       
 
        
        ros::spinOnce();
    }
    return 0;
}

5.运行效果

Gazebo仿真1自由度关节

6.全部源码地址

源码地址:

https://gitee.com/li9535/gazebo_simulation_ros_package/tree/master/