devbox
运维做开发
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS2与gazebo联合仿真自定义机械臂（ros2_control）_ros2中gazebo仿真机械臂

作者：运维做开发 | 2024-06-22 16:03:25

赞

踩

ros2中gazebo仿真机械臂

1. 准备实际的三维模型文件（stl二进制格式的）

2. 编写机械臂的urdf文件，也可以改装为xacro文件\

  1. <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="r6bot">
  2.     <xacro:property name="ModelPathHead"
  3.         value="file://"到机械臂三维模型的绝对路径" />
  4.     <xacro:property name="pi" value="3.1415926" />
  5.     <link name="base_link">
  6.         <visual>
  7.             <geometry>
  8.                 <mesh filename="${ModelPathHead}link_0.stl" />
  9.             </geometry>
  10.             <origin xyz="0 0 0" rpy="0 0 0" />
  11.             <material name="Red">
  12.                 <color rgba="1 0 0 1" />
  13.             </material>
  14.         </visual>
  15.         <collision>
  16.             <origin xyz="0 0 0" rpy="0 0 0" />
  17.             <geometry>
  18.                 <mesh filename="${ModelPathHead}link_0.stl" />
  19.             </geometry>
  20.         </collision>
  21.         <inertial>
  22.             <mass value="1" />
  23.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  24.         </inertial>
  25.     </link>
  26.  
  27.     <link name="link_1">
  28.         <visual>
  29.             <geometry>
  30.                 <mesh filename="${ModelPathHead}link_1.stl" />
  31.             </geometry>
  32.             <origin rpy="0 0 0" xyz="0 0 0" />
  33.             <material name="Red">
  34.                 <color rgba="1 0 0 1" />
  35.             </material>
  36.         </visual>
  37.         <collision>
  38.             <origin xyz="0 0 0" rpy="0 0 0" />
  39.             <geometry>
  40.                 <mesh filename="${ModelPathHead}link_1.stl" />
  41.             </geometry>
  42.         </collision>
  43.         <inertial>
  44.             <mass value="1" />
  45.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  46.         </inertial>
  47.     </link>
  48.  
  49.     <link name="link_2">
  50.         <visual>
  51.             <geometry>
  52.                 <mesh filename="${ModelPathHead}link_2.stl" />
  53.             </geometry>
  54.             <origin xyz="0 0 0" rpy="0 0 0" />
  55.             <material name="red">
  56.                 <color rgba="1 0 0 1" />
  57.             </material>
  58.         </visual>
  59.         <collision>
  60.             <origin xyx="0 0 0" rpy="0 0 0" />
  61.             <geometry>
  62.                 <mesh filename="${ModelPathHead}link_2.stl" />
  63.             </geometry>
  64.         </collision>
  65.         <inertial>
  66.             <mass value="1" />
  67.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  68.         </inertial>
  69.     </link>
  70.  
  71.     <link name="link_3">
  72.         <visual>
  73.             <geometry>
  74.                 <mesh filename="${ModelPathHead}link_3.stl" />
  75.             </geometry>
  76.             <origin xyz="0 0 0" rpy="0 0 0" />
  77.             <material name="Red">
  78.                 <color rgba="1 0 0 1" />
  79.             </material>
  80.         </visual>
  81.         <collision>
  82.             <origin xyx="0 0 0" rpy="0 0 0" />
  83.             <geometry>
  84.                 <mesh filename="${ModelPathHead}link_3.stl" />
  85.             </geometry>
  86.         </collision>
  87.         <inertial>
  88.             <mass value="1" />
  89.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  90.         </inertial>
  91.     </link>
  92.  
  93.     <link name="link_4">
  94.         <visual>
  95.             <geometry>
  96.                 <mesh filename="${ModelPathHead}link_4.stl" />
  97.             </geometry>
  98.             <origin xyz="0 0 0" rpy="0 0 0" />
  99.             <material name="Red">
  100.                 <color rgba="1 0 0 1" />
  101.             </material>
  102.         </visual>
  103.         <collision>
  104.             <origin xyx="0 0 0" rpy="0 0 0" />
  105.             <geometry>
  106.                 <mesh filename="${ModelPathHead}link_4.stl" />
  107.             </geometry>
  108.         </collision>
  109.         <inertial>
  110.             <mass value="1" />
  111.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  112.         </inertial>
  113.     </link>
  114.  
  115.     <link name="link_5">
  116.         <visual>
  117.             <geometry>
  118.                 <mesh filename="${ModelPathHead}link_5.stl" />
  119.             </geometry>
  120.             <origin xyz="0 0 0" rpy="0 0 0" />
  121.             <material name="Red">
  122.                 <color rgba="1 0 0 1" />
  123.             </material>
  124.         </visual>
  125.         <collision>
  126.             <origin xyx="0 0 0" rpy="0 0 0" />
  127.             <geometry>
  128.                 <mesh filename="${ModelPathHead}link_5.stl" />
  129.             </geometry>
  130.         </collision>
  131.         <inertial>
  132.             <mass value="1" />
  133.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  134.         </inertial>
  135.     </link>
  136.  
  137.     <link name="link_6">
  138.         <visual>
  139.             <geometry>
  140.                 <mesh filename="${ModelPathHead}link_6.stl" />
  141.             </geometry>
  142.             <origin xyz="0 0 0" rpy="0 0 0" />
  143.             <material name="Red">
  144.                 <color rgba="1 0 0 1" />
  145.             </material>
  146.         </visual>
  147.         <collision>
  148.             <origin xyx="0 0 0" rpy="0 0 0" />
  149.             <geometry>
  150.                 <mesh filename="${ModelPathHead}link_6.stl" />
  151.             </geometry>
  152.         </collision>
  153.         <inertial>
  154.             <mass value="1" />
  155.             <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
  156.         </inertial>
  157.     </link>
  158.  
  159.     <!-- <link name="world"/> -->
  160.  
  161.     <joint name="base_joint" type="fixed">
  162.         <parent link="world" />
  163.         <child link="base_link" />
  164.         <origin xyz="0 0 0" rpy="0 0 0" />
  165.         <axis xyz="0 0 1" />
  166.     </joint>
  167.  
  168.   <!-- joints - main serial chain -->
  169.     <joint name="Joint_1" type="revolute">
  170.         <parent link="base_link" />
  171.         <child link="link_1" />
  172.         <origin xyz="0 0 0.061584" rpy="0 0 0" />
  173.         <axis xyz="0 0 1" />
  174.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  175.     </joint>
  176.  
  177.     <joint name="Joint_2" type="revolute">
  178.         <parent link="link_1" />
  179.         <child link="link_2" />
  180.         <origin xyz="-0.101717 0 0.182284" rpy="${-pi/2} -${pi/3} ${pi/2}" />
  181.         <axis xyz="0 0 1" />
  182.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  183.     </joint>
  184.  
  185.     <joint name="Joint_3" type="revolute">
  186.         <parent link="link_2" />
  187.         <child link="link_3" />
  188.         <origin xyz="0.685682 0 0.041861" rpy="0 ${pi} ${pi+pi/2}" />
  189.         <axis xyz="0 0 1" />
  190.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  191.     </joint>
  192.  
  193.     <joint name="Joint_4" type="revolute">
  194.         <parent link="link_3" />
  195.         <child link="link_4" />
  196.         <origin xyz="0.518777 0 0.067458" rpy="0 ${pi} ${pi+pi/6}" />
  197.         <axis xyz="0 0 1" />
  198.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  199.     </joint>
  200.  
  201.     <joint name="Joint_5" type="revolute">
  202.         <parent link="link_4" />
  203.         <child link="link_5" />
  204.         <origin xyz="0.112654 0 0.110903" rpy="${pi/2} ${pi} ${pi/2}" />
  205.         <axis xyz="0 0 1" />
  206.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  207.     </joint>
  208.  
  209.     <joint name="Joint_6" type="revolute">
  210.         <parent link="link_5" />
  211.         <child link="link_6" />
  212.         <origin xyz="-0.085976 0 0.133436" rpy="0 ${-pi/2} 0" />
  213.         <axis xyz="0 0 1" />
  214.         <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
  215.     </joint>
  216.  
  217.  
  218.     <gazebo reference="base_link">
  219.         <material>Gazebo/Purple</material>
  220.         <self_collide>false</self_collide>
  221.         <gravity>false</gravity>
  222.     </gazebo>
  223.  
  224.     <gazebo reference="link_1">
  225.         <material>Gazebo/Purple</material>
  226.         <self_collide>false</self_collide>
  227.         <gravity>false</gravity>
  228.     </gazebo>
  229.  
  230.     <gazebo reference="link_2">
  231.         <material>Gazebo/Purple</material>
  232.         <self_collide>false</self_collide>
  233.         <gravity>false</gravity>
  234.     </gazebo>
  235.  
  236.     <gazebo reference="link_3">
  237.         <material>Gazebo/Purple</material>
  238.         <self_collide>false</self_collide>
  239.         <gravity>false</gravity>
  240.     </gazebo>
  241.  
  242.     <gazebo reference="link_4">
  243.         <material>Gazebo/Purple</material>
  244.         <self_collide>false</self_collide>
  245.         <gravity>false</gravity>
  246.     </gazebo>
  247.  
  248.     <gazebo reference="link_5">
  249.         <material>Gazebo/Purple</material>
  250.         <self_collide>false</self_collide>
  251.         <gravity>false</gravity>
  252.     </gazebo>
  253.  
  254.     <gazebo reference="link_6">
  255.         <material>Gazebo/Purple</material>
  256.         <self_collide>false</self_collide>
  257.         <gravity>false</gravity>
  258.     </gazebo>
  259.  
  260. </robot>
  1. <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="Staubli">
  2.     <xacro:property name="UrdfPathHead" value="urdf文件路径" />
  3.     <!-- <xacro:include filename="${UrdfPathHead}robot.urdf.xacro" /> -->
  4.     <link name="world" />
  5.     <xacro:include filename="${UrdfPathHead}r6bot.urdf.xacro" />
  6.  
  7.     <xacro:include filename="${UrdfPathHead}robot_ros_control.urdf.xacro" />
  8. </robot>

