ROS机械臂开发：从入门到实践---学习笔记（6）

第六章作业
第一题采用的是ur3机械臂，第二题采用的是老师的模型Probot_anno机械臂。过程如下：
第一题 编写一个程序，实现以下运动控制功能：
（1）在关节空间下，机械臂从起点A运动到B点（关节位置描述），再从B点运动到C点（终端姿态描述）；
（2）在笛卡尔空间下，机械臂从C点一次直线运动到D点和E点，最后回到初始A点。
1.1 第一小问
1.1.1 关节空间规划
关节空间运动是以关节角度为控制量的机器人运动。虽然各关节达到期望位置所经过的时间相同，但是各关节之间相互独立，互不影响。机器人状态使用各轴位置来描述，在指定运动目标的机器人状态后，通过控制各轴运动来达到目标位姿。
无论是关节位置描述还是终端姿态描述程序的大致内容是相同的，不同的是设置目标的姿态。
程序步骤：
a)创建规划组的控制对象；
b)获取机器人的终端link全称；
c)设置目标位姿对应的参考坐标系和起始、终止位姿；
d)完成规划并控制机械臂完成运动。
程序如图1-1，1-2所示。

图1-1 程序1

图1-2 程序2
关键API使用流程如下：
I.arm = moveit_commander.MoveGroupCommander(‘manipulator’)
II.end_effector_link = arm.get_end_effector_link()
III.reference_frame = ‘base_link’
IV.arm.set_pose_reference_frame(reference_frame)
V.arm.set_start_state_to_current_state()
VI.arm.set_pose_target(C, end_effector_link)
VII.arm.go()
由A（起始点）到B点运行效果如图1-3所示，由B到C点运行效果如图1-4所示。

图1-3 A到B

图1-4 B到C
1.2 第二小问
1.2.1 笛卡儿运动规划
工作空间中的运动规划并没有对机器人终端轨迹有任何约束，目标位姿给定后，可移动过运动学反解获得关节空间下的各轴弧度，接下来的规划和运动依然在关节空间中完成。但是在很多应用场景中，我们不仅关心机械臂的起始、终止位姿，对运动过程中的位姿也有要求，比如希望机器人终端能够走出一条直线或圆弧轨迹。
相比第一小问的代码显得略微复杂，因为需要兼容两种运动模式。在不需要使用笛卡儿运动规划的模式下，与第一小问几乎相同，所以主要分析笛卡儿路径规划部分的代码。
这里需要了解“waypoints”的概念，也就是路点。waypoints是一个路点列表，意味着笛儿路径中需要经过的每个位姿点，相邻两个路点之间使用直线轨迹运动。将运动需要经过的路点都加入路点列表中，但此时并没有开始运动规划，代码如下：

waypoints = []
if cartesian:
waypoints.append(start_pose)

整个笛卡儿运动规划的核心部分是使用了笛卡儿路径规划的API—compute_cartesian_path()，它共有四个参数：第一个参数是之前创建的路点列表；第二个参数是终端步进值；第三个参数是跳跃阈值；第四个参数用于设置运动过程中是否考虑避障。代码如下：

while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")

compute_cartesian_path()执行后会返回两个值：plan是规划出来的轨迹；fraction用于描述规划成功的轨迹在给定路点列表的覆盖率，从0到1。如果fraction小于1，说明给定的路点列表没办法完整规划，这种情况可以重新进行规划，但需要人为设置规划次数。代码如下：

if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")

笛卡儿运动规划的关键是了解compute_cartesian_path()这个API的使用方法，可以帮助我们实现一些列路点之间的笛卡尔直线运动规划。如果希望机器人的终端走出圆弧轨迹，也可以将圆弧分解为多段直线，然后使用compute_cartesian_path()控制机器人运动。
仿真结果如图1-5所示。

图1-5 第一题整体效果图
第二题 编写程序实现自主避障功能
2 避障规划
在很多应用场景下，机器人工作环境中会有一些障碍物，所以机械臂在进行运动规划时需要考虑避障问题。MoveIt！中默认使用的运动规划库OMPL支持避障规划，可以使用move_group中的planning scene插件的相关接口加入障碍物模型，并且维护机器人工作的场景信息。
2.1 避障规划主要流程
a)初始化场景，设置参数；
b)在可视化环境中加入障碍物模型；
c)设置机器人的起始位姿和目标位姿；
d)进行避障规划。
2.2 简单分析代码实现过程

from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, PlanningScene

PlanningSceneInterface接口为我们提供了添加、删除物体模型的功能，PlanningScene消息是场景更新话题planning_scene订阅的消息类型。

scene = PlanningSceneInterface()

创建一个PlanningSceneInterface类的实例，通过这个实例可以添加或者删除物体模型。

scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table') 
scene.remove_world_object('target')

代码可以在终端中重复运行，但之前加载的物体模型并不会自动清除，需要remove_ world_object()清除指定的物体模型。

p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = -0.015
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'tool', p, tool_size)

设置物体在场景中的位置，使用PoseStamped消息描述。确定物体的位置后，使用PlanningSceneInterface的addtach_box()，接口将物体吸附在机器人的末端关节上。然后设置目标的位姿，与第一题第一小问的方式相同，接着就可以让机器人开始运动了，整体效果如图2-1所示。

图2-1 机械臂避障