从xacro模型文件搭建Moveit!+Gazebo仿真系统

最新推荐文章于 2024-05-05 17:32:00 发布

猫十一.

最新推荐文章于 2024-05-05 17:32:00 发布

阅读量1.8k

点赞数 3

分类专栏： ROS

原文链接：https://www.lightshaker.cn/2020/07/28/anno-simulation/

版权

ROS 专栏收录该内容

7 篇文章 16 订阅

订阅专栏

使用Moveit生成机械臂的配置文件

参考官方文档即可：
http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html

Gazebo搭建ROS仿真环境

相关概念简介

what is ros_control？

ROS为开发者提供的机器人控制中间件。
包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等。
可以帮助机器人应用功能包快速落地，提高开发效率。

图中的一些模块的含义：
Controller Manager：控制中心，管理控制器与监视器，管理方式例如list，load，unload，switch等
joint_state_publisher：监视器，负责从硬件抽象层接受反馈信号。
joint_trajectory_controller：关节轨迹控制器，使用PID控制算法发布控制信号至硬件抽象层。
Hardware Resource Interface Layer：硬件抽象层，控制层与硬件资源之间的数据中转站，负责转换普通数据为ROS通讯数据（msg, srv等等），以及传输数据。
Simulation-Gazebo：Gazebo作为硬件仿真器，由仿真硬件接口中心通过读和写的方式控制
DefaultRobotHWSim：介于ros_control与Gazebo之间提供速度、位置和力矩接口的仿真接口中心
Effort Transmissions：力传动装置，接受硬件抽象层封装的数据并转化为力矩等具体控制数据。
Joint Limits：执行一些关节限制条件，例如碰撞检测，防止模型崩溃，检测后通过接口中心调用将控制力矩写入Gazebo，控制仿真机器人运动
Forward State Transmission：通过接口中心实时读取仿真运动的控制反馈数据，并传递回硬件抽象层
Hardware-Reality：实体机器人
RobotHW：介于ros_control与Gazebo之间提供速度、位置和力矩接口默认的实体硬件接口中心
Embedded Controller：实体电机上的嵌入式控制板卡，接受Joint Limits的力矩数据，转化为电压信号，通过硬件接口中心发送至执行机构Actuators。
Actuators：伺服电机或其他驱动器，按照嵌入式板卡发送的电压数据驱动实体机器人运动
Encoders：编码器，位于执行机构上，实时读取机器人的关节运动状态，由Forward State Transmission通过硬件接口中心读取并反馈至硬件抽象层
ROS常用控制器：
joint_velocity_controller：速度控制器
joint_position_controller：位置控制器
joint_effort_controller：力控制器
joint_state_controller：监控机器人运行状态的模块（接收和处理控制过程的产生的信息），与控制过程无关

ROS中的控制器插件
控制器管理器：Controller Manager，提供一种通用的接口来管理所有控制器，此处的管理指的就是统筹各个控制器的开关以及各种参数和输入输出等等。
控制器：Controller，读取硬件状态，发布控制命令至Gazebo（仿真）/硬件抽象层（实体），控制实体机器人的话硬件抽象层还会将数据发送给具体的硬件部位，例如小车车轮上的嵌入式板卡，再由该开发板根据控制方式发送电压信号给驱动电机（执行机构），实现对具体关节的控制，并且在机器人运动过程中，电机上装载的编码器将监控电机的转速并将其反馈至硬件抽象层，进行封装后再经由interface接口发送至控制层的joint_state_controller，形成完整的闭环控制。
硬件资源：为上下两层提供硬件资源的接口
机器人硬件抽象：与机器人硬件资源直接想通，通过write和read的方式完成硬件操作
机器人硬件：执行接收到的命令

