< >
Home » MoveIt!入门教程 » MoveIt!入门教程-Move Group接口

MoveIt!入门教程-Move Group接口

MoveIt!入门教程-Move Group接口

说明

  • MoveIt主要的用户接口功能通过MoveGroup类实现
  • 这个类提供简易方式去实现大部分功能，比如：设置关节或目标姿态，创建行为规划，移动机器人，在环境中增加对象或给机器人增加或减少对象。

使用

  1. 安装
  • MoveGroup类的使用非常简单，只需提供你想去控制或规划的组名
moveit::planning_interface::MoveGroup group("right_arm");
  • 我们使用PlanningSceneInterface类去直接处理.
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  • (可选)在Rviz中创建一个发布用于可视化规划.
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
  1. 获得基本信息
  • 打印这个组的参考系的名称
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
  • 打印这个组的末端执行器的名称
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
  1. 规划一个姿态目标
  • 我们为这个组计划一个动作，移动末端执行器到希望的姿态。
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.7;
target_pose1.position.z = 1.0;
group.setPoseTarget(target_pose1);
  • 现在，我们调用规划器去计算规划和可视化过程，注意：我们只是规划，不要求move_group去实际移动机器人。
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);

ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);

4. 可视化规划

  • 我们可以在Rviz里可视化这个规划。这个不是必需的，因为group.plan()的调用使得上面的过程会自动完成。在明确发布计划有用的情况下，我们想去可视化之前创建的规划。
if (1)
{
  ROS_INFO("Visualizing plan 1 (again)");
  display_trajectory.trajectory_start = my_plan.start_state_;
  display_trajectory.trajectory.push_back(my_plan.trajectory_);
  display_publisher.publish(display_trajectory);
  /* Sleep to give Rviz time to visualize the plan. */
  sleep(5.0);
}

5. 移动到目标姿态

  • 移动到一个姿势的目标是类似于上述步骤除了我们现在使用的move()功能。
  • 请注意，我们已经设置了前面的姿势目标仍然是有效的，所以机器人将尝试移动到那个目标。
  • 我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器是激活，执行后报告成功的轨迹。
/* Uncomment below line when working with a real robot*/
/* group.move() */

6. 规划到一个关节空间内的目标

  • 现在规划一个到关节空间内的目标并向其移动。这个会替代之前设置的目标
  • 首先获取指定组的当前关节的设置
std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
  • 修改一个关节，规划到新的目标并可视化
group_variable_values[0] = -1.0;
group.setJointValueTarget(group_variable_values);
success = group.plan(my_plan);

ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);

7. 路径约束规划

  • 路径约束容易指定机器人的一个连接。为组指定路径约束和目标姿态。
  • 首先定义路径约束
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = "base_link";
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;
  • 设置为组的路径约束
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
group.setPathConstraints(test_constraints);
  • 我们将重用我们曾经有过的旧的目标，并规划它。
  • 请注意，如果目前的状态已经满足路径约束。这就能开始工作，
  • 因此，我们需要将启动状态设置为一个新的姿势。
robot_state::RobotState start_state(*group.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;
const robot_state::JointModelGroup *joint_model_group =
                start_state.getJointModelGroup(group.getName());
start_state.setFromIK(joint_model_group, start_pose2);
group.setStartState(start_state);
  • 我们规划从新的开始姿态移动到之前的目标姿态
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);

ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);
  • 当做了路径约束时一定要清除它。
group.clearPathConstraints();

8. 笛卡尔路径

  • 你可以计划一个笛卡尔路径通过直接为末端执行器指定航点列表。
  • 请注意，我们是从新的开始状态。初始姿态（起始状态）不需要加入航点列表。
std::vector<geometry_msgs::Pose> waypoints;

geometry_msgs::Pose target_pose3 = start_pose2;
target_pose3.position.x += 0.2;
target_pose3.position.z += 0.2;
waypoints.push_back(target_pose3);  // up and out

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

