Gausst松鼠会

这个屌丝很懒，什么也没留下！

热门标签

ROS机器人开发

作者：Gausst松鼠会 | 2024-04-10 21:19:25

踩

ros机器人开发

文章目录

第一章 ROS基础
**第二章构建模拟两轮ROS机器人**
第三章 TurtleBot机器人操控
第四章 Turtlebot机器人导航(记录，没运行过)
第五章构建模拟的机器人手臂
第六章机器人手臂摇摆的关节控制-Baxter

第一章 ROS基础

1.ROS安装

http://wiki.ros.org/kinetic/Installtion/Source

http://wiki.ros.org/cn/noetic/Installation/Ubuntu

https://blog.csdn.net/qq_44339029/article/details/120579608?spm=1001.2101.3001.6650.1&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-1-120579608-blog-106899359.pc_relevant_baidufeatures_v6&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-1-120579608-blog-106899359.pc_relevant_baidufeatures_v6&utm_relevant_index=2

2. 安装rosinstall

$ sudo apt-get install python-rosinstall

3.创建catkin工作空间：

~$ mkdir -p ~/catkin_ws/src
~$ cd ~/catkin_ws/src
~/catkin_ws/src$ catkin_init_workspace 
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
1
2
3
4
5

4.加载源文件setup.bash

~/catkin_ws$ source ~/catkin_ws/devel/setup.bash 
~/catkin_ws$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
1
2

查看工作空间： ~/catkin_ws$ echo $ROS_PACKAGE_PATH

5.ROS功能包

http://wiki.ros.org/Packages

命令：

rospack 用于获取一个功能包的信息

$ rospack help | less
roscd 切换ROS目录

$ roscd turtlesim
rosls 列出一个功能包目录下的目录与文件清单

$ rosls turtlesim
rospack find 返回指定名称的功能包的路径

$ rospack find turtlesim

6.ros节点、话题、消息

rosnode list 列出运行的节点
rostopic list 列出与运行中的节点相关的话题

使用帮助 rosnode -h

rosnode list -h

7.ROS机器人模拟程序

rosrun 在功能包里找到并启动所需要的节点
rosmsg 显示相关消息的信息
rosservice 显示节点的运行时间，并且在请求/响应模式下在节点间传递数据
rosparam 获取或者设置节点参数值

启动ROS节点管理器	roscore
启动节点	rosrun turtlesim turtlesim_node
列出活动节点	rosnode list
查看/turtlesim节点的发布话题、订阅服务和服务相关内容	rosnode info /turtlesim
查看/turtlesim节点的话题	rostopic list
查看话题的话题类型	rostopic type /turtle1/color_sensor
查看消息类型	rosmsg list
查看ROS消息类型的具体内容	rosmsg show turtlesim/Color
查看乌龟背景颜色的数值	rostopic echo /turtle1/color_sensor

8.控制乌龟运动

启动节点管理器，并启动turtlesim_node

$ roscore
$ rosrun turtlesim turtlesim_node
1
2

turtlesim_node订阅/turtle1/cmd_vel话题，可以通过/turtle1/cmd_vel发送消息来控制乌龟运动
确定该话题的消息类型

$ rostopic type /turtle1/cmd_vel

消息类型为Twist，来自geometry_msgs功能包 geometry_msgs/Twist
控制乌龟做圆周运动

$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0,0.0,0.0]' '[0.0 0.0 1.8]'

控制乌龟以2m/s的线速度和1.8弧度/s的角速度做圆周运动

数值参量参考 http://wiki.ros.org/ROS/YAMLCommandLine

键盘控制rosrun turtlesim turtle_teleop_key
turtlesim的参数服务器

rosparam list 列出/turtlesim节点的参数列表

rosparam get / 获得整个参数服务器上所有参数的数值
将乌龟背景的颜色改为红色 rosparam set background_b 0

$ rosparam set background_g 0
$ rosparam set background_r 0
$ rosservice call /clear
1
2
3

控制乌龟移动的ROS服务

通过/turtle1/pose 话题获得乌龟的位置信息

$ rostopic type /turtle1/pose
$ rosmsg show turtlesim/Pose
1
2

获得乌龟的位置坐标、方位角度和速度参数

$ rostopic echo /turtle1/pose

使用rossevice远程控制端口移动乌龟

$ rosservice call /turtle1/teleport_absolute 1 1 0

更多命令 http://wiki.ros.org/ROS/CommandLineTools

第二章构建模拟两轮ROS机器人

rviz是ROS可视化的缩写，查看模拟的机器人模型、来自机器人传感器的传感器日志和重放已记录的传感器信息

1.安装rviz

http://wiki.ros.org/rviz/Tro-ubleshooting

ROS论坛的问题页面http://answers.ros.org/questions/

检查是否安装

~$ roscore
~$ rosrun rviz rviz
1
2

安装$ sudo apt-get install ros-kinetic-rviz

用户指南http://wiki.ros.org/rviz/UserGuide

2.生成并构建ROS功能包

第一章中的创建catkin工作空间：

~$ mkdir -p ~/catkin_ws/src
~$ cd ~/catkin_ws/src
~/catkin_ws/src$ catkin_init_workspace 
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source ~/catkin_ws/devel/setup.bash 
~/catkin_ws$ echo "source ~/catkin_ws/src:opt/ros/noetic/share"

~/catkin_ws$ echo $ROS_PACKAGE_PATH
/home/zrf/catkin_ws/src:/opt/ros/noetic/share
1
2
3
4
5
6
7
8
9
10

切换到catkin工作空间	~/catkin_ws$ cd src/
生成ROS功能包ros_robotics	~/catkin_ws/src$ catkin_create_pkg ros_robotics
	~/catkin_ws/src$ cd …
	~/catkin_ws$ catkin_make

例 catkin_create_pkg <package_name> [depend1] [depeng2]

3. 构建机器人URDF

URDF是一种特殊定义的XML格式的文件，专门用来对机器人组建级别进行抽象的模型描述，Xacro是XML宏命令语言，能够帮助用户减少文件中的重复信息。

在ros_robotics功能包下生成/urdf目录

~/catkin_ws$ cd src/
~/catkin_ws/src$ cd ros_robotics/
~/catkin_ws/src/ros_robotics$ mkdir urdf
~/catkin_ws/src/ros_robotics$ cd urdf/
1
2
3
4

描述机器人模型，两个基本的URDF组件定义的一个树状结构。

link组件描述了刚体的物理学属性（维度、初始位置、颜色等）

joint组件描述了连接的运动学以及动态属性（连接的连接杆、关节类型、旋转轴、摩擦力和阻尼的合力等）

~/catkin_ws/src/ros_robotics/urdf/dd_robotic.urdf:

<?xml version='1.0'?>
<robot name="dd_robot">

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
          <box size="0.5 0.5 0.25"/>
      </geometry>
    </visual>
  </link>

</robot>
1
2
3
4
5
6
7
8
9
10
11
12
13
14

包含一个link组件，该组件的可视化外形是长宽高为0.5m，0.5m，0.25m的矩形盒子，盒子中心位于初始原点（0，0，0）处，并且在滚转（pitch）、俯仰（pitch）、偏航（yaw）三个轴向上均不存在旋转。连接杆组件命名为base_link。

roslaunch:ROS提供的一个启动工具。简化启动多个ROS节点，简化ROS参数服务器进行参数设置

~/catkin_ws/src/ros_robotics/launch/ddrobot_rviz.launch:

<?xml version='1.0'?>
<launch>
   <!-- values passed by command line input -->     
   <arg name="model" />
   <arg name="gui" default="False" />

   <!-- set these parameters on Parameter Server -->
   <param name="robot_description" textfile="$(find ros_robotics)/urdf/$(arg model)" />
   <param name="use_gui" value="$(arg gui)"/>

   <!-- Start 3 nodes: joint_state_publisher, robot_state_publisher and rviz -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotics)/urdf.rviz" required="true" />
   <!-- (required = "true") if rviz dies, entire roslaunch will be killed -->
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

在Rviz中查看机器人模型：

(1) 命令行中设置的模型加载到参数服务器

(2) 启动发布了JointState和转换的节点

(3) 启动使用urdf.rviz文件配置的Rviz

$ roslaunch ros_robotics ddrobot_rviz.launch model:=dd_robot.urdf

例 roslaunch <package_name> <file.launch>

机器人描述：robot_description是URDF文件中存储的ROS参数的名称，文件存储于参数服务器中。连接杆组件和关节组件的描述以及两者的连接方式也存储于机器人描述中。
添加轮子、添加小脚轮、添加颜色、添加碰撞属性、添加物理学属性。

~/catkin_ws/src/ros_robotics/urdf/dd_robotic6.urdf:

