机器人——ROS程序设计学习笔记（chapter7）

川长思鸟来 2022-05-17 00:34 327阅读 0赞

书籍名称：《Learning ROS for RoboticsProgramming - Second Edition》Chapter7

参考链接：https://www.cnblogs.com/jinee/p/5047021.html

  1. 一定要记住它使用的是右手坐标系：
    x正方向朝左， y正方向向内， z轴正方向朝上
  2. 构建树结构， 即写link和joint

1.几何模型

  1. <?xml version ="1.0"?>
  2. <robot name = "text1">
  4.   <!--body-->
  5.   <link name = "base_link">
  6.     <visual>
  7.       <geometry>
  8.         <cylinder length = "0.6" radius = "0.2"/>  
  9.         <!--圆柱体 长度0.6 半径0.2-->
  10.       </geometry>
  11.       <material name = "blue">
  12.        <color rgba = "0 0 0.8 1"/>
  13.       </material>
  14.     </visual>
  15.   </link>
  16. <!--right leg-->
  17.   <link name = "right_leg">
  18.     <visual>
  19.       <geometry>
  20.         <box size = "0.6 .2 .1"/>
  21.        <!--六面体长宽高分别为0.6 0.2 0.1-->
  22.       </geometry>
  23.       <material name = "white">
  24.        <color rgba = "1 1 1 1"/>
  25.       </material>
  26.     </visual>  
  27.   </link>
  29.   <joint name = "base_to_right_leg" type = "fixed">
  30.     <parent link = "base_link"/>
  31.     <child link = "right_leg"/> 
  32.   </joint>
  34. </robot>

70

中添加几何体信息

中添加材质 颜色（rgba格式）

2.调整位置，添加另一条腿

  1. <!--right leg-->
  2.   <link name = "right_leg">
  3.     <visual>
  4.       <geometry>
  5.         <box size = "0.6 .2 .1"/>
  6.       </geometry>
  7.       <material name = "white">
  8.        <color rgba = "1 1 1 1"/>
  9.       </material>
  10.       <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
  11.     </visual>  
  12.   </link>
  13.   <joint name = "base_to_right_leg" type = "fixed">
  14.     <parent link = "base_link"/>
  15.     <child link = "right_leg"/>
  16.     <origin xyz = "0.22 0 0.25"> 
  17.   </joint>
  19. <!--left leg-->
  20.   <link name = "left_leg">
  21.     <visual>
  22.       <geometry>
  23.         <box size = "0.6 .2 .1"/>
  24.       </geometry>
  25.       <material name = "white">
  26.        <color rgba = "1 1 1 1"/>
  27.       </material>
  28.       <origin rpy = "0 1.5705 0" xyz = "-0.22 0 -0.05"/>
  29.     </visual>  
  30.   </link>
  31.   <joint name = "base_to_left_leg" type = "fixed">
  32.     <parent link = "base_link"/>
  33.     <child link = "left_leg"/> 
  34.     <origin xyz = "-0.22 0 0.25">
  35.   </joint>

70 1

每个link的参考坐标系都在它的底部，并与关节的参考坐标系正交，为了添加尺寸，需要指定偏移从一个link到它的关节的子link, 这通过添加origin到每个节点解决。
origin表示的是关节相对于父关节的距离和旋转， xyz和rpy（1.5705:90°）

3.添加剩余部位