3.编写控制文件(xacro)

  1. <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="Staubli" >
  2.     <ros2_control name="GazeboSystem" type="system">
  3.         <hardware>
  4.           <plugin>gazebo_ros2_control/GazeboSystem</plugin>
  5.         </hardware>
  6.         
  7.         <joint name="Joint_1">
  8.             <command_interface name="position" />
  9.             <state_interface name="position"/>
  10.             <state_interface name="velocity"/>
  11.         </joint>
  12.  
  13.         <joint name="Joint_2">
  14.             <command_interface name="position" />
  15.             <state_interface name="position"/>
  16.             <state_interface name="velocity"/>
  17.         </joint>
  18.  
  19.         <joint name="Joint_3">
  20.             <command_interface name="position" />
  21.             <state_interface name="position"/>
  22.             <state_interface name="velocity"/>
  23.         </joint>
  24.  
  25.         <joint name="Joint_4">
  26.             <command_interface name="position" />
  27.             <state_interface name="position"/>
  28.             <state_interface name="velocity"/>
  29.         </joint>
  30.  
  31.         <joint name="Joint_5">
  32.             <command_interface name="position" />
  33.             <state_interface name="position"/>
  34.             <state_interface name="velocity"/>
  35.         </joint>
  36.  
  37.         <joint name="Joint_6">
  38.             <command_interface name="position" />
  39.             <state_interface name="position"/>
  40.             <state_interface name="velocity"/>
  41.         </joint>
  42.     </ros2_control>
  43.  
  44.     <gazebo>
  45.       <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
  46.         <!-- 这个是编写的控制器文件的路径 -->
  47.           <parameters>$(find gazebo_robot)/config/gazebo_robot_control.yaml</parameters>
  48.       </plugin>
  49.   </gazebo>
  50.  
  51. </robot>
  52.