target_pose3.position.z -= 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // down and right (back to start)
  • 我们希望笛卡尔路径被内插在一个分辨率为1厘米，这就是为何在笛卡尔转换指定0.01作为最大值。我们指定跳阈值为0，实际上是禁用它。（没理解好留原文）We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the max step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it.
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(waypoints,
                                             0.01,  // eef_step
                                             0.0,   // jump_threshold
                                             trajectory);

ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",
      fraction * 100.0);
/* Sleep to give Rviz time to visualize the plan. */
sleep(15.0);

9. 添加/删除对象和附着/分离对象

  • 首先，我们将定义碰撞对象消息。
 moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();

/* The id of the object is used to identify it. */
collision_object.id = "box1";

/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;

/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x =  0.6;
box_pose.position.y = -0.4;
box_pose.position.z =  1.2;

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);
  • 现在，让我们添加碰撞对象到世界
ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

/* Sleep so we have time to see the object in RViz */
sleep(2.0);
  • 带有碰撞检测的规划是很慢的。让我们制定计划时间，以确保规划有足够的时间来计划。10秒应该是足够的。
group.setPlanningTime(10.0);
  • 现在当我们计划一个轨迹，它将避免障碍
group.setStartState(*group.getCurrentState());
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);

ROS_INFO("Visualizing plan 5 (pose goal move around box) %s",
  success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);
  • 现在，让我们把碰撞物体附加到机器人上。
ROS_INFO("Attach the object to the robot");
group.attachObject(collision_object.id);
/* Sleep to give Rviz time to show the object attached (different color). */
sleep(4.0);
  • 现在，让我们从机器人中分离碰撞物体。
ROS_INFO("Detach the object from the robot");
group.detachObject(collision_object.id);
/* Sleep to give Rviz time to show the object detached. */
sleep(4.0);
  • 现在，让我们从世界上删除碰撞对象。
ROS_INFO("Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
/* Sleep to give Rviz time to show the object is no longer there. */
sleep(4.0);

10. 双手臂位姿

  • 首先定义一个新组安置两个手臂。定义两个新的独立的姿态目标，分别是为每个末端执行器。
  • 注意：我们会重用以上针对右臂的目标。
moveit::planning_interface::MoveGroup two_arms_group("arms");
two_arms_group.setPoseTarget(target_pose1, "r_wrist_roll_link");

geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0.7;
target_pose2.position.y = 0.15;
target_pose2.position.z = 1.0;

two_arms_group.setPoseTarget(target_pose2, "l_wrist_roll_link");
  • 规划并可视化
moveit::planning_interface::MoveGroup::Plan two_arms_plan;
two_arms_group.plan(two_arms_plan);
sleep(4.0);

11. 完整代码

12. 编译代码

13. launch文件

  • 所有的launch文件在这
  • 代码来自moveit_tutorials软件包(是你安装MoveIt的一部分)，你可以编译和运行。

14. 启动代码

roslaunch moveit_tutorials move_group_interface_tutorial.launch
  • 效果如下：

请输入图片描述

  • 在窗口的底部的右侧部分的运动规划部分可以关闭，以获得更好的机器人视图。

15. 期望输出

  • 在Rviz，我们应该能够看到下面的（会有一个延迟5-10秒之间每一步）：
    • 机器人移动右臂到右前方的姿态目标
    • 重复上面的动作
    • 机器人移动右臂到右边的连接目标
    • 机器人移动右臂回到新的姿态目标并保持末端执行器水平
    • 机器人沿着笛卡尔路径期望的路径移动右臂（三角形向上+向前，左，下+后）
    • 盒子对象增加到右臂的右边。
      请输入图片描述
    • 机器人移动右臂到姿态目标，避开与盒子碰撞
    • 该对象连接到手腕上（它的颜色将改变到紫色/橙色/绿色）
    • 该对象是从手腕上脱离（它的颜色会变回绿色）
    • 该对象被从环境中移除。

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: moveit!入门教程