<?xml version='1.0'?>
<robot name="dd_robot">

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
          <box size="0.5 0.5 0.25"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0.5 1 1"/>
      </material>
    </visual>
    <!-- Base collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
          <box size="0.5 0.5 0.25"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="5"/>
      <inertia ixx="0.13" ixy="0.0" ixz="0.0" iyy="0.21" iyz="0.0" izz="0.13"/>
    </inertial>

    <!-- Caster -->
    <visual name="caster">
      <origin xyz="0.2 0 -0.125" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.05" />
      </geometry>
    </visual>
    <!-- Caster collision, mass and inertia -->
    <collision>
      <origin xyz="0.2 0 -0.125" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.05" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>

  </link>

  <!-- Right Wheel -->
  <link name="right_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
      <material name="darkgray">
        <color rgba=".2 .2 .2 1"/>
      </material>
    </visual>
    <!-- Right Wheel collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
    </inertial>

  </link>

  <!-- Right Wheel joint -->
  <joint name="joint_right_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin xyz="0 -0.30 0" rpy="0 0 0" /> 
    <axis xyz="0 1 0" />
  </joint>

  <!-- Left Wheel -->
  <link name="left_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
      <material name="darkgray">
        <color rgba=".2 .2 .2 1"/>
      </material>
    </visual>
    <!-- Left Wheel collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
    </inertial>
  </link>

  <!-- Left Wheel joint -->
  <joint name="joint_left_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin xyz="0 0.30 0" rpy="0 0 0" /> 
    <axis xyz="0 1 0" />
  </joint>

</robot>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

$ roslaunch ros_robotics ddrobot_rviz.launch model:=dd_robot6.urdf

移动轮子（启动GUI接口）

$ roslaunch ros_robotics ddrobot_rviz.launch model:=dd_robot6.urdf gui:=True

tf与robot_state_publisher：

节点robot_state_publisher订阅了JointState消息，并将机器人状态消息发布给tf坐标转换库，tf坐标转换库维护着系统中每个元素的坐标系随时间变化的关系。节点robot_state_publisher接收机器人的关节组件角度作为输入，计算出机器人连接杆组件的3D位姿，并将该位姿结果发布出去。

物理学属性参考http://en.wikipedia.org/wiki/List_of_moments_of_inertia

URDF工具：

安装工具 $ sudo apt-get install liburdfdom-tools
检查URDF文件的语法是否正确check_urdf:

~/catkin_ws/src/ros_robotics/urdf$ check_urdf dd_robot6.urdf

生成URDF文件对应的graphviz图表以及一个.pdf格式的图表文件urdf_to_graphiz:
~/catkin_ws/src/ros_robotics/urdf$ urdf_to_graphiz dd_robot6.urdf
打开.pdf文件 $ evince dd_robot.pdf

Gazebo

免费的开源机器人模拟环境

1.安装Gazebo

http://answers.gazebosim.org/questions/

ROS论坛的问题页面http://answers.ros.org/questions/

安装指南http://gazebosim.org/tutorials?cat=install

用户手册http://gazebosim.org/tutorials

urdf用户手册（连接杆组件和关节组件相关元素、属性的列表清单）https://classic.gazebosim.org/tutorials?tut=ros_urdf

检查是否安装

$ gazebo

终止Gazebo $ rosnode list

$ rosnode kill -a

使用roslaunch 启动Gazebo $ roslaunch gazebo_ros empty_world.launch

2.URDF修改

Gazebo支持的机器人模型文件格式为SDF,通过修改，gazebo能够自动地将URDF代码转换为SDF格式的机器人描述。

添加gazebo标签

<gazebo>标签必须要添加到URDF文件中。若<gazebo>标签没有和reference=“”属性一起使用的话，则意味着对应的<gazebo>元素将应用到整个机器人模型之中。
在gazebo中指定颜色

在Rviz中指定颜色的方法在gazebo中不适用，需要为每一个连接杆组件指定一个gazebo<material>标签。
gazebo中的<visual>和<collision>属性描述

~/catkin_ws/src/ros_robotics/urdf/dd_robotic.gezebo:

<?xml version='1.0'?>
<robot name="dd_robot">

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
          <box size="0.5 0.5 0.25"/>
      </geometry>
    </visual>
    <!-- Base collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
          <box size="0.5 0.5 0.25"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="5"/>
      <inertia ixx="0.13" ixy="0.0" ixz="0.0" iyy="0.21" iyz="0.0" izz="0.13"/>
    </inertial>

    <!-- Caster -->
    <visual name="caster">
      <origin xyz="0.2 0 -0.125" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.05" />
      </geometry>
    </visual>
    <!-- Caster collision, mass and inertia -->
    <collision>
      <origin xyz="0.2 0 -0.125" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.05" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>

  </link>

  <gazebo reference="base_link">
    <material>Gazebo/Blue</material>
    <pose>0 0 3 0 0 0</pose>
  </gazebo>

  <!-- Right Wheel -->
  <link name="right_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </visual>
    <!-- Right Wheel collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
    </inertial>

  </link>

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

  <!-- Right Wheel joint -->
  <joint name="joint_right_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin xyz="0 -0.30 0.025" rpy="0 0 0" /> 
    <axis xyz="0 1 0" />
  </joint>

  <!-- Left Wheel -->
  <link name="left_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </visual>
    <!-- Left Wheel collision, mass and inertia -->
    <collision>
      <origin xyz="0 0 0" rpy="1.570795 0 0" />
      <geometry>
          <cylinder length="0.1" radius="0.2" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
    </inertial>

  </link>

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

  <!-- Left Wheel joint -->
  <joint name="joint_left_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin xyz="0 0.30 0.025" rpy="0 0 0" /> 
    <axis xyz="0 1 0"/>
  </joint>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

gazebo模型验证：$ gz sdf -p dd_robot.gazebo

或 $ gzsdf -p $(rospack find ros_robotics)/urdf/dd_robot.gazebo

~/catkin_ws/src/ros_robotics/launch/ddrobot_gezebo.launch:

<launch>
  <!-- We resume the logic in gazebo_ros package 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 ros_robotics)/worlds/ddrobot.world"/>
   
    <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"/>

  </include>

  <!-- Spawn dd_robot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" output="screen"
     args="-file $(find ros_robotics)/urdf/dd_robot.gazebo -urdf -model ddrobot" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

由gazebo_ros功能包中的empty_world.launch修改而来，改变了world_name参数，变为了ddrobot.world。

基于URDF的dd_robot机器人模型使用来自gazebo_ros ROS节点的spawn_model服务启动于Gazebo中，若想要复用或者分享上述代码，建议在自己的ros_robotics功能包的package.xml文件中添加相应的依赖：
<exec_depend>gazebo_ros</exec_depend>

ddrobot.world环境模型文件包含一个地平面模型和两个圆锥形的建筑模型

~/catkin_ws/src/ros_robotics/world/ddrobot.world:

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://construction_cone</uri>
      <name>construction_cone</name>
      <pose>-3.0 0 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://construction_cone</uri>
      <name>construction_cone</name>
      <pose>3.0 0 0 0 0 0</pose>
    </include>
  </world>
</sdf>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

在gazebo中启动机器人模型ddrobot $ roslaunch ros_robotics ddrobot_gazebo.launch

SDF手册 http://sdformat.org/spec

3.MATLAB

机器人系统工具箱的ROS支持http://www.mathworks.com/hardware-support/robot-operating-system.html

示例http://www.mathworks.com/help/robotics/examples/get-started-with-ros.html

第三章 TurtleBot机器人操控

机器人运动控制相关http://kobuki.yujinrobot.com/about2/

TurtleBot维基百科https://wiki.ros.org/Robots/TurtleBot

TurtleBot相关内容参考手册http://learn.turtlebot.com/

ROS网络连接https://wiki.ros.org/ROS/NetworkSetup

https://wiki.ros.org/ROS/Tutorials/MultipleMachines

ROS_MASTER_URI、ROS_IP和ROS_HOSTNAME变量的相关描述https://wiki.ros.org/ROS/EnvironmentVariables

Python脚本语言 ROS客户端库 http://wiki.ros.org/rospy

用户手册http://wiki.ros.org/rospy_tutorials

http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(python)

rqt界面工具 http://wiki.ros.org/rqt/Plugins

TurtleBot3 http://turtlebot3.robotis.com/en/latest/

3D传感器工作原理https://www.youtube.com/watch?v=uq9SEJxZiUg

gmapping http://openslam.org/gmapping.html

Turtlebot3

mkdir -p ~/turtlebot3_ws/src
cd ~/turtlebot3_ws/src
catkin_init_workspace 
cd ..
catkin_make
cd turtlebot3_ws/src/

sudo apt-get install ros-noetic-joy ros-noetic-teleop-twist-joy ros-noetic-teleop-twist-keyboard ros-noetic-laser-proc ros-noetic-rgbd-launch ros-noetic-depthimage-to-laserscan ros-noetic-rosserial-arduino ros-noetic-rosserial-python ros-noetic-rosserial-server ros-noetic-rosserial-client ros-noetic-rosserial-msgs ros-noetic-amcl ros-noetic-map-server ros-noetic-move-base ros-noetic-urdf ros-noetic-xacro ros-noetic-compressed-image-transport ros-noetic-rqt-image-view ros-noetic-gmapping ros-noetic-navigation ros-noetic-interactive-markers

git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone  https://github.com/ROBOTIS-GIT/turtlebot3.git
cd turtlebot3_ws/
catkin_make
1
2
3
4
5
6
7
8
9
10
11
12
13
14

vim ~/.bashrc

export ROS_MASTER_URI=http://localhost:11311/
export ROS_HOSTNAME=localhost
export TURTLEBOT3_MODEL=burger
1
2
3

source ~/.bashrc

在riviz中启动turtlebot3模拟软件