完善上一节创建的机器人模型
以文本形式打开~/visionArm/src/probot_description/urdf/probot_anno.xacro模型文件，修改配置
第一步：为每个link添加惯性参数和碰撞属性
一个link属性不可缺少三部分：惯性参数（inertial）、可视化属性（visual）和碰撞属性（collision），打开配置文件后发现属性的形式已经是完整的了，只需要改一些参数。
我们将连杆的质量的值设置为0.00001，虽然与实际质量的差距可能较大，但能使模型在仿真的过程中更加稳定。另外，将惯性参数的ixx、iyy和izz均设置为10。

<link name="link_1">
  <inertial>
    <origin xyz="-0.010934 0.23134 0.0051509" rpy="0 0 0" />
    <mass value="0.00001" />
    <inertia ixx="10" ixy="0" ixz="0" iyy="10" iyz="0" izz="10" />
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <mesh filename="package://probot_description/meshes/link_1.STL" />
    </geometry>
    <material name="">
      <color rgba="0.79216 0.81961 0.93333 1" />
    </material>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <mesh filename="package://probot_description/meshes/link_1.STL" />
    </geometry>
  </collision>
</link>

第二步：为每个joint添加传动装置
joint的作用显然是连接连个link，但link的运动方式有多种，例如平移、旋转等等，因此需要为joint添加传动装置以实现连杆的多种运动方式，传动装置一般就是电机和减速器的组合体，不可缺少。
利用宏为六个joint添加作为传动装置的电机和减速器

 <!-- Transmissions for ROS Control -->
 <xacro:macro name="transmission_block" params="joint_name">
    <transmission name="tran1">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${joint_name}">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
</xacro:macro>

<xacro:transmission_block joint_name="joint_1"/>
<xacro:transmission_block joint_name="joint_2"/>
<xacro:transmission_block joint_name="joint_3"/>
<xacro:transmission_block joint_name="joint_4"/>
<xacro:transmission_block joint_name="joint_5"/>
<xacro:transmission_block joint_name="joint_6"/>

第三步：添加gazebo控制器插件

<!-- ros_control plugin -->
<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/probot_anno</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    <legacyModeNS>true</legacyModeNS>
  </plugin>
</gazebo>

构建Moveit!+Gazebo联合仿真

MoveIt!机器人控制框架
在这里插入图片描述
其中各个模块的功能如下：

Moveit!模块：运动学+路径规划+碰撞检测
输入：通过可视化控制界面Rviz发布的指令，或包含控制指令的C++、Python源代码
输出：关节空间运动轨迹数据，由多个路径点组成，单点的数据由各个轴的位置、速度、加速度组合描述
Follow Joint Trajectory：将Moveit!模块输出的数据封装成具体的通讯机制（话题、服务等）
Trajectory Controller：机器人控制器（仿真/实体），包含上文所说的控制中心、控制器与硬件抽象层
输入：经过格式转换的Moveit!模块输出路径点数据
控制特点：
线性样条：位置连续，速度、加速度不连续
三次样条：位置和速度连续，加速度不连续
五次样条：位置、速度、加速度都连续（ROS默认的插补方式）
Trajectory Interpolation：轨迹插补，输入的轨迹由于路径点之间存在时间差而整体较粗糙，通过在路径点的时间差内进行插补的方式，可以细化整个轨迹（简单地说就是增大路径点密度）
Position Servo：位置伺服，通过分析插补后的轨迹，确定下一时刻的机器人的位姿，并产生控制机器人各个驱动电机的电压信号，属于硬件抽象层。
输出：驱动机器人各个电机的电压信号
Robot Body：机器人本体（仿真/实体），6个电机（Ｍ）根据伺服信号驱动机器人运动，并通过伺服电机上的编码器实时反馈数据。
Joint State Controller：控制层的监控模块，接受并处理反馈信号

首先添加功能包probot_gazebo，编写launch文件
probot_anno_gazebo_world.launch，内容如下

<launch>

  <!-- 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 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <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>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find probot_description)/urdf/probot_anno.xacro'" /> 


  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model probot_anno -param robot_description"/> 


</launch>

运行查看机械臂模型在gazebo中是否显示正常

cd ~/visionArm_ws
catkin_make
roslaunch probot_gazebo probot_anno_gazebo_world.launch

