路径规划[4/4]：robot_trajectory::RobotTrajectory以及后处理

ancientcc

文中如果出现RobotTrajectory，那指的是robot_trajectory::RobotTrajectory。对moveit_msgs::RobotTrajectory，会写全。

  • ModelBasedPlanningContext::getSolutionPath负责把PathGeometric中路径转成robot_trajectory::RobotTrajectory。在转换出的RobotTrajectory，waypoints_存储着规划出路径，每个路点类型是RobotState，position_字段来自PathGeometric的对应路点。duration_from_previous_中double全是0，和waypoints_有着一样size()。
  • RobotTrajectory的duration_from_previous_会用来生成moveit_msgs::RobotTrajectory中的time_from_start，后者决定js(JointState)发向机器人的间隔。getSolutionPath后，duration_from_previous_中填的值都是0，按这算的话，所有路点对应的js将没有间隔地发向机器人，这会导致机械臂从初始状态立刻就转到目标状态，就不会旋转到中间那些路点。由于机械臂用力摆动，会造成机器人不稳。为避免这个问题，需要有计算duration_from_previous_的后绪处理。
<moveit_planners>/ompl/ompl_interface/src/model_based_planning_context.cpp
------
bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res)
{
  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
  {
    double ptime = getLastPlanTime();
    if (simplify_solutions_)
    {
      simplifySolution(request_.allowed_planning_time - ptime);
      ptime += getLastSimplifyTime();
    }

    if (interpolate_)
      interpolateSolution();

    // fill the response
    ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(),
                    getOMPLSimpleSetup()->getSolutionPath().getStateCount());

    res.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
    getSolutionPath(*res.trajectory_);
    res.planning_time_ = ptime;
    return true;
  } else {
    ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem");
    res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
    return false;
  }
}

经过interpolateSolution，规划出的路径存放在了ompl::geometric::PathGeometric。PathGeometric定义在ompl空间，要没意外，client无法访问ompl，因而还须要进一步转换。对client，极可能路径要放在rosmsg命令可直接查看的moveit_msgs::RobotTrajectory。但在转换上，moveit提供了一种中间类型：robot_trajectory::RobotTrajectory。即使最终要的是moveit_msgs::RobotTrajectory，那也要先转成RobotTrajectory，再由它转过去。当然，RobotTrajectory的作用并不仅仅是做为转换到moveit_msgs::RobotTrajectory的中间类型，moveit要以它为参数作后处理，具体是那些从PlanningRequestAdapter派生的类。在讨论后处理前，让看下moveit是如何把PathGeometric中路径转换成RobotTrajectory。

 

一、ModelBasedPlanningContext::getSolutionPath

通过看ModelBasedPlanningContext::solve，可知interpolateSolution后就会执行getSolutionPath，执行成功后，参数traj保存着转换出的路径。

bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory::RobotTrajectory& traj) const
{
  traj.clear();
  if (ompl_simple_setup_->haveSolutionPath())
    convertPath(ompl_simple_setup_->getSolutionPath(), traj);
  return ompl_simple_setup_->haveSolutionPath();
}

ompl::geometric::PathGeometric &ompl::geometric::SimpleSetup::getSolutionPath() const
{
    if (pdef_)
    {
        const base::PathPtr &p = pdef_->getSolutionPath();
        if (p)
            return static_cast<PathGeometric &>(*p);
    }
    throw Exception("No solution path");
}

getSolutionPath只一个操作，调用convertPath。看下convertPath第一个参数ompl_simple_setup_->getSolutionPath()，它就是interpolateSolution后生成、存储在pdef_->solutions_->getTopSolution()的PathGeometric。

  • @pg[IN]。作为转换输入的PathGeometric。
  • @traj[OUT]。作为转换输出的RobotTrajectory。
void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
                                                            robot_trajectory::RobotTrajectory& traj) const
{
  moveit::core::RobotState ks = complete_initial_robot_state_;
  for (std::size_t i = 0; i < pg.getStateCount(); ++i)
  {
    spec_.state_space_->copyToRobotState(ks, pg.getState(i));

在pg，每一个路点的存储类型是base::State，说来就是个double数组，存储着规划组中的可变关节值。在RobotTrajectory，每一个路点的存储类型是RobotState，除存储关节值的position_，它还有其它成员。在生成可放入RobotTrajectory的RobotState时用了两步。

  1. ks = complete_initial_robot_state_。给ks赋值初，确保RobotState中其它成员是正确值，像指向urdf模型的robot_model_。
  2. 通过copyToRobotState，把PathGeometric中索引i路点的double数组复制到ks的position_。也就是说，除position_外，各RobotState表示的路点其它成员是一样值。
    traj.addSuffixWayPoint(ks, 0.0);
  }
}

convertPath依次处理PathGeometric每个路点，路点类型由double数组转换到RobotState，double数组赋值给RobotState中的position_。接下就是调用addSuffixWayPoint把RobotState类型的路点添加到traj，注意第二个参数固定传了0。

class RobotTrajectory {
  RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
  {
    return addSuffixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
  }

  RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
  {
    state->update();
    waypoints_.push_back(state);
    duration_from_previous_.push_back(dt);
    return *this;
  }
private:
  moveit::core::RobotModelConstPtr robot_model_;
  const moveit::core::JointModelGroup* group_;
  std::deque<moveit::core::RobotStatePtr> waypoints_;
  std::deque<double> duration_from_previous_;
};

addSuffixWayPoint较好理解，一是把路点ks添加到waypoints_，二是dt添加到duration_from_previous_。综合上面逻辑，让看下ModelBasedPlanningContext::getSolutionPath生成的RobotTrajectory，它的4个成员是什么值。

  • robot_model_。std::shared_ptr智能指针，指向机器人urdf模型。
  • group_。指向此次规划针对的规划组。
  • waypoints_。存储着规划出路径，路点的类型是RobotState，position_来自PathGeometric的对应路点。waypoints_.size()等于duration_from_previous_.size()。
  • duration_from_previous_。当中填的值都是0。duration_from_previous_会用来生成moveit_msgs::RobotTrajectory中的time_from_start，后者决定js(JointState)发向机器人的间隔。固定全0，按这算的话，所有路点对应的js将没有间隔地发向机器人，这会导致机械臂从初始状态立刻就转到目标状态，就不会旋转到中间那些路点。由于机械臂用力摆动，会造成机器人不稳。为避免这个问题，需要有计算duration_from_previous_的后绪处理。
发布于 2022-05-29 21:36:27

全部评论: 0 条

    写评论: