< >
Home » MoveIt!入门教程 » MoveIt!入门教程-运动规划管道(传递途径)

MoveIt!入门教程-运动规划管道(传递途径)

MoveIt!入门教程-运动规划管道(传递途径)

说明

  • 在MoveIt！，运动规划是设置计划路径
  • 很多时候我们预处理运动规划请求和后处理已规划的路径（例如：时间参数化），这时候就要用到规划管道。
  • 规划管道可以让运动规划连通预处理和后处理阶段。
  • 预处理和后处理叫做规划请求适配器，可在ROS parameter server中通过名字来配置。
  • 本教程利用C++演示，如何实例化并调用规划管道

使用

  1. 开始
  • 在加载规划器前，需要建立两个对象RobotModel和PlanningScene
  • 首先实例化RobotModelLoader对象，它可从ROS parameter server查找robot description和构建RobotModel。
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
  • 使用RobotModel，可构建PlanningScene，它可以保持世界和机器人的状态。
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
  • 现在建立PlanningPipeline对象，它使用ROS param server去确定请求适配器的集合和使用的规划插件。
planning_pipeline::PlanningPipelinePtr planning_pipeline(new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(20.0);
sleep_time.sleep();

姿态目标

  • 为PR2的右臂创建规划请求，指定终端执行器的期望的姿态
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "torso_lift_link";
pose.pose.position.x = 0.75;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.0;
pose.pose.orientation.w = 1.0;
  • 设置位置偏差是0.01m, 角度偏差是0.01度
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
  • 使用来自kinematic_constraints包help函数构建一个约束请求
req.group_name = "right_arm";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
  • 调用管道检查规划是否成功
planning_pipeline->generatePlan(planning_scene, req, res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}

可视化结果

  • 代码：
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;

/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);

display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);

sleep_time.sleep();

节点空间目标

  • 代码：
/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
const robot_model::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("right_arm");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  • 建立目标
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values(7, 0.0);
joint_values[0] = -2.0;
joint_values[3] = -0.2;
joint_values[5] = -0.15;
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);

req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
  • 调用管道并可视化结果
planning_pipeline->generatePlan(planning_scene, req, res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
  • 现在你应该看到两个计划中的轨迹
display_publisher.publish(display_trajectory);
sleep_time.sleep();

使用规划请求适配器

  • 一个规划请求适配器允许我们指定一系列的操作，应该发生在规划发生之前或之后的已经完成结果路径的规划
  • 有针对性设置初始状态超出关节限制，让规划请求适配器来处理它。
/* First, set the state in the planning scene to the final state of the last plan */
robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  • 设置一个关节稍微超出它的限制
const robot_model::JointModel* joint_model = joint_model_group->getJointModel("r_shoulder_pan_joint");
const robot_model::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds();
std::vector<double> tmp_values(1, 0.0);
tmp_values[0] = joint_bounds[0].min_position_ - 0.01;
robot_state.setJointPositions(joint_model, tmp_values);
req.goal_constraints.clear();
  • 再次调用规划器并可视化轨迹
 planning_pipeline->generatePlan(planning_scene, req, res);
  if(res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }
  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  res.getMessage(response);
  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  /* Now you should see three planned trajectories in series*/
  display_publisher.publish(display_trajectory);

  sleep_time.sleep();
  ROS_INFO("Done");
  return 0;
}
req.goal_constraints.push_back(pose_goal);

完整代码

编译代码

launch文件

启动代码

roslaunch moveit_tutorials motion_planning_api_tutorial.launch

理想输出

  • 在Rviz, 我应该看到轨迹重播:

    • 机器人移动右臂到前面的姿态目标
    • 机器人移动右臂到侧边的节点目标
    • 机器人移动右臂回前面的原姿态目标

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

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


标签: moveit!入门教程