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 / 2, M_PI / 2);

我把不能运行的工作空间删了又把之前可以运行的工作空间拷回来重新catkin，但是仍然规划错误，主臂没有动作，奇怪的是今天之前这个抓取点一直是可达的，抓取也没有问题，同样的程序同样的配置今天却跑不通了…

————————————————————————————————————
解决：今天打开代码的时候灵光乍现，改掉了抓取函数中的参数：

void pick(moveit::planning_interface::MoveGroupInterface& move_group)
{
	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;


	grasps[0].pre_grasp_approach.direction.header.frame_id = "root";
	grasps[0].pre_grasp_approach.direction.vector.y = 0.001;//这里本来是y = 0;
	grasps[0].pre_grasp_approach.min_distance = 0.005;
	grasps[0].pre_grasp_approach.desired_distance = 0.015;

	grasps[0].post_grasp_retreat.direction.header.frame_id = "root";
	grasps[0].post_grasp_retreat.direction.vector.y = 0.001;//同样y = 0;
	grasps[0].post_grasp_retreat.min_distance = 0.005;
	grasps[0].post_grasp_retreat.desired_distance = 0.015;

	openGripper1(grasps[0].pre_grasp_posture);
	openGripper1(grasps[0].grasp_posture);

	move_group.pick("cylinder", grasps);
}

两个vector决定了末端的抓取和退出方向，当y = 0的时候相当于没有设定抓取和退出方向，导致规划器求解一直失败。

这段函数因为一开始不会用pick函数，所以从大佬那里kiao来的，实现的功能应该是机械臂末端设置抓取姿态，通过末端的微小移动抓取到物体再撤回到抓取姿态。改一下参数就可以实现末端没有进退的直接夹爪闭合，也就是末端直接运动到夹爪闭合的姿态，然后将物体attach到主臂末端，等待从臂规划。实现效果图：

问题二

• 双臂抓取物体后同时抬起

刚接触moveit的时候思路是给从臂末端添加一个相对于主臂的动态约束，但是后来发现这玩意基本上是行不通的，只能另找办法…可能有一些更加涉及底层的东西可以实现这个思路，但是现阶段能想到的办法只有同时控制两个机械臂的各个关节，实现两臂的同时运动（之前师姐给我建议用ros的message filter通过回调函数同时调用两个topic，然后保持接收时间戳同步，奈何搞了好几天也没搞明白回调函数的两个传参在进行机械臂控制的时候到底咋用，就放弃了）。

在moveit配置的时候除去夹爪的规划组定义了三个，group1主臂，group2从臂，body是不加基座的所有joint（非链式结构，只是添加了joint，也就是说body这个组规划的时候不能通过逆运动学求解器来计算状态）：

moveit::planning_interface::MoveGroupInterface group1("group1");
moveit::planning_interface::MoveGroupInterface group2("group2");
moveit::planning_interface::MoveGroupInterface body("body");

在主从臂分别到达抓取点以后，以抓取位姿为起点，到目标位姿为终点，先利用moveit的正运动学计算两条单臂规划组group1和group2末端轨迹：

moveit_msgs::RobotTrajectory trajectory1;
std::string end_effector_link1 = group1.getEndEffectorLink();
group1.setPoseReferenceFrame("root");
geometry_msgs::Pose start_pose1 = group1.getCurrentPose(end_effector_link1).pose;
std::vector<geometry_msgs::Pose> waypoints1;
    
    waypoints1.push_back(start_pose1);
start_pose1.position.z = 0.6;
	waypoints1.push_back(start_pose1);	
start_pose1.position.x = 0.5;
	waypoints1.push_back(start_pose1);
start_pose1.position.y = -0.15;
	waypoints1.push_back(start_pose1);
group1.computeCartesianPath(waypoints1, eef_step, jump_threshold, trajectory1);//这句计算出到达目标点的路径，储存到trajectory1中；

这样用cout可以输出trajectory1中的数据：各个关节角的角度、速度和加速度。其中关节角的值就是重点，通过循环语句将每个规划点的所有关节值提取出来给规划组body中的关节角赋值，并且让body再循环中持续运动到达目标点：

std::vector<double> target_pose_state(12);//这个数组里面一共存12个关节值，顺序为主臂的1-6再从臂的1-6；
int i, j, k;
for(i = 1; i <= 3; i++)//i的最终大小取决于规划出来的路径点数量；
{
	for(j = 0, k = 6; j < 6, k < 12; j++, k++)
    {
    	target_pose_state[j] = trajectory1.joint_trajectory.points[i].positions[j];
        target_pose_state[k] = trajectory2.joint_trajectory.points[i].positions[j];
    }
    body.setJointValueTarget(target_pose_state);
    body.move();
}

moveit中非链式结构的机械臂没法执行轨迹，所以只能用这种循环让机械臂连续移动，而且在rviz里面这个循环里的所有轨迹点都是通过一条规划轨迹运动的，可以说等价于轨迹运动。

trajectory中的轨迹点数量不固定，决定于规划路径的长度和每步步长限制，也就是computeCartesianPath()语句里中间那两个变量。如果双臂目标位置和起始位置距离过远导致机械臂有夸张的运动轨迹从而脱离抓取物，这时就要考虑在轨迹点中插入中间点，让两条臂规划的路线尽可能保持一致。

从臂轨迹规划点原理上是主臂轨迹点末端的位姿通过平移转换得到的（抓取物为刚性物体，所以两个臂的末端夹爪不会有相对位姿的改变，可以只考虑两个臂的位姿），可以通过对主臂的运动学建模得到从臂末端位姿，然后再求解逆运动学得到从臂关节值，在相应的关节空间控制步骤时赋值给body规划组的后六个关节，就能实现从臂规划点根据主臂规划点的位姿进行规划。

抬起时的路径较为简单时，比如仅仅在xyz轴上移动，通过路径点添加实现关节空间的运动就足够了，原因还是上面说的抓取刚性物体两臂末端相对位姿不变，所以仅在插入路径点时保持两臂末端固定方向的距离即可。