显示正常，说明模型没有问题，但此时模型还不能运动，因为我们还没有配置gazebo中的控制器，也就暂时无法接受和处理Moveit发送的轨迹路径点数据。对应的控制器就是Moveit!机器人控制框架中Gazebo部分的Trajectory Controller与Joint State Controller。另外，在配置完Gazebo控制器后还需要配置Moveit中的Follow Joint Trajectory控制器，才可完成Moveit!与Gazebo的通讯。

配置Gazebo控制器

首先配置关节轨迹控制器（轨迹插补）Trajectory Controller
在新创建的功能包probot_gazebo/config路径下新建配置文件probot_anno_trajectory_control.yaml，配置内容如下，内容含义为明确控制器接受数据的对象是哪几个关节，注意这里的6个关节名要与原xacro模型文件中的关节名一致，gains表示每个关节在做闭环PID控制时的参数，此处设置了一个默认值，在之后的调试中若要提高控制精度，需要改动这些pid参数。

probot_anno:
  arm_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6

    gains:
      joint_1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

之后我们需要创建一个新的launch文件，用于加载这个控制器，这里命名为probot_anno_trajectory_controller.launch，内容如下，大致意义为加载上面的yaml文件，并通过启动已有的节点spawner来启动控制器节点arm_controller_spawner，节点名与yaml中的控制器名对应。

<launch>

    <rosparam file="$(find probot_gazebo)/config/probot_anno_trajectory_control.yaml" command="load"/>

    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/probot_anno" args="arm_joint_controller"/>

</launch>

然后配置关节状态控制器Joint State Controller
同样在probot_gazebo/config路径下创建配置文件probot_anno_gazebo_joint_states.yaml，内容如下，意义为按照50hz的频率反馈信息。

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

创建launch文件probot_anno_gazebo_states.launch用于启动该控制器，内容如下

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find probot_gazebo)/config/probot_anno_gazebo_joint_states.yaml" command="load"/>

    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/probot_anno" args="joint_state_controller" />

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
        <remap from="/joint_states" to="/probot_anno/joint_states" />
    </node>

</launch>

配置Moveit!控制器

刚刚配置的是Gazebo环境中的关节轨迹控制器与关节状态控制器，现在要配置的是Moveit!的控制器Follow Joint Trajectory，作用是转换Moveit!的轨迹路径点数据为ROS通讯数据。

在Moviet生成的配置包的配置目录probot_anno_moveit_config/config下创建配置文件controllers_gazebo.yaml，添加以下内容：

controller_manager_ns: controller_manager
controller_list:
 - name: probot_anno/arm_joint_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - joint_1
     - joint_2
     - joint_3
     - joint_4
     - joint_5
     - joint_6

在/launch路径中打开moveit控制器启动文件probot_anno_moveit_controller_manager.launch.xml，按照下面的内容修改，添加读取Moveit仿真控制器配置文件的内容

<launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- 删除该段 -->
  <!-- oads ros_controllers to the param server -->
  <rosparam file="$(find probot_anno_moveit_config)/config/ros_controllers.yaml"/>

  <!-- 添加该段 -->  
  <!-- gazebo Controller -->
  <rosparam file="$(find probot_anno_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>

完成之后需要另外创建一个launch文件moveit_planning_execution.launch，以同时启动原有的两个moveit相关的launch文件move_group.launch与moveit_rviz.launch，并且启动关节状态发布器节点joint_state_publisher

<launch>
 <include file="$(find probot_anno_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>

 # The visualization component of MoveIt!
 <include file="$(find probot_anno_moveit_config)/launch/moveit_rviz.launch">
  <arg name="config" value="true" />
 </include>

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/probot_anno/joint_states]</rosparam>
  </node>

</launch>

最后，我们在probot_gazebo/launch路径下再创建一个launch文件probot_anno_bringup_moveit.launch，用于同时启动刚刚完成配置的三个控制器（Gazebo关节轨迹控制器，Gazebo关节状态控制器，Moveit!控制器），并启动刚刚创建的moveit_planning_execution.launch。

