【MoveIt】入门教程-第一章（下）Move Group C++ Interface（官方教程翻译+个人补充）

环境：Ubuntu20.04 + ros-noetic + moveit1
安装教程：https://blog.csdn.net/qq_43557907/article/details/125081735
上篇：https://blog.csdn.net/qq_43557907/article/details/125124633
本文为我的个人笔记，是翻译的官方手册。因为我个人插入了图片和注释加以补充，所以不提倡转载。
我只是一个小白，文中肯定会有一些错误，欢迎指正。
本篇文章为 Move Group C++ Interface 的下半部分。

9.路径约束的规划 Planning with Path Constraints

在机器人关节上，路径约束可以很容易地指定。让我们为 group 指定一个路径约束和一个目标位姿。第一步，定义路径约束：

// 创建一个方向约束信息
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
// 设置公差
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;

然后，设定其作为 group 的路径约束。

// 创建一个约束信息
moveit_msgs::Constraints test_constraints;
// 将上述方向约束信息添加到此约束信息中
test_constraints.orientation_constraints.push_back(ocm);
// 将此约束信息添加到环境约束中
move_group_interface.setPathConstraints(test_constraints);

10.在关节空间中执行规划

Moveit 可以选择在关节空间或笛卡尔空间来描述规划问题。

在 ompl_planning.yaml 文件中设定 group 的参数enforce_joint_model_state_space:true ，这样就可以在关节空间中执行所有规划。

默认情况下，通过方向路径约束的规划需要在笛卡尔空间中进行采样，以便调用 IK 作为生成式采样器。

使用关节空间的话，规划进程将使用拒绝采样去寻找有效请求，这很有可能增加规划时间。

我们将重复使用之前规划好的目标，工作前提是当前状态已经满足路径约束，所以我们需要将机械臂的起始状态设置到一个新的位姿（见上图）。

// 获得机械臂当前的状态
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
// 创建新的起始点位姿
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
// 根据新的位姿，计算运动学逆解，得到新的起始状态
start_state.setFromIK(joint_model_group, start_pose2);
// 更新起始状态
move_group_interface.setStartState(start_state);

现在，我们要从新的起始状态规划到之前创建的目标点

// 设置目标位姿
move_group_interface.setPoseTarget(target_pose1);

带有约束的规划可能会很慢，因为每一个样本都必须请求一个运动学逆解。让我们增加规划时间，从默认的5s到确保有足够的时间去成功规划（这里设置成10s）。

// 设置规划时间
move_group_interface.setPlanningTime(10.0);

success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

在 rviz 中可视化规划

visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");

当路径约束完成后，确保清除它

// 清除路径约束
move_group_interface.clearPathConstraints();

11.笛卡尔路径

通过指定一个末端执行器的路径点列表，你可以直接规划一个笛卡尔路径。我们从上文的新起始状态开始，最初的位姿（起始状态）不需要去添加到路径点列表中，但是添加的话有助于可视化。

// 创建一个 vector 容器，用来储存路径点
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(start_pose2);

geometry_msgs::Pose target_pose3 = start_pose2;

target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3);  // down

target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // right

target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // up and left

我们想将笛卡尔路径设置为 1cm 的分辨率，所以指定0.01作为笛卡尔变换中的最大步幅。我们设置跳跃阈值 jump threshold 为 0.0，代表不允许跳跃。

关于jump threshold：https://blog.csdn.net/Bobby95/article/details/118663350

规划的关节连续运动之间的距离<=Jump_threshold*关节位置的平均距离/绝对距离

用于在笛卡尔空间运动时， 避免在经过奇异点时产生关节的抖动 。

警告：当操作真实的硬件时禁用 jump threshold 会引起较大不可预测的冗余关节的动作，成为一个安全问题。

moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0; // 跳跃阈值
const double eef_step = 0.01; // 终端步进值
// 规划路径 ，fraction返回1代表规划成功
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

在 rviz 中可视化规划

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
  visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

笛卡尔动作通常是缓慢的。例如当接近对象时，笛卡尔规划的速度不能顺利地通过 maxVelocityScalingFactor 设置，但是需要你手动计算轨迹的时间，正如 这里 所描述的。

你可以像这样实现一个轨迹：

move_group_interface.execute(trajectory);

12.向环境中添加一个物体

首先，让我们在无物体的情况下规划到另一个简单的目标。

move_group_interface.setStartState(*move_group_interface.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.x = 1.0;
another_pose.position.x = 0.7;
another_pose.position.y = 0.0;
another_pose.position.z = 0.59;
move_group_interface.setPoseTarget(another_pose);

success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");

现在，让我们定义一个让机器人去避开的碰撞对象信息。

moveit_msgs::CollisionObject collision_object;
// 这个对象参考机器人的参考坐标系，即世界坐标系
collision_object.header.frame_id = move_group_interface.getPlanningFrame();

设置碰撞对象的 id

collision_object.id = "box1";

定义一个形状 box，并将其添加到 world 中

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;

定义 box 的位姿（相对于 frame_id）

geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.5;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;
// 将形状 box 及其位姿添加到碰撞对象中
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
// 创建一个碰撞对象集，并添加上述碰撞对象
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

现在，让我们添加这个碰撞对象集到 world 中

ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

在 rviz 中展示状态文本，等待 MoveGroup 接受和处理碰撞对象信息。

visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

现在，当我们规划一个轨迹时，它将会避开障碍物

success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

13.为机器人链接对象

你可以为机械臂链接一个对象，使其随着机械臂几何学而移动。这个仿真拾起了待操作的对象，其动作规划应该避免两个对象间的碰撞。

moveit_msgs::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";

shape_msgs::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

我们为这个圆柱定义坐标系/位姿，使其出现在夹具中。

object_to_attach.header.frame_id = move_group_interface.getEndEffectorLink();
geometry_msgs::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;

首先，我们添加对象到 world（不用vector）

object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
// 注意，添加单个对象时，这里是 apply 而不是 add
planning_scene_interface.applyCollisionObject(object_to_attach);

然后，我们链接物体到机械臂上，它使用 frame_id 去决定哪个关节是要链接的。你可以使用 applyAttachedCollisionObject 去直接链接一个物体到机械臂上。

ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group_interface.attachObject(object_to_attach.id, "panda_hand");

visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");

重新规划轨迹，现在机械爪中有物体了。

move_group_interface.setStartStateToCurrentState();
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

14.分离并移除物体

现在，让我们使圆柱从机械臂夹具上分开。

ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group_interface.detachObject(object_to_attach.id);

在 rviz 中展示状态信息

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");

现在，让我们从 world 中移除物体

ROS_INFO_NAMED("tutorial", "Remove the objects from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
object_ids.push_back(object_to_attach.id);
planning_scene_interface.removeCollisionObjects(object_ids);

在 rviz 中展示状态信息

visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears");

15.launch 文件

完整的 launch 文件在 here 。教程中的所有代码都可以通过 moveit_tutorials 包来运行，前提是你已经安装好 Moveit。

<launch>

  <node name="move_group_interface_tutorial" pkg="moveit_tutorials" type="move_group_interface_tutorial" respawn="false" output="screen">
  </node>

</launch>