调整一下，全部代码：

  1. <?xml version ="1.0"?>
  2. <robot name = "text1">
  4.   <!--body-->
  5.   <link name = "base_link">
  6.     <visual>
  7.       <geometry>
  8.         <cylinder length = "0.6" radius = "0.2"/>
  9.       </geometry>
  10.       <material name = "blue">
  11.        <color rgba = "0 0 0.8 1"/>
  12.       </material>
  13.     </visual>
  14.   </link>
  16. <!--right leg-->
  17.   <link name = "right_leg">
  18.     <visual>
  19.       <geometry>
  20.         <box size = "0.6 .2 .1"/>
  21.       </geometry>
  22.       <material name = "white">
  23.        <color rgba = "1 1 1 1"/>
  24.       </material>
  25.       <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
  26.     </visual>  
  27.   </link>
  28.   <joint name = "base_to_right_leg" type = "fixed">
  29.     <parent link = "base_link"/>
  30.     <child link = "right_leg"/> 
  31.     <origin xyz = "0.22 0 .25"/>
  32.   </joint>
  34. <!--left leg-->
  35.   <link name = "left_leg">
  36.     <visual>
  37.       <geometry>
  38.         <box size = "0.6 .2 .1"/>
  39.       </geometry>
  40.       <material name = "white">
  41.        <color rgba = "1 1 1 1"/>
  42.       </material>
  43.       <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
  44.     </visual>  
  45.   </link>
  46.   <joint name = "base_to_left_leg" type = "fixed">
  47.     <parent link = "base_link"/>
  48.     <child link = "left_leg"/>
  49.     <origin xyz = "-0.22 0 .25"/> 
  50.   </joint>
  51. <!--head-->
  52.  <link name = "head">
  53.   <visual>
  54.    <geometry>
  55.     <sphere radius = "0.2"/>
  56.    </geometry>
  57.    <material name  = "white"/>
  58.   </visual>
  59.  </link>
  60.  <joint name="base_to_head" type="fixed">
  61.    <parent link="base_link"/>
  62.    <child link="head"/>
  63.    <origin xyz="0 0 0.3"/>
  64.   </joint>
  65. <!--right foot-->
  66.  <link name = "right_foot">
  67.   <visual>
  68.     <geometry>
  69.      <box size = "0.1 .4 .1"/>
  70.     </geometry>
  71.     <material name = "yellow">
  72.      <color rgba = "1 1 0 1"/>
  73.     </material>
  74.   </visual>
  75.  </link>
  76.  <joint name = "leg_to_right_foot" type = "fixed">
  77.   <parent link = "right_leg"/>
  78.   <child link = "right_foot"/>
  79.   <origin xyz = "0 0 -0.65"/>
  80.  </joint>
  81.  <!--left foot--> 
  82.  <link name = "left_foot">
  83.   <visual>
  84.     <geometry>
  85.      <box size = "0.1 .4 .1"/>
  86.     </geometry>
  87.     <material name = "yellow">
  88.      <color rgba = "1 1 0 1"/>
  89.     </material>
  90.   </visual>
  91.  </link>
  92.  <joint name = "leg_to_left_foot" type = "fixed">
  93.   <parent link = "left_leg"/>
  94.   <child link = "left_foot"/>
  95.   <origin xyz = "0 0 -0.65"/>
  96.  </joint>
  97. <!--right foot front wheel-->
  98.  <link name = "right_front_wheel">
  99.   <visual>
  100.    <geometry>
  101.     <cylinder length= "0.08" radius = "0.05"/>
  102.    </geometry>
  103.    <material name = "black">
  104.     <color rgba = "0 0 0 1"/>
  105.    </material>
  106.   </visual>
  107.  </link>
  108.  <joint name  = "right_foot_to_front_wheel" type = "fixed">
  109.   <parent link ="right_foot"/>
  110.   <child link = "right_front_wheel"/>
  111.   <origin xyz = "0 0.14 -0.05" rpy = "0 1.5705 0"/>
  112.  </joint>
  113. <!--right foot back wheel-->
  114.  <link name = "right_back_wheel">
  115.   <visual>
  116.    <geometry>
  117.     <cylinder length= "0.08" radius = "0.05"/>
  118.    </geometry>
  119.    <material name = "black">
  120.     <color rgba = "0 0 0 1"/>
  121.    </material>
  122.   </visual>
  123.  </link>
  124.  <joint name  = "right_foot_to_back_wheel" type = "fixed">
  125.   <parent link ="right_foot"/>
  126.   <child link = "right_back_wheel"/>
  127.   <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
  128.  </joint>
  129. <!--left foot front wheel-->
  130.  <link name = "left_front_wheel">
  131.   <visual>
  132.    <geometry>
  133.     <cylinder length= "0.08" radius = "0.05"/>
  134.    </geometry>
  135.    <material name = "black">
  136.     <color rgba = "0 0 0 1"/>
  137.    </material>
  138.   </visual>
  139.  </link>
  140.  <joint name  = "left_foot_to_front_wheel" type = "fixed">
  141.   <parent link ="left_foot"/>
  142.   <child link = "left_front_wheel"/>
  143.   <origin xyz = "0 0.14 -0.05" rpy =  "0 1.5705 0"/>
  144.  </joint>
  145. <!--left foot back wheel-->
  146.  <link name = "left_back_wheel">
  147.   <visual>
  148.    <geometry>
  149.     <cylinder length= "0.08" radius = "0.05"/>
  150.    </geometry>
  151.    <material name = "black">
  152.     <color rgba = "0 0 0 1"/>
  153.    </material>
  154.   </visual>
  155.  </link>
  156.  <joint name  = "left_foot_to_back_wheel" type = "fixed">
  157.   <parent link ="left_foot"/>
  158.   <child link = "left_back_wheel"/>
  159.   <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
  160.  </joint>
  161. </robot>

