Python版本的Moveit如何实现路径约束

最近在作机械臂的控制任务,需要连续密集的调整机械臂末端的位置和姿态。

大家都知道,机械臂每次调整位姿,如果使用自由曲线的方式,机械臂末端肯定会绕老绕去,在空间里动作太大。

有时候末端仅仅移动1cm也需要绕很长的路径。

关于平移移动的问题,可以使用笛卡尔空间约束,强制让机械臂走直线,这样的话也可以实现满意的效果。

关键的问题是如何控制末端的姿态。

有时候,我们需要控制机械臂末端执行器在xyz三个轴的某个方向稍微旋转一定角度,但是步移动末端的位置。

这就很麻烦了,因为我们无法像平移运动一样用笛卡尔方法去约束。只能让算法以自由曲线的方式运动,这样的话又回到一开始的那个问题了:

即使末端叔敖为旋转一个很小的角度,末端也会产生很长的路径。

于是,我想到了路径约束这个方法。经过多方搜索,我发现Moveit这个接口好像只有C++才提供。

/*Let's create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;

moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
/* 一、将规划场景中的状态设置为上一个规划的最终状态 */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* 现在,让我们尝试移动到这个新的姿势目标 */
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);

/*但是,让我们对运动施加路径约束。*/
/*在这里,我们要求末端执行器保持水平 */
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "torso_lift_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", quaternion);

想知道如何使用python实现路径约束的功能,有人研究过吗?