roslaunch turtlebot3_fake turtlebot3_fake.launch

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
1
2
3

在gazebo中启动turtlebot3模拟软件

roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch
终止上述进程，打开一个包含有多个物体的场景

roslaunch turtlebot3_gazebo turtlebot3_world.launch 
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 
关闭键盘控制节点，打开自主导航和运动
roslaunch turtlebot3_gazebo turtlebot3_simulation.launch 
roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
1
2
3
4
5
6
7
8

实际Turtlebot3机器人

1.远程电脑安装Ubuntu、ROS、Turtlebot3、git克隆turtlebot3的包后，编译
2.SBC上安装Ubuntu、ROS、Turtlebot3、git克隆turtlebot3的包后，编译
3.Turtlebot3设置udev规则，树莓派上的USB口设置为无需root权限，保证外设设备正常运行

Turtlebot3与远程计算机网络连接

1.Turtlebot：
export ROS_MASTER_URI=http://IP_OF_REMOTE_PC:11311/
export ROS_HOSTNAME=IP_OF_Turtlebot
树莓派设置网络中始终连接到指定的网络
2.Remote PC：
export ROS_MASTER_URI=http://IP_OF_REMOTE_PC:11311/
export ROS_HOSTNAME=IP_OF_REMOTE_PC

SSH连接

1.树莓派上检查SSH状态 sudo service ssh status
2.inactive则重启SSH服务：sudo service ssh restart sudo systemctl enable ssh
3.远程计算机上建立连接 ssh <username>@<IP_OF_Turtlebot>
检测ping <IP_OF_Turtlebot>
4.如果ping能够建立但SSH不能正常工作，重新安装SSH

sudo apt-get remove openssh-client openssh-server
sudo apt-get install openssh-client openssh-server
1
2

第四章 Turtlebot机器人导航(记录，没运行过)

配置3D传感器Kinect

Kinect的环境变量：
export KINECT_DRIVER=freenect
export TURTLEBOT_3D_SENSOR=kinect

独立模式下测试3D传感器

Turtlebot2上运行

启动turtlebot的minimal launch
roslaunch turtlebot3_bringup  minimal.launch

启动相机nodelet包
roslaunch freenect_launch freenect.launch
ASUS或Carmine:
roslaunch openni2_launch openni2.launch
Inter RealSense:
roslaunch realsense_camera r200_nodelet_default.launch
1
2
3
4
5
6
7
8
9

运行ROS节点进行可视化

使用Image Viewer可视化数据

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

ssh <username>@<IP_OF_Turtlebot>
roslaunch freenect_launch freenect.launch

rosrun image_view image_view image:=/camera/rgb/image_color
深度图像则是：
rosrun image_view image_view image:=/camera/depth/image
1
2
3
4
5
6
7
8
9

使用Riz可视化数据

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

启动3D传感器相关软件（设置TURTLEBOT_3D_SENSOR环境变量）：
ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup 3dsensor.launch

rosrun turtlebot_rviz_launchers view_robot.launch
1
2
3
4
5
6
7
8

Turtlebot机器人导航

构建地图

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

启动gmapping
ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_navigation gmapping_demo.launch

启动Rviz
roslaunch turtlebot_rviz_launchers view_navigation.launch
Displays(Grid/RobotModel/LaseScan/Bumper Hit/Map/GlobalMap/LocalMap/Amcl Particle Swarm/Full Plan)

键盘控制
roslaunch turtlebot_teleop turtlebot_teleop_key.launch 

显示了完整的地图后，保存地图
ssh <username>@<IP_OF_Turtlebot>
rosrun map_server map_saver -f /home/<turtlebot's username>/my_map
<turtlebot's username>SSH登陆Turtlebot后使用pwd命令查看
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

gmapping功能包基于OpenSlam(http://openslam.org/gmapping.html)

Turtlebot机器人自主导航

无人控制模式下驱动

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

启动amcl处理模块
ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_navigation amcl_demo.launch map_file:=home/<turtlebot's username>/my_map.yaml

rosrun turtlebot_rviz_launchers view_navigation.launch
1
2
3
4
5
6
7
8

导航至目标点

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

rostopic echo /odom/pose/pose

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_navigation gmapping_demo.launch

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header:{ frame_id:"map"} ,pose:{ position:{ x: 1.0,y: 0,z: 0 },orientation:{ x: 1.0,y: 0,z: 0,w: 1 }}}'
1
2
3
4
5
6
7
8
9

基于python脚本与地图实现航路点导航

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_bringup  minimal.launch

启动acml程序
ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot_navigation amcl_demo.launch map_file:=home/<turtlebot's username>/my_map.yaml

启动rviz并显示地图
rosrun turtlebot_rviz_launchers view_navigation.launch

地图上第一机器人的位置
rostopic echo /initialpose
定义路径点
rostopic echo /clicked_point
使用Python代码移动机器人
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

python MoveTBtoGoalPoints.py

#!/usr/bin/env python    # MoveTBtoGoalPoints

import rospy
import actionlib       # Use the actionlib package for client and server

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

# Define Goal Points and orientations for TurtleBot in a list
GoalPoints = [ [(3.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)] ,
[(3.0, 3.6, 0.0), (0.0, 0.0, 0.707, 0.707)]]

# The function assign_goal initializes the goal_pose variable as a MoveBaseGoal action type.
#


def assign_goal(pose):  

    goal_pose = MoveBaseGoal()        
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]

    return goal_pose

if __name__ == '__main__':
    rospy.init_node('MoveTBtoGoalPoints')
# Create a SimpleActionClient of a move_base action type and wait for server.
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)  
    client.wait_for_server()
    
#       
    for TBpose in GoalPoints:  
        TBgoal = assign_goal(TBpose)   # For each goal point assign pose
        client.send_goal(TBgoal)
        client.wait_for_result()
    
    if(client.get_state() == GoalStatus.SUCCEEDED):
        rospy.loginfo("success")
    else:
        rospy.loginfo("failed")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

Turtlebot3机器人的SLAM

远程计算机上启动ROS MASTER
roscore

ssh <username>@<IP_OF_Turtlebot>
roslaunch turtlebot3_bringup  turtlebot3_robot.launch

export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_slam turtlebot3_slam.launch

可视化
rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 

保存地图
rosrun map_server map_saver -f ~/map
1
2
3
4
5
6
7
8
9
10
11
12
13

Turtlebot3自主导航

启动导航软件
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=map.yaml

可视化
rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz
1
2
3
4
5
6

ROS导航
http://wiki.ros.org/navigation
http://wiki.ros.org/navigation/Tutorials/RobotSetup
http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning20Guide

第五章构建模拟的机器人手臂

Xacro是用于ROS的XML宏语言，提供一系列的宏操作来替换一些重复性的声明语句。

1.利用Xacro建立一个关节式机器人手臂URDF

~/catkin_ws/src/ros_robotics/urdf/rrbot.xacro:

<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="width" value="0.1" />   <!-- Beams are square in length and width -->
  <xacro:property name="height1" value="2" />   <!-- Link 1 -->
  <xacro:property name="height2" value="1" />   <!-- Link 2 -->
  <xacro:property name="height3" value="1" />   <!-- Link 3 -->
  <xacro:property name="axle_offset" value="0.05" /> <!-- Space between joint and end of beam -->
  <xacro:property name="damp" value="0.7" />    <!-- damping coefficient -->

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
	  ixx="1.0" ixy="0.0" ixz="0.0"
	  iyy="1.0" iyz="0.0"
	  izz="1.0"/>
    </inertial>
  </link>

  <!-- Joint between Base Link and Middle Link -->
  <joint name="joint_base_mid" type="revolute">
    <parent link="base_link"/>
    <child link="mid_link"/>
    <origin xyz="0 ${width} ${height1 - axle_offset}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="${damp}"/>
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Middle Link -->
  <link name="mid_link">
    <visual>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
	  ixx="1.0" ixy="0.0" ixz="0.0"
	  iyy="1.0" iyz="0.0"
	  izz="1.0"/>
    </inertial>
  </link>

  <!-- Joint between Middle Link and Top Link -->
  <joint name="joint_mid_top" type="revolute">
    <parent link="mid_link"/>
    <child link="top_link"/>
    <origin xyz="0 ${width} ${height2 - axle_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="${damp}"/>
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Top Link -->
  <link name="top_link">
    <visual>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
	  ixx="1.0" ixy="0.0" ixz="0.0"
	  iyy="1.0" iyz="0.0"
	  izz="1.0"/>
    </inertial>
  </link>

</robot>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

包含三连杆元素，长宽均为0.1m，高分别为2m，1m，1m。base_link的原点坐标为（0,0,1），目的在于机械臂在Rviz下能够位于地面之上。

关节元素两个新的标签： <dynamics damping>和 <limit>，动态阻尼系数设置为0.7Nms/rad，该阻尼是对任意速度的关节施加的反向作用力的数值之和，用于降低机械臂的运动速度。

2.扩展Xacro

将所有的宏进行扩展，并输出一个生成的URDF文件。

位于文件相同的目录下运行 $ rosrun xacro xacro --inorder rrbot.xacro > rrbot.urdf

通过launch文件生成URDF文件：

  <param name="robot_description"
    command="$(find xacro)/xacro --inorder 
    '$(find ros_robotics)/urdf/$(arg model)'" />
