matlab机械臂工作空间代码_ROS中机械臂笛卡尔空间规划姿态求解无效-Moveit！

1　任务描述

在使用Moveit对机械臂运动进行规划时，我们定义Pose，然后规划，目标位姿Pose的定义代码如下：

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()     
target_pose.pose.position.x = -0.545
target_pose.pose.position.y = 0.426
target_pose.pose.position.z = 0.267

target_pose.pose.orientation.x = -0.628
target_pose.pose.orientation.y = 0.226
target_pose.pose.orientation.z = 0.708
target_pose.pose.orientation.w = 0.230        

2　错误表现

这里面xyz的定义是没有问题的，但是姿态这么定义有问题，查看源代码这里的orientation是单位四元数的表示，然而规划也总是失败的。具体表现为无论你定义为多少姿态总是运动到四元数为[0 0 0 1]的姿态位置。就是下面这个姿态

3　原因分析

经过查找原因是数据格式问题，我具体的操作方法是在rviz界面拖动机械臂到预定位置，然后使用 tf_echo 查看坐标变换数值，然后把这个Quaternion数值输入上面定义的位置，如下图

tf_echo 查看坐标变换数值

问题就出在这里，这里显示的变换四元数只保留了三位小数，而实际应该是8位小数，如下图形式

4　解决办法步骤

• rviz中拖动机械臂到合适位置，“Plan and Execute”使机械臂运动到目标位姿
• 运行下面代码查看目标位姿的pose表示

rosrun tf tf_echo 参考坐标系 目标坐标系

• 使用里面的 In RPY 数值，求解四元数（需要自己写代码），然后把这个四元数赋值给姿态数值

r, p, y = 1.567, 1.469, -3.080   # tf_echo 中查看的弧度表示
rm = ax2rm(r, p, y)   # 弧度2旋转矩阵
goal_quat = rm2q(rm)   #　旋转矩阵２四元数

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now() 
    
target_pose.pose.position.x = -0.545
target_pose.pose.position.y = 0.426
target_pose.pose.position.z = 0.267

target_pose.pose.orientation.x = goal_quat[0]
target_pose.pose.orientation.y = goal_quat[1]
target_pose.pose.orientation.z = goal_quat[2]
target_pose.pose.orientation.w = goal_quat[3]

５感悟

！！！当你代码流程没有问题，却怎么也找不到解决办法时，可以好好检查下使用到的数据格式是否一致正确