子帧

子框架是定义在 CollisionObjects. 它们可用于定义您放置在场景中的对象上的兴趣点，例如瓶子的开口、螺丝刀的尖端或螺丝的头部。它们可用于规划和编写机器人指令，例如“拿起瓶子，然后将开口移到水龙头的喷口下方”，或“拿起螺丝刀，然后将其放在螺丝头上方”。

编写专注于机器人操纵对象的代码不仅更易读，而且在机器人之间也更强大和可移植。本教程向您展示如何在碰撞对象上定义子帧，将它们发布到规划场景并使用它们来规划运动，因此您可以执行以下操作：

../../../_images/subframes_tutorial_cylinder_demo.gif

在此动画中，机器人将圆柱体的尖端移动到盒子上的不同位置。

运行演示

完成以下步骤后 Getting Started, 打开两个终端。在第一个终端中，执行此命令加载熊猫，然后等待所有内容完成加载：:

roslaunch panda_moveit_config demo.launch

在第二个终端运行教程：:

rosrun moveit_tutorials subframes_tutorial

在这个终端您应该能够输入 1-12 的数字来发送命令，并查看机器人和场景如何反应。

代码

此示例的代码可见 here 在 moveit_tutorials GitHub 项目中，下面将详细说明。

代码在规划场景中生成一个盒子和一个圆柱体，将圆柱体连接到机器人，然后让您通过命令行发送运动命令。它还定义了两个便利函数，用于发送运动命令和发布对象。

Defining two CollisionObjects with subframes

This helper function creates two objects and publishes them to the PlanningScene: a box and a cylinder. The box spawns in front of the gripper, the cylinder at the tip of the gripper, as if it had been grasped.

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;

First, we start defining the CollisionObject as usual.

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;

Then, we define the subframes of the CollisionObject. The subframes are defined in the frame_id coordinate system, just like the shapes that make up the object. Each subframe consists of a name and a pose. In this tutorial, we set the orientation of the subframes so that the z-axis of the subframe points away from the object. This is not strictly necessary, but it is helpful to follow a convention, and it avoids confusion when setting the orientation of the target pose later on.

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);

Lastly, the objects are published to the PlanningScene. In this tutorial, we publish a box and a cylinder.

  box.operation = moveit_msgs::CollisionObject::ADD;
  cylinder.operation = moveit_msgs::CollisionObject::ADD;
  planning_scene_interface.applyCollisionObjects({ box, cylinder });
}

Creating the planning request

In this tutorial, we use a small helper function to create our planning requests and move the robot.

bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group,
                    const std::string& end_effector_link)
{

To use subframes of objects that are attached to the robot in planning, you need to set the end effector of your move_group to the subframe of the object. The format has to be object_name/subframe_name, as shown in the “Example 1” line. Do not forget to reset your end_effector_link to a robot link when you detach your object, and the subframe is not part of your robot anymore!

group.clearPoseTargets();
group.setEndEffectorLink(end_effector_link);
/*
group.setEndEffectorLink("cylinder/tip");    // Example 1
group.setEndEffectorLink("panda_hand");      // Example 2
*/
group.setStartStateToCurrentState();
group.setPoseTarget(pose);

The rest of the planning is done as usual. Naturally, you can also use the go() command instead of plan() and 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;
}

Preparing the scene

In the main function, we first spawn the objects in the planning scene, then attach the cylinder to the robot. Attaching the cylinder turns it purple in 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);

Interactively testing the robot

We set up a small command line interface so you can interact with the simulation and see how it responds to certain commands. You can use it to experiment with the behavior of the robot when you remove the box and cylinder, respawn and reattach them, or create new planning requests. Try moving the robot into a new position and respawn the box and cylinder there (they are spawned relative to the robot wrist). Or try commands 7 and 8 to move different frames to the same position in space.

The command “2” moves the cylinder tip to the top of the box (the right side in the top animation).

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");
}

Setting the orientation

The target pose is given relative to a box subframe:

target_pose.header.frame_id = "box/bottom";

The orientation is determined by RPY angles to align the cylinder and box subframes:

target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
target_pose.pose.orientation = tf2::toMsg(target_orientation);

To keep some distance to the box, we use a small offset:

target_pose.pose.position.z = 0.01;
showFrames(target_pose, "cylinder/tip");
moveToCartPose(target_pose, group, "cylinder/tip");

技术说明

TF 不知道子帧，因此它们不能在 MoveIt 规划请求之外使用。如果您需要转换到子帧，可以使用 getFrameTransform 函数从 PlanningScene 的 CollisionRobot 获取它。这将返回一个 Eigen::Isometry3d 对象，您可以从中提取平移和四元数（参见 here). 然后可以使用平移和四元数来创建变换并将其注册到您的 TFListener.

可视化子框架

目前没有子框架的可视化，但欢迎对此功能提出请求！

故障排除

对于您最近未自行生成的较旧的 moveit_config 包，子框架所需的规划适配器可能未配置，并且可能找不到子框架链接。要为您的 moveit_config 包修复此问题，请打开 ompl_planning_pipeline.launch 存档于 <robot_moveit_config>/launch 机器人的文件夹。对于 Panda 机器人来说，它是 this 文件. 编辑此启动文件，找到以下行 <arg name="planning_adapters"> 被提及并插入 default_planning_request_adapters/ResolveConstraintFrames 行后 default_planning_request_adapters/FixStartStatePathConstraints.