1
2
3

~/catkin_ws/src/ros_robotics/launch/rrbot_rviz.launch:

<launch>
  <!-- set parameter on Parameter Server -->
  <arg name="model" />
  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find ros_robotics)/urdf/$(arg model)'" />

  <!-- send joint values from gui -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="TRUE"/>
  </node>

  <!-- use joint positions to update tf -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <!-- visualize robot model in 3D -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotics)/urdf.rviz" required="true" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

需要将.bashrc文件(主目录隐藏文件)中已经添加了自己的环境变量ROS_MASTER_URI和ROS_HOSTNAME或ROS_IP注释掉，添加以下两行

export ROS_MASTER_URI=http://localhost:11311//
export ROS_HOSTNAME=localhost
1
2

运行rviz的roslaunch命令:

$ roslaunch ros_robotics rrbot_rviz.launch model:=rrbot.xacro

3.添加颜色

~/catkin_ws/src/ros_robotics/urdf/materials.xacro:

<?xml version="1.0"?>
<robot>

  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>

  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>

  <material name="green">
    <color rgba="0.0 1.0 0.0 1.0"/>
  </material>

  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>

  <material name="orange">
    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
  </material>

  <material name="brown">
    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
  </material>

  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>

  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

在每一个连接杆组件添加

 <xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />
1

4.惯性的宏使用

  <!-- Default Inertial -->
  <xacro:macro name="default_inertial" params="z_value i_value mass">
    <inertial>
      <origin xyz="0 0 ${z_value}" rpy="0 0 0"/>
      <mass value="${mass}" />
      <inertia ixx="${i_value}" ixy="0.0" ixz="0.0"
               iyy="${i_value}" iyz="0.0"
               izz="${i_value}" />
      </inertial>
  </xacro:macro>
1
2
3
4
5
6
7
8
9
10

在每一个link里，使用以下代码替换完整的<inertial>块：

<xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1"/>
1

5.添加抓手

使用来自PR2机器人的网格文件

**主文件rrbot4.xacro中添加：**