4. 定义控制器

  1. controller_manager:
  2.   ros__parameters:
  3.     update_rate: 100
  4.     sue_sim_time: true
  5.  
  6.     arm_controller:
  7.       type: position_controllers/JointGroupPositionController // ros2_control 里面定义的控制器
  8.  
  9.     joint_state_broadcaster:
  10.       type: joint_state_broadcaster/JointStateBroadcaster   //固定的
  11.  
  12. arm_controller:
  13.   ros__parameters:
  14.     joints:
  15.       - Joint_1  // 注意和机械臂的urdf中定义的关节角保持一致
  16.       - Joint_2
  17.       - Joint_3
  18.       - Joint_4
  19.       - Joint_5
  20.       - Joint_6
  21.     command_interface:
  22.       - position
  23.  
  24.     state_interface:
  25.       - position
  26.       - velocity

5.编写launch文件

  1. import os
  2. from launch import LaunchDescription
  3. from launch.actions import DeclareLaunchArgument, ExecuteProcess,RegisterEventHandler
  4. from launch_ros.substitutions import FindPackageShare
  5. from launch_ros.actions import Node
  6. from launch.substitutions import Command
  7. from launch.event_handlers import OnProcessExit
  8. import launch.actions
  9. import xacro
  10.  
  11.  
  12.  
  13. def generate_launch_description():
  14.     robot_name_in_model ="r6bot"
  15.     package_name='gazebo_robot'
  16.     urdf_name = "r6bot_main.urdf.xacro"
  17.     ld = LaunchDescription()
  18.     #找到自己的ros包
  19.     pkg_share = FindPackageShare(package='gazebo_robot').find('gazebo_robot')
  20.     urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')
  21.  
  22.     start_gazebo_cmd =  ExecuteProcess(
  23.         cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
  24.         output='screen')
  25.     
  26.     doc = xacro.parse(open(urdf_model_path))
  27.     xacro.process_doc(doc)
  28.     params = {'robot_description': doc.toxml()}
  29.     
  30.  
  31.     node_robot_state_publisher = Node(
  32.         package='robot_state_publisher',
  33.         executable='robot_state_publisher',
  34.         output='screen',
  35.         parameters=[params]
  36.     )
  37.  
  38.     # Launch the robot
  39.     spawn_entity_cmd = Node(
  40.         package='gazebo_ros', 
  41.         executable='spawn_entity.py',
  42.         arguments=['-topic', 'robot_description','-entity', 'robot_gazebo'], output='screen')
  43.     
  44.  
  45. #将自己定义的环境模型，写成类似与gazebo自带的模型文件形式，注意有几个文件定义。
  46. #自己如何撰写gazebo模型，可以参考网上的教程 最后需要export路径，这里用python实现
  47.     model_path = 自定义的模型文件的路径"
  49.     if "GAZEBO_MODEL_PATH" in os.environ:
  50.         os.environ["GAZEBO_MODEL_PATH"] += ":" + model_path
  51.     else:
  52.         os.environ["GAZEBO_MODEL_PATH"] = model_path
  53.     EnvModel = Node(
  54.         package='gazebo_ros', 
  55.         executable='spawn_entity.py',
  56. #'.sdf文件路径'是自定义模型中有个sdf文件的，直接传入这个文件的路径。
  57.         arguments=['-entity', 'demo', '-file', '.sdf文件路径'], output='screen')
  58.     
  59.     
  60.     joint_broad_spawner = Node(
  61.         package="controller_manager",
  62.         executable="spawner",
  63.         arguments=["joint_state_broadcaster"]
  64.     )
  66.     arm_broad_spawner=Node(
  67.         package="controller_manager",
  68.         executable="spawner",
  69.         arguments=["arm_controller"]
  70.     )
  72.     ld.add_action(start_gazebo_cmd)
  73.     ld.add_action(node_robot_state_publisher)
  74.     ld.add_action(spawn_entity_cmd)
  75.     ld.add_action(EnvModel)
  76.     ld.add_action(joint_broad_spawner)
  77.     ld.add_action(arm_broad_spawner)
  79.     return ld

后续把这部分实际代码分享到git中。路径：Ros2_Control: Ros2与gazebo联合仿真

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/运维做开发/article/detail/746979
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号