ROS机器人开发
文章目录
- 第一章 ROS基础
-
- 1.ROS安装
- 2. 安装rosinstall
- 3.创建catkin工作空间：
- 4.加载源文件setup.bash
- 5.ROS功能包
- 6.ros节点、话题、消息
- 7.ROS机器人模拟程序
- 8.控制乌龟运动
- **第二章 构建模拟两轮ROS机器人**
-
- 1.安装rviz
- 2.生成并构建ROS功能包
- 3. 构建机器人URDF
- **Gazebo**
-
- 1.安装Gazebo
- **2.URDF修改**
- **3.MATLAB**
- 第三章 TurtleBot机器人操控
-
- Turtlebot3
-
- 在riviz中启动turtlebot3模拟软件
- 在gazebo中启动turtlebot3模拟软件
- 实际Turtlebot3机器人
-
- Turtlebot3与远程计算机网络连接
- SSH连接
- 第四章 Turtlebot机器人导航(记录，没运行过)
-
- 配置3D传感器Kinect
- 独立模式下测试3D传感器
- 运行ROS节点进行可视化
-
- 使用Image Viewer可视化数据
- 使用Riz可视化数据
- Turtlebot机器人导航
- Turtlebot机器人自主导航
-
- 无人控制模式下驱动
- 导航至目标点
- 基于python脚本与地图实现航路点导航
- Turtlebot3机器人的SLAM
- 第五章 构建模拟的机器人手臂
-
- 1.利用Xacro建立一个关节式机器人手臂URDF
- **2.扩展Xacro**
- **3.添加颜色**
- **4.惯性的宏使用**
- **5.添加抓手**
- 6.在Gazebo中控制关节式机器人手臂
- 7.给Xacro添加控制组件
-
- **（1）定义关节的传动元素**
- **（2）添加Gazebo的ROS控制插件**
- **（3）生成YAML配置文件**
- （4）创建控制启动文件
- 采用ROS命令行控制机器人手臂
- 采用rqt控制机器人手臂
- 第六章 机器人手臂摇摆的关节控制-Baxter
-
- 下载Baxter软件
-
- 安装Baxter SDK软件
- 安装Baxter模拟器
- 配置Baxter shell
- 安装MoveIt ！
- 在Gazebo中启动Baxter模拟器
- Baxter手臂与正向运动
-
- 关节与关节状态发布器 有关话题/robot/joint_states
- 理解tf
- 直接指定关节组件角度
- 查看机器人元素的tf树
- MoveIt 简介
-
- 控制真实的Baxter机器人
- 反向运动
- 使用状态机实现YMCA
第一章 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
4.加载源文件setup.bash
~/catkin_ws$ source ~/catkin_ws/devel/setup.bash
~/catkin_ws$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
查看工作空间： ~/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
-
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
- 控制乌龟移动的ROS服务
通过/turtle1/pose 话题获得乌龟的位置信息
$ rostopic type /turtle1/pose
$ rosmsg show turtlesim/Pose
获得乌龟的位置坐标、方位角度和速度参数
$ 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
安装$ 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
| 切换到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
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/
-
描述机器人模型，两个基本的URDF组件定义的一个树状结构。
link组件描述了刚体的物理学属性（维度、初始位置、颜色等）
joint组件描述了连接的运动学以及动态属性（连接的连接杆、关节类型、旋转轴、摩擦力和阻尼的合力等）
~/catkin_ws/src/ros_robotics/urdf/dd_robotic.urdf:
<robot name="dd_robot">
<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>
包含一个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:
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" textfile="$(find ros_robotics)/urdf/$(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<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" />
launch>
在Rviz中查看机器人模型：
(1) 命令行中设置的模型加载到参数服务器
(2) 启动发布了JointState和转换的节点
(3) 启动使用urdf.rviz文件配置的Rviz
$ roslaunch ros_robotics ddrobot_rviz.launch model:=dd_robot.urdf
例 roslaunch
-
机器人描述：robot_description是URDF文件中存储的ROS参数的名称，文件存储于参数服务器中。连接杆组件和关节组件的描述以及两者的连接方式也存储于机器人描述中。
-
添加轮子、添加小脚轮、添加颜色、添加碰撞属性、添加物理学属性。
~/catkin_ws/src/ros_robotics/urdf/dd_robotic6.urdf:
<robot name="dd_robot">
<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>
<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>
<visual name="caster">
<origin xyz="0.2 0 -0.125" rpy="0 0 0" />
<geometry>
<sphere radius="0.05" />
geometry>
visual>
<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>
<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>
<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>
<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>
<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>
<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>
<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>
$ 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标签
标签必须要添加到URDF文件中。若 标签没有和reference=“”属性一起使用的话，则意味着对应的 元素将应用到整个机器人模型之中。 -
在gazebo中指定颜色
在Rviz中指定颜色的方法在gazebo中不适用，需要为每一个连接杆组件指定一个gazebo
标签。 -
gazebo中的
和 属性描述
~/catkin_ws/src/ros_robotics/urdf/dd_robotic.gezebo:
<robot name="dd_robot">
<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>
<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>
<visual name="caster">
<origin xyz="0.2 0 -0.125" rpy="0 0 0" />
<geometry>
<sphere radius="0.05" />
geometry>
visual>
<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/Bluematerial>
<pose>0 0 3 0 0 0pose>
gazebo>
<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>
<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/Blackmaterial>
gazebo>
<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>
<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>
<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/Blackmaterial>
gazebo>
<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>
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>
<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>
<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>
由gazebo_ros功能包中的empty_world.launch修改而来，改变了world_name参数，变为了ddrobot.world。
基于URDF的dd_robot机器人模型使用来自gazebo_ros ROS节点的spawn_model服务启动于Gazebo中，若想要复用或者分享上述代码，建议在自己的ros_robotics功能包的package.xml文件中添加相应的依赖：
- ddrobot.world环境模型文件包含一个地平面模型和两个圆锥形的建筑模型
~/catkin_ws/src/ros_robotics/world/ddrobot.world:
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_planeuri>
include>
<include>
<uri>model://sunuri>
include>
<include>
<uri>model://construction_coneuri>
<name>construction_conename>
<pose>-3.0 0 0 0 0 0pose>
include>
<include>
<uri>model://construction_coneuri>
<name>construction_conename>
<pose>3.0 0 0 0 0 0pose>
include>
world>
sdf>
在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
vim ~/.bashrc
export ROS_MASTER_URI=http://localhost:11311/
export ROS_HOSTNAME=localhost
export TURTLEBOT3_MODEL=burger
source ~/.bashrc
在riviz中启动turtlebot3模拟软件
roslaunch turtlebot3_fake turtlebot3_fake.launch
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
在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
实际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
检测ping
4.如果ping能够建立但SSH不能正常工作，重新安装SSH
sudo apt-get remove openssh-client openssh-server
sudo apt-get install openssh-client openssh-server
第四章 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
运行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
使用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
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
s username>SSH登陆Turtlebot后使用pwd命令查看
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
导航至目标点
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 }}}'
基于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代码移动机器人
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")
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
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
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:
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="width" value="0.1" />
<xacro:property name="height1" value="2" />
<xacro:property name="height2" value="1" />
<xacro:property name="height3" value="1" />
<xacro:property name="axle_offset" value="0.05" />
<xacro:property name="damp" value="0.7" />
<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 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>
<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 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>
<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>
包含三连杆元素，长宽均为0.1m，高分别为2m，1m，1m。base_link的原点坐标为（0,0,1），目的在于机械臂在Rviz下能够位于地面之上。
关节元素两个新的标签：
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)'" />
~/catkin_ws/src/ros_robotics/launch/rrbot_rviz.launch:
<launch>
<arg name="model" />
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ros_robotics)/urdf/$(arg model)'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotics)/urdf.rviz" required="true" />
launch>
需要将.bashrc文件(主目录隐藏文件)中已经添加了自己的环境变量ROS_MASTER_URI和ROS_HOSTNAME或ROS_IP注释掉，添加以下两行
export ROS_MASTER_URI=http://localhost:11311//
export ROS_HOSTNAME=localhost
运行rviz的roslaunch命令:
$ roslaunch ros_robotics rrbot_rviz.launch model:=rrbot.xacro
3.添加颜色
~/catkin_ws/src/ros_robotics/urdf/materials.xacro:
<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>
在每一个连接杆组件添加
<xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />
4.惯性的宏使用
<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>
在每一个link里，使用以下代码替换完整的
<xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1"/>
5.添加抓手
使用来自PR2机器人的网格文件
**主文件rrbot4.xacro中添加：**
```xml
-
四个连接杆分别为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>
~/catkin_ws/src/ros_robotics/urdf/gripper.xacro:
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<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/SimpleTransmissiontype>
<joint name="left_gripper_joint">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="gripper_motor1">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<transmission name="gripper_transmission2">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="right_gripper_joint">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="gripper_motor2">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
robot>
~/catkin_ws/src/ros_robotics/urdf/rrbot3.xacro:
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="width" value="0.1" />
<xacro:property name="height1" value="2" />
<xacro:property name="height2" value="1" />
<xacro:property name="height3" value="1" />
<xacro:property name="axle_offset" value="0.05" />
<xacro:property name="damp" value="0.7" />
<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>
<xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />
<xacro:include filename="$(find ros_robotics)/urdf/gripper.xacro" />
<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 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>
<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 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>
<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>
运行Rviz的roslaunch命令
$ roslaunch ros_robotics rrbot_rviz.launch model:=rrbot3.xacro
6.在Gazebo中控制关节式机器人手臂
gazebo下模拟环境下所独有的特性元素主要有
标签，指定每一个连接杆的颜色或纹理 和 标签，用于定义四个表面相互接触的机器人连接杆的摩擦系数 - 用于控制rrbot的旋转副关节的插件
这些特定的Gazebo XML元素，存储在单独的文件中：
~/catkin_ws/src/ros_robotics/urdf/rrbot.gazebo:
<robot>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/rrbotrobotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
plugin>
gazebo>
<gazebo reference="base_link">
<material>Gazebo/Redmaterial>
gazebo>
<gazebo reference="mid_link">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Greenmaterial>
gazebo>
<gazebo reference="top_link">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Bluematerial>
gazebo>
<gazebo reference="left_gripper">
<mu1>0.2mu1>
<mu2>0.2mu2>
gazebo>
<gazebo reference="right_gripper">
<mu1>0.2mu1>
<mu2>0.2mu2>
gazebo>
<gazebo reference="left_tip" />
<gazebo reference="right_tip" />
robot>
在主文件使用Xacro的声明
将机器人手臂固定在世界坐标系下
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
joint>
~/catkin_ws/src/ros_robotics/urdf/rrbot4.xacro:
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.14"/>
<xacro:property name="width" value="0.1" />
<xacro:property name="height1" value="2" />
<xacro:property name="height2" value="1" />
<xacro:property name="height3" value="1" />
<xacro:property name="axle_offset" value="0.05" />
<xacro:property name="damp" value="0.7" />
<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>
<xacro:include filename="$(find ros_robotics)/urdf/materials.xacro" />
<xacro:include filename="$(find ros_robotics)/urdf/gripper.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>
<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 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>
<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 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>
<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/SimpleTransmissiontype>
<joint name="joint_base_mid">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<transmission name="transmission2">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="joint_mid_top">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="motor2">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
robot>
launch文件：
~/catkin_ws/src/ros_robotics/launch/rrbot_gazebo.launch:
<launch>
<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>
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ros_robotics)/urdf/rrbot4.xacro'" />
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-param robot_description -urdf -model rrbot" />
launch>
在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 name="transmission1">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="joint_base_mid">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
上述代码在rrbot4.xacro中
（2）添加Gazebo的ROS控制插件
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/rrbotrobotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
plugin>
gazebo>
上述代码在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}
（4）创建控制启动文件
初始化机器人手臂rrbot的最佳方法是生成用于将参数加载至参数服务器的启动文件，并通过该文件启动所有的ros_control控制器。配置文件YAML文件对控制器进行设置,rosparam声明将控制器设置加载至参数服务器。然后control_spawner节点使用controller_manager包为rrbot生成五个控制器。此外还将为robot_state_publisher启动另一个节点。
~/catkin_ws/src/ros_robotics/launch/rrbot_control.launch:
<launch>
<rosparam file="$(find ros_robotics)/config/rrbot_control.yaml" command="load"/>
<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"/>
<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>
在节点启动之后，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_managerexec_depend>
<exec_depend>robot_state_publisherexec_depend>
采用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
安装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
配置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变量
安装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
在Gazebo中启动Baxter模拟器
启动Baxter模拟器
cd ~/baxter_ws
./baxter.sh sim
检查ROS环境
env | grep ROS
启动控制器，开始模拟操作
roslaunch baxter_gazebo baxter_world.launch
(gazebo死机 移除所有gezebo进程 killall gzserver)
故障说明
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
样例
在模拟的机器人头部显示屏显示一张图片
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
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()
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
理解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()
python编译成可执行
chmod +x arms_to_zero_angles.py
运行脚本
python arms_to_zero_angles.py
直接指定关节组件角度
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']}"
参考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
控制真实的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>
其他示例参考 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
利用反向运动移动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
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()
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())
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()
使用状态机实现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()
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()
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()
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
.