```xml
  <xacro:include filename="$(find ros_robotics)/urdf/gripper.xacro" />  
1
2
3
4

四个连接杆分别为left_gripper、left_tip、right_gripper、right_tip
pr2_description是ros-kinetic-desktop-full的组成部分，可以在/opt/ros/kinetic/share/pr2_description/meshes/gripper_v0目录找到文件
需要在ros_robotics功能包目录下创建一个/meshes文件目录，并将1_finger.dae和1_finger_tip.dae复制到该目录下。

left_gripper:

 <link name="left_gripper">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ros_robotics/meshes/l_finger.dae"/>
      </geometry>
    </visual>
1
2
3
4
5
6
7

~/catkin_ws/src/ros_robotics/urdf/gripper.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Gripper -->
  <joint name="left_gripper_joint" type="revolute">
    <parent link="top_link"/>
    <child link="left_gripper"/>
    <origin xyz="0 0 ${height2 - axle_offset}" rpy="0 -1.57 0"/>
    <axis xyz="0 0 -1"/>
    <limit effort="30.0" lower="-0.548" upper="0.0" velocity="0.1"/>
  </joint>

  <link name="left_gripper">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ros_robotics/meshes/l_finger.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin rpy="0 0 0" xyz="0.05 0.025 0"/>
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
    </collision>
    <xacro:default_inertial z_value="0" i_value="1e-6" mass="1"/>
  </link>

  <joint name="left_tip_joint" type="fixed">
    <parent link="left_gripper"/>
    <child link="left_tip"/>
  </joint>

  <link name="left_tip">
    <visual>
      <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
      <geometry>
        <mesh filename="package://ros_robotics/meshes/l_finger_tip.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin rpy="0 0 0" xyz="0.11 0.005 0"/>
      <geometry>
        <box size="0.02 0.03 0.02"/>
      </geometry>
    </collision>
    <xacro:default_inertial z_value="0" i_value="1e-6" mass="1e-5"/>
  </link>

  <joint name="right_gripper_joint" type="revolute">
    <parent link="top_link"/>
    <child link="right_gripper"/>
    <origin xyz="0 0 ${height2 - axle_offset}" rpy="0 -1.57 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="30.0" lower="-0.548" upper="0.0" velocity="0.1"/>
  </joint>

  <link name="right_gripper">
    <visual>
      <origin rpy="3.1415 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ros_robotics/meshes/l_finger.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin rpy="0 0 0" xyz="0.05 -0.025 0"/>
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
    </collision>
    <xacro:default_inertial z_value="0" i_value="1e-6" mass="1"/>
  </link>

  <joint name="right_tip_joint" type="fixed">
    <parent link="right_gripper"/>
    <child link="right_tip"/>
  </joint>

  <link name="right_tip">
    <visual>
      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
      <geometry>
        <mesh filename="package://ros_robotics/meshes/l_finger_tip.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin rpy="0 0 0" xyz="0.11 -0.005 0"/>
      <geometry>
        <box size="0.02 0.03 0.02"/>
      </geometry>
    </collision>
    <xacro:default_inertial z_value="0" i_value="1e-6" mass="1e-5"/>
  </link>

  <transmission name="gripper_transmission1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_gripper_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="gripper_transmission2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_gripper_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_motor2">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

~/catkin_ws/src/ros_robotics/urdf/rrbot3.xacro:

<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="width" value="0.1" />   <!-- Beams are square in length and width -->
  <xacro:property name="height1" value="2" />   <!-- Link 1 -->
  <xacro:property name="height2" value="1" />   <!-- Link 2 -->
  <xacro:property name="height3" value="1" />   <!-- Link 3 -->
  <xacro:property name="axle_offset" value="0.05" /> <!-- Space between joint and end of beam -->
  <xacro:property name="damp" value="0.7" />    <!-- damping coefficient -->

  <!-- Default Inertial -->
  <xacro:macro name="default_inertial" params="z_value i_value mass">
    <inertial>
      <origin xyz="0 0 ${z_value}" rpy="0 0 0"/>
      <mass value="${mass}" />
      <inertia ixx="${i_value}" ixy="0.0" ixz="0.0"
               iyy="${i_value}" iyz="0.0"
               izz="${i_value}" />
      </inertial>
  </xacro:macro>

  <!-- Import Rviz colors -->
  <xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />

  <!-- Import gripper URDF -->
  <xacro:include filename="$(find ros_robotics)/urdf/gripper.xacro" />

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1"/>
  </link>

  <!-- Joint between Base Link and Middle Link -->
  <joint name="joint_base_mid" type="revolute">
    <parent link="base_link"/>
    <child link="mid_link"/>
    <origin xyz="0 ${width} ${height1 - axle_offset}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="${damp}"/>
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Middle Link -->
  <link name="mid_link">
    <visual>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
      <material name="green"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height2/2 - axle_offset}" i_value="1.0" mass="1"/>
  </link>

  <!-- Joint between Middle Link and Top Link -->
  <joint name="joint_mid_top" type="revolute">
    <parent link="mid_link"/>
    <child link="top_link"/>
    <origin xyz="0 ${width} ${height2 - axle_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="${damp}"/>
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Top Link -->
  <link name="top_link">
    <visual>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
      <material name="blue"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height3/2 - axle_offset}" i_value="1.0" mass="1"/>
  </link>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

运行Rviz的roslaunch命令

$ roslaunch ros_robotics rrbot_rviz.launch model:=rrbot3.xacro

6.在Gazebo中控制关节式机器人手臂

gazebo下模拟环境下所独有的特性元素主要有

<material>标签，指定每一个连接杆的颜色或纹理
<mu1>和<mu2>标签，用于定义四个表面相互接触的机器人连接杆的摩擦系数
用于控制rrbot的旋转副关节的插件

这些特定的Gazebo XML元素，存储在单独的文件中：

~/catkin_ws/src/ros_robotics/urdf/rrbot.gazebo:

<?xml version="1.0"?>
<robot>

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

  <!-- Base Link -->
  <gazebo reference="base_link">
    <material>Gazebo/Red</material>
  </gazebo>

  <!-- Middle Link -->
  <gazebo reference="mid_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Green</material>
  </gazebo>

  <!-- Top Link -->
  <gazebo reference="top_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Blue</material>
  </gazebo>

  <!-- Gripper Elements -->
  <gazebo reference="left_gripper">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
    
  <gazebo reference="right_gripper">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
 
  <gazebo reference="left_tip" />
  <gazebo reference="right_tip" />

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

在主文件使用Xacro的声明

<xacro:include filename="$(find ros_robotics)/urdf/rrbot.gazebo" />

将机器人手臂固定在世界坐标系下

  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
1
2
3
4
5
6

~/catkin_ws/src/ros_robotics/urdf/rrbot4.xacro:

<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.14"/>
  <xacro:property name="width" value="0.1" />   <!-- Beams are square in length and width -->
  <xacro:property name="height1" value="2" />   <!-- Link 1 -->
  <xacro:property name="height2" value="1" />   <!-- Link 2 -->
  <xacro:property name="height3" value="1" />   <!-- Link 3 -->
  <xacro:property name="axle_offset" value="0.05" /> <!-- Space between joint and end of beam -->
  <xacro:property name="damp" value="0.7" />    <!-- damping coefficient -->

  <!-- Default Inertial -->
  <xacro:macro name="default_inertial" params="z_value i_value mass">
    <inertial>
      <origin xyz="0 0 ${z_value}" rpy="0 0 0"/>
      <mass value="${mass}" />
      <inertia ixx="${i_value}" ixy="0.0" ixz="0.0"
               iyy="${i_value}" iyz="0.0"
               izz="${i_value}" />
      </inertial>
  </xacro:macro>

  <!-- Import Rviz colors -->
  <xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />

  <!-- Import gripper URDF -->
  <xacro:include filename="$(find ros_robotics)/urdf/gripper.xacro" />  

  <!-- Import Gazebo elements, including Gazebo colors -->
  <xacro:include filename="$(find ros_robotics)/urdf/rrbot.gazebo" />

  <!-- Used for fixing rrbot frame to Gazebo world frame -->
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height1}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1"/>
  </link>

  <!-- Joint between Base Link and Middle Link -->
  <joint name="joint_base_mid" type="revolute">
    <parent link="base_link"/>
    <child link="mid_link"/>
    <origin xyz="0 ${width} ${height1 - axle_offset}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="${damp}"/>
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Middle Link -->
  <link name="mid_link">
    <visual>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
      <material name="green"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height2}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height2/2 - axle_offset}" i_value="1.0" mass="1"/>
  </link>

  <!-- Joint between Middle Link and Top Link -->
  <joint name="joint_mid_top" type="revolute"> 
    <parent link="mid_link"/>
    <child link="top_link"/>
    <origin xyz="0 ${width} ${height2 - axle_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/> 
    <dynamics damping="${damp}"/> 
    <limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
  </joint>

  <!-- Top Link -->
  <link name="top_link">
    <visual>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
      <material name="blue"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
      <geometry>
	<box size="${width} ${width} ${height3}"/>
      </geometry>
    </collision>

    <xacro:default_inertial z_value="${height3/2 - axle_offset}" i_value="1.0" mass="1"/>
  </link>

  <transmission name="transmission1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_base_mid">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="transmission2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_mid_top">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

launch文件：

~/catkin_ws/src/ros_robotics/launch/rrbot_gazebo.launch:

<launch>
  <!-- We resume the logic in gazebo_ros package 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 ros_robotics)/worlds/rrbot.world"/>
   
    <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"/>

  </include>

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

  <!-- Spawn rrbot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     args="-param robot_description -urdf -model rrbot" />

</launch>


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

在gazebo中启动机器人手臂命令

$ roslaunch ros_robotics rrbot_gazebo.launch

7.给Xacro添加控制组件

在gazebo模拟环境下为机器人手臂添加控制组件的步骤如下

为rrbot以及抓手Xacro文件中的关节组件定义为传动元素
为gazebo特性元素添加gazebo_ros_control插件
生成用于控制参数的YAML配置文件
生成用于启动机器人关节控制器的控制启动文件

需要安装四个功能包：gazebo_ros_pkgs、gezebo_ros_control、ros_control、ros_controllers。

gazebo_ros_pkgs提供gazebo下机器人的接口和控制。ros_control、ros_controllers提供用于ROS机器人控制的基本控制器。

安装Debian功能包

$ sudo apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control ros-kinetic-ros-control ros-kinetic-ros-controllers

参考http://wiki.ros.org/ros_control

（1）定义关节的传动元素

<transmission>用于定义机器人关节和执行器间的相互关系。每一个旋转副关节(joint_base_mid、joint_mid_top、left_gripper、right_gripper)都需要一个<transmission>元素。

<transmission>元素的内容相似:

 <transmission name="transmission1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_base_mid">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
1
2
3
4
5
6
7
8
9

上述代码在rrbot4.xacro中

（2）添加Gazebo的ROS控制插件

  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/rrbot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
1
2
3
4
5
6
7

上述代码在rrbot.gazebo中

（3）生成YAML配置文件

YAML是一种常见于ROS参数中的标记语言。在参数服务器上使用YAML格式的文件对ROS的参数进行设置。创建一个用于存放关节控制器配置参数的YAML文件，该YAML文件由控制启动文件加载。定义四个rrbot控制器的比例积分微分增益。

~/catkin_ws/src/ros_robotics/config/rrbot_control.yaml:

rrbot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  
  # Position Controllers ---------------------------------------
  joint_base_mid_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_base_mid
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint_mid_top_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint_mid_top
    pid: {p: 100.0, i: 0.01, d: 10.0}
  left_gripper_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: left_gripper_joint
    pid: {p: 1.0, i: 0.00, d: 0.0}
  right_gripper_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: right_gripper_joint
    pid: {p: 1.0, i: 0.00, d: 0.0}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

（4）创建控制启动文件

初始化机器人手臂rrbot的最佳方法是生成用于将参数加载至参数服务器的启动文件，并通过该文件启动所有的ros_control控制器。配置文件YAML文件对控制器进行设置,rosparam声明将控制器设置加载至参数服务器。然后control_spawner节点使用controller_manager包为rrbot生成五个控制器。此外还将为robot_state_publisher启动另一个节点。

~/catkin_ws/src/ros_robotics/launch/rrbot_control.launch:

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find ros_robotics)/config/rrbot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="control_spawner" pkg="controller_manager" type="spawner" respawn="false"
	output="screen" ns="/rrbot" args="joint_state_controller
					  joint_base_mid_position_controller
					  joint_mid_top_position_controller
                                          left_gripper_joint_position_controller
                                          right_gripper_joint_position_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
	respawn="false" output="screen">
    <remap from="/joint_states" to="/rrbot/joint_states" />
  </node>

</launch>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

在节点启动之后，joint_state_controller开始发布通过JointState话题rrbot所有的非固定关节的状态。robot_state_publisher订阅JointState消息并发布机器人的变换信息到tf变换库中。其他关节的位置控制器分别对相应的旋转副关节进行控制。

在gazebo下启动rrbot： $ roslaunch ros_robotics rrbot_gazebo.launch

当在gazebo中看到rrbot时，打开第二个终端，并使用以下命令启动控制器

$ roslaunch ros_robotics rrbot_control.launch

控制启动文件中使用了controller_manager和robot_state_publisher功能包，需要将它们的依赖添加到ros_robotics功能包的package.xml文件中。在相应的依赖后，需要添加以下声明：

<exec_depend>controller_manager</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
1
2

采用ROS命令行控制机器人手臂

通过第三个终端窗口发送命令来对机器人手臂rrbot进行控制

（相对于中间的连接杆）把顶部的连接杆移动到1.57弧度（90°）的位置

$ rostopic pub -1 /rrbot/joint_mid_top_position_controller/command std_msgs/Float64 "data: 1.57"

由于受到环境重力以及控制器执行精度的影响，可能并非达到90°的位置，适当进行PID增益调节，能够提高执行的精确度。

持续发送一下两条命令，可以将左右两个抓手从中心位置打开至-0.5弧度的位置：
$ rostopic pub -1 /rrbot/right_gripper_joint_position_controller/command std_msgs/Float64 "data: -0.5";rostopic pub -1 /rrbot/left_gripper_joint_position_controller/command std_msgs/Float64 "data: 1.57"

采用rqt控制机器人手臂

启动rqt

$ rosrun rqt_gui rqt_gui 或 $ rqt

Plugins|Topic|Message Publiser：添加/rrbot/joint_base_mid_position_controller/command，/rrbot/joint_mid_top_position_controller/command到主界面，修改表达式发布消息。

**Plugins|Visualization|Plot:**joint_xxx_position_controller/command/data或者joint_xxx_position_controller/state/error绘制在屏幕上,误差图可视化的形式显示。

Plugins|Configuration|Dynamic Reconfigre:Expand all,为任意关节控制器选择pid，对参数进行动态调整，从而调整控制器的性能参数。

第六章机器人手臂摇摆的关节控制-Baxter

每个手臂都具有七个自由度
Baxter更多技术规格参数，可见：
http://www.rethinkrobotics.com/baxter/tech-specs/
http://sdk.rethinkrobotics.com/wiki/Hardware_Specifications
http://www.active8robots.com/wp-content/uploads/BaxterHardware-Specifications-Architecture-Datasheet.pdf
机器人上进行配置和代码执行http://sdk.rethinkrobotics.com/wiki/SSH
演示视频http://sdk.rethinkrobotics.com/wiki/Customer_Videos
研究论文http://sdk.rethinkrobotics.com/wiki/Published_Work
Baxter模拟器的ROS功能包和API更多细节性内容
http://sdk.rethinkrobotics.com/wiki/Simulator_Architecture
http://sdk.rethinkrobotics.com/wiki/Baxter_Simulator
http://sdk.rethinkrobotics.com/wiki/API_Reference#tab.3DSimulator_API
https://github.com/RethinkRobotics/baxter_simulator

下载Baxter软件

安装Baxter SDK软件

SDK下载安装到工作站计算机的ROS的catkin工作空间
http://sdk.rethinkrobotics.com/wiki/Workstation_Setup

参考
https://blog.csdn.net/weixin_43731206/article/details/114584436#commentBox

安装Baxter SDK依赖
sudo apt-get update
sudo apt-get install git python-argparse python3-wstool python3-vcstools python3-rosdep ros-noetic-control-msgs ros-noetic-joystick-drivers

wstool工具检查
cd ~/baxter_ws/src
wstool init
sudo wstool merge https://raw.githubusercontent.com/RethinkRobotics/baxter/master/baxter_sdk.rosinstall
wstool update

(修改github hosts
sudo vim /etc/hosts
199.232.28.133 raw.githubusercontent.com)

$ cd ~/baxter_ws
$ catkin_make
$ catkin_make install
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

安装Baxter模拟器

http://sdk.rethinkrobotics.com/wiki/Simulator_Installation

sudo apt-get update
sudo apt-get install gazebo2 ros-indigo-qt-build ros-indigo-driver-common ros-indigo-gazebo-ros-control ros-indigo-gazebo-ros-pkgs ros-indigo-ros-control ros-indigo-control-toolbox ros-indigo-realtime-tools ros-indigo-ros-controllers ros-indigo-xacro python-wstool ros-indigo-tf-conversions ros-indigo-kdl-parser(官网未修改，改为noetic)

cd ~/baxter_ws/src
wstool init
sudo wstool merge https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/noetic_devel/baxter_simulator.rosinstall
wstool update

cd ~/baxter_ws
catkin_make
1
2
3
4
5
6
7
8
9
10

配置Baxter shell

SDK需要通过baxter.sh脚本建立机器人与工作站计算机之间的连接
模拟器需要使用baxter.sh脚本建立一种模拟模式

复制到baxter_ws目录下，权限修改为允许所有用户运行
cp ~/baxter_ws/src/baxter/baxter.sh ~/baxter_ws
chmod +x baxter.sh

打开baxter.sh文件，将your_ip的值更改为工作站计算机的IP地址，修改ros_version变量
1
2
3
4
5

安装MoveIt ！

http://sdk.rethin-krobotics.com/wiki/MoveIt_Tutorial

cd ~/baxter_ws/src
git clone https://github.com/ros.planning/moveit_robots.git
sudo apt-get update
sudo apt-get install ros-noetic-moveit

cd ~/baxter_ws
catkin_make
1
2
3
4
5
6
7

在Gazebo中启动Baxter模拟器

启动Baxter模拟器
cd ~/baxter_ws
./baxter.sh sim

检查ROS环境
env | grep ROS

启动控制器，开始模拟操作
roslaunch baxter_gazebo baxter_world.launch
(gazebo死机 移除所有gezebo进程 killall gzserver)
1
2
3
4
5
6
7
8
9
10

故障说明
http://sdk.rethinkrobotics.com/wiki/Troubleshooting

第二个终端
cd ~/baxter_ws
./baxter.sh sim

确认状态
rosrun baxter_tools enable_robot.py -s

使能Baxter
rosrun baxter_tools enable_robot.py -e

确认状态
rosrun baxter_tools enable_robot.py -s

使能机器人，将手臂设置为一个已知的位置
rosrun baxter_tools tuck_arms.py -u
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

样例

在模拟的机器人头部显示屏显示一张图片
rosrun baxter_examples xdisplay_image.py --file=`rospack find baxter_examples`/share/images/baxterworking.png

控制Baxter的头部上下或者左右运动
rosrun baxter_examples head_wobbler.py 

1.手臂展开，Baxter到达闭合位置
rosrun baxter_tools tuck_arms.py -u
参考http://sdk.rethinkrobotics.com/wiki/Tuck_Arms_Tool


2.手臂摇摆运动
rosrun baxter_examples joint_velocity_wobbler.py
参考http://sdk.rethinkrobotics.com/wiki/Wobbler_Example

3.键盘控制手臂和抓手
rosrun baxter_examples joint_position_keyboard.py

4.使用游戏手柄控制手臂和抓手
joint_position_joystick程序使用源自joy功能包的ROS驱动程序

检查手柄驱动功能包joy的安装情况
rospack find joy
可使用以下命令安装
sudo apt-get install ros-noetic-joystick-drivers
PS3手柄安装ps3joy功能包 参考http://wiki.ros.org/ps3joy/Tutorials/PairingJoystickAndBluetoothDongle

启动joint_position_joystick程序，需要指定手柄类型(xbox/logitech/ps3)
roslaunch baxter_examples joint_position_joystick.launch joystick:=<joystick_type>

5.使用Python脚本控制手臂
python home_arms.py

6.记录和重演手臂的运动
rosrun baxter_examples joint_recorder.py -f armRoutine
armRoutine为文件名

回放
rosrun baxter_examples joint_position_file_playback.py -f armRoutine
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

home_arms.py

#!/usr/bin/env python

"""
Script to return Baxter's arms to a "home" position
"""

# rospy - ROS Python API
import rospy

# baxter_interface - Baxter Python API
import baxter_interface

# initialize our ROS node, registering it with the Master
rospy.init_node('Home_Arms')

# create instances of baxter_interface's Limb class
limb_right = baxter_interface.Limb('right')
limb_left = baxter_interface.Limb('left')

# store the home position of the arms
home_right = {'right_s0': 0.08, 'right_s1': -1.00, 'right_w0': -0.67, 'right_w1': 1.03, 'right_w2': 0.50, 'right_e0': 1.18, 'right_e1': 1.94}
home_left = {'left_s0': -0.08, 'left_s1': -1.00, 'left_w0': 0.67, 'left_w1': 1.03, 'left_w2': -0.50, 'left_e0': -1.18, 'left_e1': 1.94}

# move both arms to home position
limb_right.move_to_joint_positions(home_right)
limb_left.move_to_joint_positions(home_left)

quit()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

Baxter手臂与正向运动

关节与关节状态发布器有关话题/robot/joint_states

cd ~/baxter_ws
./baxter.sh sim
roslaunch baxter_gazebo baxter_world.launch

cd ~/baxter_ws
./baxter.sh sim
rosrun baxter_tools enable_robot.py -e
将Baxter手臂置于home位置
python home_arms.py

显示相关信息
rostopic echo /robot/joint_states -nl
显示左侧手臂末端的位置和朝向
 rostopic echo /robot/limb/left/endpoint_state/pose -n1
1
2
3
4
5
6
7
8
9
10
11
12
13
14

理解tf

控制Baxter手臂移动到0角度位置的程序
arms_to_zero_angles.py：

#!/usr/bin/env python   # arms_to_zero_angles.py 
#   
"""
Script to return Baxter's arms to a "zero angles" position  
"""

# rospy - ROS Python API
import rospy

# baxter_interface - Baxter Python API
import baxter_interface

# initialize our ROS node, registering it with the Master
rospy.init_node('Zero_Arms')

# create instances of baxter_interface's Limb class
limb_right = baxter_interface.Limb('right')
limb_left = baxter_interface.Limb('left')

# store the zero position of the arms
zero_right = {'right_s0': 0.0, 'right_s1': 0.00, 'right_w0': 0.00, 'right_w1': 0.00, 'right_w2': 0.00, 'right_e0': 0.00, 'right_e1': 0.00}
zero_left = {'left_s0': 0.0, 'left_s1': 0.00, 'left_w0': 0.00, 'left_w1': 0.00, 'left_w2': 0.00, 'left_e0': 0.00, 'left_e1': 0.00}

# move both arms to home position
limb_right.move_to_joint_positions(zero_right)
limb_left.move_to_joint_positions(zero_left)
quit()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

python编译成可执行
chmod +x arms_to_zero_angles.py

运行脚本
python arms_to_zero_angles.py
1
2
3
4
5

直接指定关节组件角度

rostopic pub -r 10 /robot/limb/left/joint_command baxter_core_msgs/Jointcommand "{mode:1, command: [0.0, 0.0, 0.0,  0.0], name: ['left_w1', 'left_e1', 'left_s0', 'left_s1']}"
1

参考http://sdk.rethinkrobotics.com/wiki/Arm_Control_Modes

查看机器人元素的tf树

启动view_frames
rosrun tf view_frames
查看evince frames.pdf
参考http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf

MoveIt 简介

用户手册 http://sdk.rethinkrobotics.com/wiki/MoveIt_Tutorial
对MoveIt!的使用进行演示

cd ~/baxter_ws
./baxter.sh sim
roslaunch baxter_gazebo baxter_world.launch

打开手臂，运行py脚本启动joint_trajectory_action_server
cd ~/baxter_ws
./baxter.sh sim
rosrun baxter_tools tuck_arms.py -u
rosrun baxter_interface joint_trajectory_action_server.py

启动moveit
cd ~/baxter_ws
./baxter.sh sim
roslaunch baxter_moveit_config baxter_grippers.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14

控制真实的Baxter机器人

到达航路点
rosrun baxter_examples joint_position_waypoint.py -l <left or right>

控制关节的力矩弹簧
rosrun baxter_examples joint_torque_springs.py -l <left or right>
rqt对关节力矩进行修改和配置
rosrun rqt_reconfigure rqt_reconfigue

控制关节速度
rosrun baxter_examples joint_velocity_puppet.py -l <left or right>
1
2
3
4
5
6
7
8
9
10

其他示例参考 http://sdk.rethinkrobotics.com/wiki/Examples

反向运动

正向运动——计算出抓手在任意时刻的位置
反向运动——控制抓手运动到指定位置和朝向，需要计算关节的角度
参考 http://sdk.rethinkrobotics.com/wiki/IK_Service_Example
http://sdk.rethinkrobotics.com/wiki/IK_Service_-Code Walkthrough

运行py脚本得到运行到固定位置时左侧手臂有关关节的角度值
rosrun baxter_examples ik_service_client.py -l left
1
2

利用反向运动移动Baxter的手臂（真实baxter）
1.启动Baxter，展开手臂，此时位于home位置
2.记录左侧手臂终点状态的位置和朝向
3.将Baxter的左侧手臂移动到任意位置
4.baxter_examples功能包里的ik_service_client.py，输入展开后的左侧手臂位置和朝向，以不同的名字保存在catkin_workspace下
5.执行脚本，获取左侧手臂的关节角度
6.将角度输入到修改后的home_arms.py中，执行该脚本
7.记录新终点位置和朝向，并与步骤2中为的原始值进行对比

1.
cd ~/baxter_ws
./baxter.sh
roslaunch baxter_tools tuck_arms.py -u

2.
rostopic echo /robot/limb/left/endpoint_state/pose -n1 -w4

3.

4.
roscd baxter_examples/scripts
找到要修改的脚本，以新名字保存
chmod +x ik_home_arms_ch6RealBaxter.py

5.运行相应的例子，获取将Baxter左侧手臂移动到展开位置的关节角度
python ik_home_arms_ch6RealBaxter.py -l left

6.使用编辑过的py脚本home_arms.py得到的角度来控制Baxter手臂的移动，改变左侧手臂关节的角度值，以新文件命名
chmod +x MoveLeftArmToHome.py
python MoveLeftArmToHome.py

7.
rostopic echo /robot/limb/left/endpoint_state/pose -n1 -w4
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

MoveLeftArmToHome.py:

#!/usr/bin/env python

"""
Script to return Baxter's arms to a "home" position
"""

# rospy - ROS Python API
import rospy

# baxter_interface - Baxter Python API
import baxter_interface

# initialize our ROS node, registering it with the Master
rospy.init_node('Home_Arms')

# create instances of baxter_interface's Limb class
limb_right = baxter_interface.Limb('right')
limb_left = baxter_interface.Limb('left')

# store the home position of the arms
home_right = {'right_s0': 0.08, 'right_s1': -1.00, 'right_w0': -0.67, 'right_w1': 1.03, 'right_w2': 0.50, 'right_e0': 1.18, 'right_e1': 1.94}
home_left = {'left_w0': -1.8582664616409326, 'left_w1': -1.460468102595922, 'left_w2': 2.2756459061545797, 'left_e0': -1.6081637990992477, 'left_e1': 1.9645288022495901, 'left_s0': 0.044896665837355125, 'left_s1': -0.3326492980686455}

# move both arms to home position
limb_right.move_to_joint_positions(home_right)
limb_left.move_to_joint_positions(home_left)

quit()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

ik_home_arms_ch6RealBaxter.py:

#!/usr/bin/env python      # ik_home_arms_ch6RealBaxter.py

# Copyright (c) 2013-2015, Rethink Robotics
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
#    this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
# 3. Neither the name of the Rethink Robotics nor the names of its
#    contributors may be used to endorse or promote products derived from
#    this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

"""
Baxter RSDK Inverse Kinematics Example
"""
import argparse
import struct
import sys

import rospy

from geometry_msgs.msg import (
    PoseStamped,
    Pose,
    Point,
    Quaternion,
)
from std_msgs.msg import Header

from baxter_core_msgs.srv import (
    SolvePositionIK,
    SolvePositionIKRequest,
)


def ik_test(limb):
    rospy.init_node("rsdk_ik_service_client")
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    poses = {
        'left': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x=0.57,
                    y=0.18,
                    z=0.10,
                ),
                orientation=Quaternion(
                    x=0.13,
                    y=0.99,
                    z=0.00,
                    w=0.02,
                ),
            ),
        ),
        'right': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x=0.656982770038,
                    y=-0.852598021641,
                    z=0.0388609422173,
                ),
                orientation=Quaternion(
                    x=0.367048116303,
                    y=0.885911751787,
                    z=-0.108908281936,
                    w=0.261868353356,
                ),
            ),
        ),
    }

    ikreq.pose_stamp.append(poses[limb])
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1

    # Check if result valid, and type of seed ultimately used to get solution
    # convert rospy's string representation of uint8[]'s to int's
    resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
                               resp.result_type)
    if (resp_seeds[0] != resp.RESULT_INVALID):
        seed_str = {
                    ikreq.SEED_USER: 'User Provided Seed',
                    ikreq.SEED_CURRENT: 'Current Joint Angles',
                    ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
                   }.get(resp_seeds[0], 'None')
        print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
              (seed_str,))
        # Format solution into Limb API-compatible dictionary
        limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
        print "\nIK Joint Solution:\n", limb_joints
        print "------------------"
        print "Response Message:\n", resp
    else:
        print("INVALID POSE - No Valid Joint Solution Found.")

    return 0


def main():
    """RSDK Inverse Kinematics Example

    A simple example of using the Rethink Inverse Kinematics
    Service which returns the joint angles and validity for
    a requested Cartesian Pose.

    Run this example, passing the *limb* to test, and the
    example will call the Service with a sample Cartesian
    Pose, pre-defined in the example code, printing the
    response of whether a valid joint solution was found,
    and if so, the corresponding joint angles.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.add_argument(
        '-l', '--limb', choices=['left', 'right'], required=True,
        help="the limb to test"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    return ik_test(args.limb)

if __name__ == '__main__':
    sys.exit(main())
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

home_arms.py:

#!/usr/bin/env python

"""
Script to return Baxter's arms to a "home" position
"""

# rospy - ROS Python API
import rospy

# baxter_interface - Baxter Python API
import baxter_interface

# initialize our ROS node, registering it with the Master
rospy.init_node('Home_Arms')

# create instances of baxter_interface's Limb class
limb_right = baxter_interface.Limb('right')
limb_left = baxter_interface.Limb('left')

# store the home position of the arms
home_right = {'right_s0': 0.08, 'right_s1': -1.00, 'right_w0': -0.67, 'right_w1': 1.03, 'right_w2': 0.50, 'right_e0': 1.18, 'right_e1': 1.94}
home_left = {'left_s0': -0.08, 'left_s1': -1.00, 'left_w0': 0.67, 'left_w1': 1.03, 'left_w2': -0.50, 'left_e0': -1.18, 'left_e1': 1.94}

# move both arms to home position
limb_right.move_to_joint_positions(home_right)
limb_left.move_to_joint_positions(home_left)

quit()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

使用状态机实现YMCA

SMACH-基于python的状态机结构库
参考http://wiki.ros.org/smach
http://wiki.ros.org/smach/Tutorials
舞出YMCA以及一个中立位置，实现程序为YMCAStateMach.py
YMCAStateMach.py

#!/usr/bin/env python

# Using the ROS SMACH package, this Python script creates five states 
# corresponding to Baxter's arm poses for each letter Y, M, C, A and a 
# fifth state for a neutral pose. When one pose of the arms completes, 
# the state will successfully complete and the next state will begin. 

# Refer to ROS Robotics By Example 2nd edition for a detailed
# explanation of this software.

import rospy
from smach import State,StateMachine

from time import sleep
from MoveControl import Baxter_Arms

class Y(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_y = {
            'letter': {
                      'left':  [0.0, -1.0, 0.0, 0.0,  0.0, 0.0, 0.0], 
                      'right':  [0.0, -1.0, 0.0, 0.0,  0.0, 0.0, 0.0]
                       } }
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me a Y!')
        barms.supervised_move(self.letter_y)
        sleep(2)
        return 'success'

class M(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_m = {
            'letter': {
                      'left':  [0.0, -1.50, 1.0, -0.052,  3.0, 2.094, 0.0], 
                      'right':  [0.0, -1.50, -1.0, -0.052,  -3.0, 2.094, 0.0]
                       } }
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me a M!')
        barms.supervised_move(self.letter_m)
        sleep(2)
        return 'success'

class C(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_c = {
            'letter': {
                      'left':  [0.80, 0.0, 0.0, -0.052,  3.0, 1.50, 0.0], 
                      'right':  [0.0, -1.50, -1.0, -0.052, -3.0, 1.0, 0.0]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me a C!')
        barms.supervised_move(self.letter_c)
        sleep(2)
        return 'success'

class A(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_a = {
            'letter': {
                      'left':  [0.50, -1.0, -3.0, 1.0, 0.0, 0.0, 0.0], 
                      'right':  [-0.50, -1.0, 3.0, 1.0,  0.0, 0.0, 0.0]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me an A!')
        barms.supervised_move(self.letter_a)
        sleep(2)
        return 'success'


class Zero(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.zero = {
            'letter': {
                      'left':  [0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00], 
                      'right':  [0.00, 0.00, 0.00, 0.00,  0.00, 0.00, 0.00]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Ta-da')
        barms.supervised_move(self.zero)
        sleep(2)
        return 'success'

if __name__ == '__main__':

    barms = Baxter_Arms()
    rospy.on_shutdown(barms.clean_shutdown)

    sm = StateMachine(outcomes=['success'])
    with sm:
        StateMachine.add('Y', Y(), transitions={'success':'M'})
        StateMachine.add('M', M(), transitions={'success':'C'})
        StateMachine.add('C', C(), transitions={'success':'A'})
        StateMachine.add('A', A(), transitions={'success':'ZERO'})
        StateMachine.add('ZERO', Zero(), transitions={'success':'success'})  

    sm.execute()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116

MoveControl.py

#!/usr/bin/env python

# Origional code from Rethink Robotics, Copyright (c) 2013-2015  
# This code has been modified encapsulate Baxter's arms in a class and 
# provide methods to control the arms.
# It works with the YMCAStateMach.py

# Refer to ROS Robotics By Example 2nd edition for a detailed
# explanation of this software.

import argparse

from copy import deepcopy

import rospy

from std_msgs.msg import (
    Empty,
    Bool,
)

import baxter_interface

from baxter_core_msgs.msg import (
    CollisionAvoidanceState,
)
from baxter_interface import CHECK_VERSION


class Baxter_Arms():
    def __init__(self):
        rospy.loginfo("Initializing node now... ")
        rospy.init_node("moving_arms")
        rospy.loginfo("Node Initialized. ")
        self._done = False
        self._limbs = ('left', 'right')
        self._arms = {
            'left': baxter_interface.Limb('left'),
            'right': baxter_interface.Limb('right'),
            }
        self._pose_rate = rospy.Rate(20.0)  # Hz
        self._pose_threshold = 0.2   # radians
        self._peak_angle = -1.6      # radians
        self._arm_state = {
                           'pose': {'left': 'none', 'right': 'none'},
                           'collide': {'left': False, 'right': False},
                           'flipped': {'left': False, 'right': False}
                          }

     	  #Empty positional library that will be fed in and overwritten as arms_pose.
        self._joint_moves = {
            'letter': {
                       'left':  [0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0], 
                       'right':  [0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0]
                       } 
            
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]
            }
        self._collide_lsub = rospy.Subscriber(
                             'robot/limb/left/collision_avoidance_state',
                             CollisionAvoidanceState,
                             self._update_collision, 'left')
        self._collide_rsub = rospy.Subscriber(
                             'robot/limb/right/collision_avoidance_state',
                             CollisionAvoidanceState,
                             self._update_collision, 'right')
        self._disable_pub = {
            'left': rospy.Publisher(
                 'robot/limb/left/suppress_collision_avoidance',
                 Empty, queue_size=10),
            'right': rospy.Publisher(
                 'robot/limb/right/suppress_collision_avoidance',
                 Empty, queue_size=10)
        }
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._enable_pub = rospy.Publisher('robot/set_super_enable', 
                                           Bool, queue_size=10)

    def _update_collision(self, data, limb):
        self._arm_state['collide'][limb] = len(data.collision_object) > 0
        self._check_arm_state()

    def _check_arm_state(self):
        """
        Check for goals and behind collision field.

        If s1 joint is over the peak, collision will need to be disabled
        to get the arm around the head-arm collision force-field.
        """
        diff_check = lambda a, b: abs(a - b) <= self._pose_threshold
        for limb in self._limbs:
            angles = [self._arms[limb].joint_angle(joint)
                      for joint in self._arms[limb].joint_names()]

            # Check if in a goal position
            letter_goal = map(diff_check, angles,
                            self._joint_moves['letter'][limb])                      
            if all(letter_goal):
                self._arm_state['pose'][limb] = 'letter' 
            else:
                self._arm_state['pose'][limb] = 'none'

            # Check if shoulder is flipped over peak
            self._arm_state['flipped'][limb] = (
                self._arms[limb].joint_angle(limb + '_s1') <= self._peak_angle)

    def _move_to(self, new_pose, disabled):
        if any(disabled.values()):
            [pub.publish(Empty()) for pub in self._disable_pub.values()]
        while (any(self._arm_state['pose'][limb] != goal
                   for limb, goal in new_pose.viewitems())
               and not rospy.is_shutdown()):
            if self._rs.state().enabled == False:
                self._enable_pub.publish(True)
            for limb in self._limbs:
                if disabled[limb]:
                    self._disable_pub[limb].publish(Empty())
                if limb in new_pose:
                    self._arms[limb].set_joint_positions(dict(zip(
                                      self._arms[limb].joint_names(),
                                      self._joint_moves[new_pose[limb]][limb])))
            self._check_arm_state()
            self._pose_rate.sleep()

        if any(self._arm_state['collide'].values()):
            self._rs.disable()
        return


    #Importing data in the form of arms_pose, origionally paired with YMCAStateMach.py 
    def supervised_move(self, arms_pose):
        self._joint_moves = arms_pose    # Updating private variable with new arms_pose
        
        self._check_arm_state()
        #Update our starting state to check if arms are posed?
        rospy.loginfo("Movement in progress.")
        suppress = deepcopy(self._arm_state['flipped'])
        actions = {'left': 'letter', 'right': 'letter'}    
        self._move_to(actions, suppress)
        self._done = True
        return

    def clean_shutdown(self):
        """Handles ROS shutdown (Ctrl-C) safely."""
        if not self._done:
            rospy.logwarn('Aborting: Shutting down safely...')
        if any(self._arm_state['collide'].values()):
            while self._rs.state().enabled != False:
                [pub.publish(Empty()) for pub in self._disable_pub.values()]
                self._enable_pub.publish(False)
                self._pose_rate.sleep()


def main():
    barms = Baxter_Arms()
    rospy.on_shutdown(barms.clean_shutdown)
    barms.supervised_move()
    rospy.loginfo("Finished move.")
    
if __name__ == "__main__":
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

YMCAStateMach_for_Sim.py

#!/usr/bin/env python

# Using the ROS SMACH package, this Python script creates five states 
# corresponding to Baxter's arm poses for each letter Y, M, C, A and a 
# fifth state for a neutral pose. When one pose of the arms completes, 
# the state will successfully complete and the next state will begin. 

# Refer to ROS Robotics By Example 2nd edition for a detailed
# explanation of this software.

import rospy
from smach import State,StateMachine

from time import sleep
from MoveControl import Baxter_Arms

class Y(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_y = {
            'letter': {
                      'left':  [0.0, -1.0, 0.0, 0.0,  0.0, 0.0, 0.0], 
                      'right':  [0.0, -1.0, 0.0, 0.0,  0.0, 0.0, 0.0]
                       } }
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me a Y!')
        barms.supervised_move(self.letter_y)
        sleep(2)
        return 'success'

class M(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_m = {
            'letter': {
                      'left':  [0.0, -1.50, 1.0, -0.05,  2.4, 2.09, 0.0], 
                      'right':  [0.0, -1.50, -1.0, -0.05,  -2.4, 2.09, 0.0]
                       } }
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]
        # 'left':  [0.0, -1.50, 1.0, -0.052,  3.0, 2.094, 0.0], original for real Baxter
        # 'right':  [0.0, -1.50, -1.0, -0.052,  -3.0, 2.094, 0.0]  original for real Baxter

    def execute(self, userdata):
        rospy.loginfo('Give me a M!')
        barms.supervised_move(self.letter_m)
        sleep(2)
        return 'success'

class C(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_c = {
            'letter': {
                      'left':  [0.80, 0.0, 0.0, -0.052,  3.0, 1.50, 0.0], 
                      'right':  [0.0, -1.50, -1.0, -0.052, -3.0, 1.0, 0.0]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me a C!')
        barms.supervised_move(self.letter_c)
        sleep(2)
        return 'success'

class A(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.letter_a = {
            'letter': {
                      'left':  [0.50, -1.0, -3.0, 1.0, 0.0, 0.0, 0.0], 
                      'right':  [-0.50, -1.0, 3.0, 1.0,  0.0, 0.0, 0.0]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Give me an A!')
        barms.supervised_move(self.letter_a)
        sleep(2)
        return 'success'


class Zero(State):
    def __init__(self):
        State.__init__(self, outcomes=['success'])

        self.zero = {
            'letter': {
                      'left':  [0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00], 
                      'right':  [0.00, 0.00, 0.00, 0.00,  0.00, 0.00, 0.00]
                       } } 
                         #DoF Key [s0,s1,e0,e1,w0,w1,w2]

    def execute(self, userdata):
        rospy.loginfo('Ta-da')
        barms.supervised_move(self.zero)
        sleep(2)
        return 'success'

if __name__ == '__main__':

    barms = Baxter_Arms()
    rospy.on_shutdown(barms.clean_shutdown)

    sm = StateMachine(outcomes=['success'])
    with sm:
        StateMachine.add('Y', Y(), transitions={'success':'M'})
        StateMachine.add('M', M(), transitions={'success':'C'})
        StateMachine.add('C', C(), transitions={'success':'A'})
        StateMachine.add('A', A(), transitions={'success':'ZERO'})
        StateMachine.add('ZERO', Zero(), transitions={'success':'success'})  

    sm.execute()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

PillarTable.scene：

(noname)+
* pillar
1
box
0.308 0.13056 0.6528
0.7 -0.01 0.03
0.0108439 0.706876 0.0103685 0.707178
0 0 0 0
* tabletop
1
box
0.7 1.3 0.02
0.7 0.04 -0.13
0 0 0 1
0.705882 0.705882 0.705882 1
.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Gausst松鼠会/article/detail/401024?site