Ros2- Moveit2- Subrame( 子坐标 )
子坐标是在CollisionObjects上定义的坐标。
它们可用于定义您放置在场景中的对象上的兴趣点，例如瓶子的开口、螺丝刀的尖端或螺丝的头部。它们可用于规划和编写机器人指令，例如“拿起瓶子，然后将开口移到水龙头的喷口下方”，或“拿起螺丝刀，然后将其放在螺丝头上方”。
编写专注于机器人操纵对象的代码不仅更易读，而且在机器人之间也更强大、更易于移植。本教程将向您展示如何在碰撞对象上定义子坐标，将它们发布到规划场景并使用它们来规划运动，以便您可以执行以下操作：
运行演示
完成入门中的步骤后，打开两个终端。在第一个终端中，执行此命令加载 panda，然后等待所有内容加载完成：
roslaunch panda_moveit_config demo.launch
在第二个终端运行教程：
rosrun moveit_tutorials subframes_tutorial
在这个终端您应该能够输入 1-12 的数字来发送命令，并查看机器人和场景如何反应。
代码
此示例的代码可以在 moveit_tutorials GitHub 项目中看到，并在下面详细说明。
代码在规划场景中生成一个盒子和一个圆柱体，将圆柱体连接到机器人，然后让您通过命令行发送运动命令。它还定义了两个方便的函数，用于发送运动命令和发布对象。
使用子坐标定义两个 CollisionObjects
此辅助函数创建两个对象并将它们发布到 PlanningScene：一个盒子和一个圆柱体。盒子出现在夹持器前面，圆柱体出现在夹持器的尖端，就像被抓取一样。
void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface) { double z_offset_box = .25; // The z-axis points away from the gripper double z_offset_cylinder = .1;
首先，我们像往常一样开始定义CollisionObject 。
moveit_msgs::CollisionObject box; box.id = "box"; box.header.frame_id = "panda_hand"; box.primitives.resize(1); box.primitive_poses.resize(1); box.primitives[0].type = box.primitives[0].BOX; box.primitives[0].dimensions.resize(3); box.primitives[0].dimensions[0] = 0.05; box.primitives[0].dimensions[1] = 0.1; box.primitives[0].dimensions[2] = 0.02; box.primitive_poses[0].position.z = z_offset_box;
然后，我们定义 CollisionObject 的子坐标。子坐标在frame_id坐标系中定义，就像组成对象的形状一样。每个子坐标都包含一个名称和一个姿势。在本教程中，我们设置子坐标的方向，使子坐标的 z 轴指向远离对象的方向。这不是绝对必要的，但遵循惯例是有帮助的，并且可以避免在稍后设置目标姿势的方向时产生混淆。
box.subframe_names.resize(5); box.subframe_poses.resize(5); box.subframe_names[0] = "bottom"; box.subframe_poses[0].position.y = -.05; box.subframe_poses[0].position.z = 0.0 + z_offset_box; tf2::Quaternion orientation; orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0); box.subframe_poses[0].orientation = tf2::toMsg(orientation);
最后，将对象发布到 PlanningScene。在本教程中，我们发布一个盒子和一个圆柱体。
box.operation = moveit_msgs::CollisionObject::ADD; cylinder.operation = moveit_msgs::CollisionObject::ADD; planning_scene_interface.applyCollisionObjects({ box, cylinder }); }
创建计划请求
在本教程中，我们使用一个小辅助函数来创建规划请求并移动机器人。
bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, const std::string& end_effector_link) {
要在规划中使用附在机器人上的对象的子坐标，您需要将 move_group 的末端效应器设置为对象的子坐标。格式必须是object_name/subframe_name，如“示例 1 ”行中所示。当您分离对象时，不要忘记将您的 end_effector_link 重置为机器人链接，并且子坐标不再是机器人的一部分！
group.clearPoseTargets(); group.setEndEffectorLink(end_effector_link); /* group.setEndEffectorLink("cylinder/tip"); // Example 1 group.setEndEffectorLink("panda_hand"); // Example 2 */ group.setStartStateToCurrentState(); group.setPoseTarget(pose);
其余的规划工作照常进行。当然，你也可以使用命令go()代替 plan()和execute()。
ROS_INFO_STREAM("Planning motion to pose:"); ROS_INFO_STREAM(pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z); moveit::planning_interface::MoveGroupInterface::Plan myplan; if (group.plan(myplan) && group.execute(myplan)) return true; ROS_WARN("Failed to perform motion."); return false; }
准备场景
在主函数中，我们首先在规划场景中生成对象，然后将圆柱体附加到机器人上。附加圆柱体后，它在 Rviz 中变为紫色。
spawnCollisionObjects(planning_scene_interface); moveit_msgs::AttachedCollisionObject att_coll_object; att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; ROS_INFO_STREAM("Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object);
交互式测试机器人
我们设置了一个小型命令行界面，以便您可以与模拟进行交互，并查看它如何响应某些命令。您可以使用它来试验机器人在移除盒子和圆柱体、重生并重新连接它们或创建新的规划请求时的行为。尝试将机器人移动到新位置并在那里重生盒子和圆柱体（它们是相对于机器人手腕生成的）。或者尝试命令 7 和 8 将不同的帧移动到空间中的相同位置。
命令“2”将圆柱尖端移动到盒子的顶部（顶部动画中的右侧）。
else if (character_input == 2) { ROS_INFO_STREAM("Moving to top of box with cylinder tip"); target_pose.header.frame_id = "box/top"; target_orientation.setRPY(180.0 / 180.0 * M_PI, 0, 90.0 / 180.0 * M_PI); target_pose.pose.orientation = tf2::toMsg(target_orientation); target_pose.pose.position.z = 0.01; showFrames(target_pose, "cylinder/tip"); moveToCartPose(target_pose, group, "cylinder/tip"); }
设置方向
目标姿势相对于箱体子坐标给出：
target_pose.header.frame_id = "box/bottom";
方向由 RPY 角度决定，以对齐圆柱体和箱形副车架：
target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI); target_pose.pose.orientation = tf2::toMsg(target_orientation);
为了与盒子保持一定的距离，我们使用一个小的偏移：
target_pose.pose.position.z = 0.01; showFrames(target_pose, "cylinder/tip"); moveToCartPose(target_pose, group, "cylinder/tip");
技术说明
TF 无法识别子坐标，因此它们不能在 MoveIt 规划请求之外使用。如果您需要对子坐标进行变换，可以使用getFrameTransform函数从PlanningScene中 获取它。这将返回一个Eigen::Isometry3d对象，您可以从中提取平移和四元数（请参阅此处）。然后可以使用平移和四元数来创建变换并将其注册到您的 TFListener中。
可视化子坐标
目前没有子坐标的可视化！
故障排除
对于您最近没有自己生成的旧版 moveit_config 包，可能没有配置用于子坐标的规划适配器，可能找不到子坐标链接。要修复您的 moveit_config 包，请打开机器人配置文件中的 ompl_planning_pipeline.launch 文件，该文件位于 <robot_moveit_config>/launch 文件夹中。以 Panda 机器人为例，这就是这个文件。编辑此启动文件，找到提到 <arg name="planning_adapters"> 的行，然后在 default_planning_request_adapters/FixStartStatePathConstraints 之后插入 default_planning_request_adapters/ResolveConstraintFrames。
