1. 工作空间运动

机械臂关节空间的规划是给定各轴位置，不需要考虑机器人终端的姿态
与之相对应的是工作空间规划，机械臂的目标位姿不再通过各轴位置给定
而是通过机器终端的三维坐标位置和姿态给定，在运动规划时使用逆向运动学求解各轴位置
这会也就是“指哪打哪”了

例如：
控制机器人终端在word坐标系下的x轴方向上移动5cm，同时围绕z轴旋转10°
就要将该位姿反解得到各轴位置，然后再进行规划、运动

2. 运动规划

Movelt!支持工作空间下的目标位姿设置，创建一个moveit_ik_demo.py文件运行关节空间规划

import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveItIkDemo:
    def __init__(self):

        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化moveit_ik_demo节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # 设置机械臂的目标位置为自定义位姿中的home
        arm.set_named_target('home')

        # 控制机械臂规划和运动期望位置
        arm.go()

        # 保存一段时间的延时，确保机械臂已经完成运动
        rospy.sleep(2)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.191995
        target_pose.pose.position.y = 0.213868
        target_pose.pose.position.z = 0.520436
        target_pose.pose.orientation.x = 0.911822
        target_pose.pose.orientation.y = -0.0269758
        target_pose.pose.orientation.z = 0.285694
        target_pose.pose.orientation.w = -0.293653
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)
         
        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItIkDemo()
    except rospy.ROSInterruptException:
        pass

代码相对较多，下面对一部分代码做一个简单解析

3. 部分代码解析

获取终端link的名称
```
end_effector_link = arm.get_end_effector_link()
```
HHArm的终端是二指夹爪，也就是模型中的grasping_frame
可以使用get_end_effector_link接口获取该模型文件中终端link的名称，该名称在后续规划时要用到
设置目标位置所使用的参考坐标系
```
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
```
工作空间的位姿需要使用笛卡尔坐标值进行描述，所以必须声明该位姿所在的坐标系
set_pose_reference_frame用于设置目标位姿所在的坐标系
这里设置为机器人的基坐标系——base_link
当运动规划失败后，允许重新规划
```
arm.allow_replanning(True)
```
机械臂反向运动学求解时存在无解或多解的情况，其中有些情况可能无法实现运动规划
这种情况下可以使用allow_replanning设置是否允许规划失败之后的重新规划
如果设置为true，Movelt!会尝试求解五次，否则只求解一次

设置机械臂工作空间中的目标位姿

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()     
target_pose.pose.position.x = 0.191995
target_pose.pose.position.y = 0.213868
target_pose.pose.position.z = 0.520436
target_pose.pose.orientation.x = 0.911822
target_pose.pose.orientation.y = -0.0269758
target_pose.pose.orientation.z = 0.285694
target_pose.pose.orientation.w = -0.293653

使用ROS中的PoseStamped消息数据描述机器人的目标位要
首先需要设置位姿所在的参考坐标系，基于base_link坐标系
然后创建时间戳
接着设置目标位姿的x、y、z坐标值和四元数姿态值

设置机器臂运动初始状态和目标位姿
```
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
```
在输入目标位姿前，需要设置运动规划的起始状态
一般情况下使用set_start_state_to_current_state接口设置当前状态为起始状态
然后使用set_pose_target设置目标位姿，同时需要设置就是之前获取的终端link名称
规划路径
```
traj = arm.plan()
arm.execute(traj)
```
运动规划的第一步是规划路径，使用plan完成
如果路径规划成功，则会返回一条规划好的运动轨迹，然后execute会控制机器人沿轨迹完成运动
如果规划失败，则会根据设置项，重新进行规划，或者在终端中提示规划失败的日志信息
单轴方向上的目标设置和规划
```
# 控制机械臂终端向右移动5cm
arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()
# 控制机械臂终端反向旋转90度
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
```
除了使用PoseStamped数据描述目标位姿并进行运动规划外
也可以使用shift_pose_target实现单轴方向上的目标设置和规划
shift_pose_target有以下三个参数：
第一个参数：描述机器人需要在哪个轴向上进行运动； 0、1、2、3、4、5 分别对应于x、y、z、 r、 p、y，即 xyz 方向上的平移和围绕 xyz 三个轴的旋转
第二个参数：描述运动尺度；如果是平移运动，则单位为米；如果是旋转运动，单位为弧度
第三个参数：描述运动针对的终端link
首先让机器人终端在y轴的负方向上平移0.05米，然后围绕z轴反向旋转90度

总之，Movelt!工作空间下运动控制的主要API使用流程如下：

arm = moveit_commander.MoveGroupCommander('arm')

end_effector_link = arm.get_end_effector_link()

reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)

arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)

arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()

首先需要创建规划组的控制对象
然后获取机器人的终端link名称
其次设置目标位姿的参考坐标系
接着设置起始位姿和终止位姿，进行规划并控制机器人运动

4. 启动规划

启动arm_planning.launch文件启动机械臂
运行moveit_ik_demo.py文件启动工作空间规划

$ roslaunch hharm_planning arm_planning.launch
$ rosrun hharm_planning moveit_ik_demo.py

运行成功后，先运行到达指定目标点
然后终端在y轴的负方向上平移0.05米，接着围绕z轴反向旋转90度，最后复位
在这里插入图片描述
同时在终端中可以看到运动规划过程中的输出日志
其中包含KDL运动学求解器完成反向运动学求解的时间

在这里插入图片描述

参考：

ROS官方wiki
古月居

ROS笔记（34） 工作空间规划