70 2

4.添加碰撞collision

碰撞只要将link包裹住就可以了

  1. <!--body-->
  2.   <link name = "base_link">
  3.     <visual>
  4.       <geometry>
  5.         <cylinder length = "0.6" radius = "0.2"/>
  6.       </geometry>
  7.       <material name = "blue">
  8.        <color rgba = "0 0 0.8 1"/>
  9.       </material>
  10.     </visual>
  11.     <collision>
  12.      <geometry>
  13.       <cylinder length = "0.6" radius = "0.2"/>
  14.      </geometry>
  15.     </collision>
  16.   </link>

5.xacro

使用宏，简化代码，顺便添加inertial

这里使用了三个宏：代码如下(中间代码省略)

  1. <?xml version ="1.0"?>
  2. <robot name = "text1" xmlns:xacro="http://www.ros.org/wiki/xacro">
  4.  <xacro:property name="wheel_length" value="0.08" />
  5.  <xacro:property name="wheel_radius" value="0.05" />
  6.  <xacro:macro name="default_inertial" params="mass">
  7.   <inertial>
  8.    <mass value="${mass}" />
  9.    <inertia ixx="1.0" ixy="0.0" ixz="0.0"
  10.             iyy="1.0" iyz="0.0"
  11.             izz="1.0" />
  12.    </inertial>
  13.  </xacro:macro>
  14. .
  15. .
  16. .
  17. <!--left foot back wheel-->
  18.  <link name = "left_back_wheel">
  19.   <visual>
  20.    <geometry>
  21.     <cylinder length= "${wheel_length}" radius = "${wheel_radius}"/>
  22.    </geometry>
  23.    <material name = "black">
  24.     <color rgba = "0 0 0 1"/>
  25.    </material>
  26.   </visual>
  27.   <collision>
  28.    <geometry>
  29.     <cylinder length= "${wheel_length}" radius = "${wheel_radius}"/>
  30.    </geometry>
  31.   </collision>
  32.   <xacro:default_inertial mass = "1"/>
  33.  </link>
  34.  <joint name  = "left_foot_to_back_wheel" type = "continuous">
  35.   <parent link ="left_foot"/>
  36.   <child link = "left_back_wheel"/>
  37.   <axis xyz = "0 0 1"/>
  38.   <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
  39.  </joint>
  41. </robot>

6.让机器人动起来

让机器人走圆

