ROS双臂定点抓取问题记录

平台：ROS-Melodic+MoveIt！
编程语言：C++
机械臂模型：KINOVA-j2s6s200

双臂抓取基本思路分主臂从臂，先后规划并到达各自抓取点，然后对从臂末端添加相对于主臂末端的运动学约束，通过求解逆运动学并规划添加路径点形成轨迹实现双臂同时运动。

问题一

• 主臂抓取点位姿问题

最开始的时候没有添加运动学约束，只是通过添加两个机械臂的路径点和C++的API生成轨迹，然后取出轨迹中规划好的路径点给非链式的双臂整体movegroup循环赋关节值，实现双臂同时运动。主臂抓取点设置的语段如下：

	std::vector<moveit_msgs::Grasp> grasps;
	grasps.resize(1);

	grasps[0].grasp_pose.header.frame_id = "root";
	tf2::Quaternion orientation;
	orientation.setRPY(0, -M_PI / 2, M_PI / 2);
	grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
	grasps[0].grasp_pose.pose.position.x = 0.6;
	grasps[0].grasp_pose.pose.position.y = -0.3;
	grasps[0].grasp_pose.pose.position.z = 0.35;

在我直接给路径点赋值的时候这段运行的很顺利，但是在我尝试加入运动学以后这个抓取点在终端一直提示没有规划出来的解决方案，无法执行。如图：

以为是目标物体挡住了主臂的规划路线，但是去掉以后仍然无法规划。图中从臂规划顺利，主从臂姿态对称，从臂姿态：

orientation1.setRPY(0, M_PI