ROS笔记（35） 笛卡尔运动规划
1. 笛卡尔运动规划
而在ROS笔记（34） 工作空间规划 中的运动规划并没有对机器人终端轨迹有任何约束
目标位资给定后，可以通过运动学反解获得关节空间下的各轴弧度
接下来的规划和运动依然在关节空间中完成
但是在很多应用场景中，不仅关心机械臂的起始、终止位姿，对运动过程中的位姿也有要求
比如希望机器人终端能够走出一条直线或圆弧轨迹
Movelt!同样提供笛卡尔运动规划的接口
笛卡尔何许人也？
人可能不认识，但他的哲学命题“我思故我在”一定听说过
不会哲学的数学家和物理学家，不是一个好的神学家
所运用的是笛卡尔坐标系，就是直角坐标系和斜坐标系的统称
2. 启动轨迹可视化机械臂
在hharm_planning功能包中创建一个arm_planning_with_trail.launch文件启动所需要的各种节点并且可视化终端轨迹
内容与arm_planning.launch的几乎一致，只是修改了rviz启动文件
添加了终端轨迹的可视化显示设置，可以更方便地对比运动效果
<!-- 启动rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find hharm_planning)/rviz/arm_paths.rviz" required="true" />
启动arm_planning_with_trail.launch文件
$ roslaunch hharm_planning arm_planning_with_trail.launch
3. 运动规划
创建一个moveit_cartesian_demo.py文件启动笛卡尔运动规划
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy
class MoveItCartesianDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化moveit_cartesian_demo节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“forward”姿态
arm.set_named_target('forward')
arm.go()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(start_pose)
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
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...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItCartesianDemo()
except rospy.ROSInterruptException:
pass
还增加了cartesian参数设置，方便看到不同类型规划的运动效果
4. 部分代码解析
相比之前的例程以上代码略显复杂，因为需要兼容两种运动模式
在不需要使用笛卡尔运动规划的模式下，与 ROS笔记（34） 工作空间规划 的工作空间规划相同
主要分析笛卡尔路径规划部分的代码
waypoints = []
if cartesian:
waypoints.append(start_pose)
这里需要了解 “waypoints” 的概念，也就是路点
waypoints 是一个路点列表，意味着笛卡尔路径中需要经过的每个位姿点，相邻两个路点之间使用直线轨迹运动
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
这里要说明一下为什么用深度复制deepcopy
如果直接用“=”，而start_pose是可变对象，只是拷贝了内存中的地址引用，两个对象的地址引用一样
所以两个对象的值会随着一方的修改而修改
而深度复制的值是完全隔离的，与复制对象是完全不同的两个对象
这样可以在最后执行调用规划时
后续才可以实现每个阶段累加修改，而不是某个变量取最后修改的状态
即，第二和第三步实现终端位置x-0.2，y-0.2的改变
然后在此基础上x+0.05，y+0.15，z-0.15的改变
并且最后start_pose维持不变
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...")
将运动需要经过的路点都加入路点列表中，但此时并没有开始运动规划
这是整个例程的核心部分，使用了笛卡尔路径规划的API compute_cartesian_path
它共有四个参数：
第一个参数：之前创建的路点列表
第二个参数：终端步进值
第三个参数：跳跃阈值，设置为0代表不允许跳跃
第四个参数：设置运动过程中是否考虑避障
compute_cartesian_path 执行后会返回两个值：plan 和 fraction
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.")
如果规划成功，fraction的值为1
此时就可以使用execute控制机器人执行规划成功的路径轨迹了
这个例程的关键是了解compute_cartesian_path 这个API的使用方法
可以实现一系列路点之间的笛卡尔直线运动规划
如果希望机器人的终端走出圆弧轨迹
也可以将圆弧分解为多段直线后，使用compute_cartesian_path控制机器人运动
5. 启动规划
运行moveit_cartesian_demo.py文件启动笛卡尔运动规划
rosrun hharm_planning moveit_cartesian_demo.py _cartesian:=True
可以看到运动轨迹，机器人终端以直线方式完成多个目标点之间的运动
再使用如下命令运行不带路径约束的运动规划
rosrun hharm_planning moveit_cartesian_demo.py _cartesian:=False
机器人终端的运动轨迹不再以直线轨迹运动
从以上两种运动规划的轨迹可以看出，笛卡尔运动规划需要保证机械臂运动的中间姿态
参考：
相关推荐：
ROS笔记（34） 工作空间规划
ROS笔记（33） 关节空间规划
ROS笔记（32） MoveIt!关节控制器
ROS笔记（31） ArbotiX关节控制器
ROS笔记（30） Movelt!配置文件
谢谢！