cpp如下：

  1. #include <string>
  2. #include <ros/ros.h>
  3. #include <sensor_msgs/JointState.h>
  4. #include <tf/transform_broadcaster.h>
  6. int main(int argc, char** argv) {
  7.     ros::init(argc, argv, "state_publisher_text");
  8.     ros::NodeHandle n;
  9.     ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
  10.     tf::TransformBroadcaster broadcaster;
  11.     ros::Rate loop_rate(30);
  13.     const double degree = M_PI/180;
  15.     // robot state
  16.     double inc= 0.005,wheel_1_inc= -0.05,wheel_2_inc= -0.05,wheel_3_inc= -0.07,wheel_4_inc= -0.07;    //轮子向后转，机器人向前移动，所以这里是负
  17.     double angle= 0,base_head = 0,wheel_1= 0,wheel_2= 0,wheel_3= 0,wheel_4= 0;
  18.     // message declarations
  19.     geometry_msgs::TransformStamped odom_trans;
  20.     sensor_msgs::JointState joint_state;
  21.     odom_trans.header.frame_id = "odom";
  22.     odom_trans.child_frame_id = "base_link";
  24.     while (ros::ok()) {
  25.         //update joint_state
  26.         joint_state.header.stamp = ros::Time::now();
  27.         joint_state.name.resize(9);
  28.         joint_state.position.resize(9);
  29.         joint_state.name[0] ="base_to_head";
  30.         joint_state.position[0] = base_head;
  31.         joint_state.name[1] ="base_to_right_leg";
  32.         joint_state.position[1] = 0;
  33.         joint_state.name[2] ="base_to_left_leg";
  34.         joint_state.position[2] = 0;
  35.         joint_state.name[3] ="right_leg_to_right_foot";
  36.         joint_state.position[3] = 0;
  37.         joint_state.name[4] ="left_leg_to_left_foot";
  38.         joint_state.position[4] = 0;
  39.         joint_state.name[5] ="right_foot_to_front_wheel";
  40.         joint_state.position[5] = wheel_1;
  41.         joint_state.name[6] ="right_foot_to_back_wheel";
  42.         joint_state.position[6] = wheel_2;
  43.         joint_state.name[7] ="left_foot_to_front_wheel";
  44.         joint_state.position[7] = wheel_3;
  45.         joint_state.name[8] ="left_foot_to_back_wheel";
  46.         joint_state.position[8] = wheel_4;
  48.         // update transform
  49.         // (moving in a circle with radius)
  50.         //改变二维平面的x,y的坐标，z轴垂直于xOy，故z的坐标是0
  51.         odom_trans.header.stamp = ros::Time::now();
  52.         odom_trans.transform.translation.x = cos(angle);
  53.         odom_trans.transform.translation.y = sin(angle);
  54.         odom_trans.transform.translation.z = 0.0;
  55.         odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
  57.         //send the joint state and transform
  58.         joint_pub.publish(joint_state);
  59.         broadcaster.sendTransform(odom_trans);
  62.         // Create new robot state
  63.         base_head += inc;
  64.         if(base_head< -2.0||base_head> 2.0) base_head *= -1;
  65.         wheel_1 += wheel_1_inc;
  66.         if(wheel_1< -2.0||wheel_1> 2.0) wheel_1 *= -1;
  67.         wheel_2 += wheel_2_inc;
  68.         if(wheel_2< -2.0||wheel_2> 2.0) wheel_2 *= -1;
  69.         wheel_3 += wheel_3_inc;
  70.         if(wheel_3< -2.0||wheel_3> 2.0) wheel_3 *= -1;
  71.         wheel_4 += wheel_4_inc;
  72.         if(wheel_4< -2.0||wheel_4> 2.0) wheel_4 *= -1;
  74.         angle += degree/4;
  76.         // This will adjust as needed per iteration
  77.         loop_rate.sleep();
  78.     }
  81.     return 0;
  82. }

70 3

Chapter7部分错误与解决方法：

P275：
问题：执行以下命令时，出现错误提示：

  1. $ check_urdf robot1.urdf
  2. Error:   Error document empty.
  3.         at line 72 in /build/buildd/urdfdom 0.2.10+dfsg/urdf_parser/src/model.cpp
  4. ERROR: Model Parsing the xml failed

解决：在.urdf前添加路径

  1. check_urdf /home/<user_name>/ROS/catkin_ws/src/learning_urdf/urdf/robot1.urdf

P276：
问题：执行$ roslaunch …命令后没有3D模型
解决：在<?xml version=”1.0”?>代码段，在倒数第二行添加，注意结尾是 urdf.rviz，否则会报错。

  1. <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />

P277：
问题：在urdf中添加package：

  1. <mesh filename = "package://pr2_description/meshes/gripper_v0/l_finger.dae"/>

编译时一直报错，提示没有找到资源
解决：我将含有.dae的文件夹（meshes）放在”robot1_description“（功能包）中，相对路径改为

  1. <mesh filename = "package://robot1_description/meshes/gripper_v0/l_finger.dae"/>

加载成功。另外urdf中不能含有中文

发表评论

表情：
评论列表 （有 0 条评论，327人围观）

还没有评论，来说两句吧...

相关阅读

    相关 ROS基础学习笔记1

    ROS开发基础部分： 基本文件介绍 1.CMakeLists.txt 规定了catkin编译的规则 例如：源文件，依赖项，目标文件，会总动生成命令模板

    小咪咪小咪咪/ 0 赞/ 136 阅读