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 gripperdouble 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。