<launch>

    <!-- Launch Gazebo  -->
    <include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_world.launch" />

    <!-- ros_control arm launch file -->
    <include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_states.launch" />   

    <!-- ros_control trajectory control dof arm launch file -->
    <include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_trajectory_controller.launch" />

    <!-- moveit launch file -->
    <include file="$(find probot_anno_moveit_config)/launch/moveit_planning_execution.launch" />

</launch>

试运行仿真并查看数据

配置完成后，执行roslaunch命令运行总的launch文件查看效果

roslaunch probot_gazebo probot_anno_bringup_moveit.launch

发现报错：unused args [config] for include of xxx/moveit_rviz.launch

打开probot_anno_moveit_config/launch/moveit_rviz.launch文件，进行如下修改

<!-- 原文件内容 -->
<launch>

  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="rviz_config" default="" />
  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />

  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
    args="$(arg command_args)" output="screen">
  </node>

</launch>

<!-- 更改后的内容 -->
<launch>

  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="config" default="false" />
  <arg unless="$(arg config)" name="command_args" default="" />
  <arg     if="$(arg config)" name="command_args" default="-d $(find probot_anno_moveit_config)/launch/moveit.rviz" />

  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
    args="$(arg command_args)" output="screen">
    <rosparam command="load" file="$(find probot_anno_moveit_config)/config/kinematics.yaml"/>
  </node>

</launch>

再次执行launch命令，这回正常了，同时启动了gazebo与rviz

现在，我们可以拖动rviz中机械臂末端的球体设定目标位姿，点击Planning中的Plan and Execute，观察到rviz和gazebo中的机械臂均按照规划的轨迹运动并且二者的位姿时刻保持一致，至此基础环境配置部分完成。

为了查询关节空间轨迹规划的数据信息，我们执行rostopic list，找到以probot_anno开头topic：
在这里插入图片描述
其中的/goal就是我们需要的目标轨迹话题，我们使用rostopic echo指令输出它的内容

rostopic echo /probot_anno/arm_joint_controller/follow_joint_trajectory/goal

发现提示没有输出内容 “no messages received and simulated time is active”，我们需要让机器人动起来才有规划的数据，因此随便在rviz中规划一次运动，再执行一次查看数据：
在这里插入图片描述
其中header与goal_id是本次规划的标志信息，而goal里的points储存的就是本次规划的具体内容，可见一个路径点的数据由位置position、速度velocities、加速度accelerations、力矩effort以及时间戳time_from_start构成。这些数据将传输给Moveit控制器Follow Joint Trajectory，进行封装后再传递给Gazebo或实际的机器人控制器进行插补，最后传输给驱动装置控制机器人运动。

另外，可以用rqt_plot可以查看各个关节在本次规划中的运动轨迹（属于关节空间），在上一步的rostopic list中可查找到名为joint_states的话题，我们输出这个话题的内容：

rostopic echo joint_states

在这里插入图片描述
可见话题内的数据为不同时刻下的各个关节的位置、速度以及力的数值。我们直接使用rqt_plot输出各个关节的位置曲线看看（需要进行一次任意规划才有曲线，否则位置不变一直会是直线）

rqt_plot joint_states/position[0]:position[1]:position[2]:position[3]:position[4]:position[5]

在这里插入图片描述

原文链接: https://www.lightshaker.cn/2020/07/28/anno-simulation/

猫十一.

关注

3
点赞
踩
27

收藏

觉得还不错? 一键收藏
4
评论
从xacro模型文件搭建Moveit!+Gazebo仿真系统

使用Moveit生成机械臂的配置文件参考官方文档即可：http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.htmlGazebo搭建ROS仿真环境相关概念简介what is ros_control？ROS为开发者提供的机器人控制中间件。包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等。可以帮助机器人应用功能包快速落地，提高开发效
复制链接

扫一扫