规划[1/5]：关节规划

ancientcc
  • ompl相关编程分为四步，依次是设置目标状态、设置起始状态、求解和读取规划结果。
  • 向ompl设置的初始状态是实时从joint_state_publiser结点收到的/joint_states。
  • ompl规化出的路径存放在ompl::base::PlannerSolution，成员path_对应路径。注意，一个path_封装的是一条路径，不是路径中的一个点。path_类型是ompl::base::Path，它是个协议类。对基于Geometric方式的路径规划算法，像RRTConnect，最终类型是个从Path派生的PathGeometric。在PathGeometric内，路径具体存放在states_成员，其类型std::vector<base::State *>。vector中的一个base::State单元表示路径中的一个点。

为方便叙述，本文给出一个关节规划示例

  • 机械臂有a、b、c三个关节，都是单自由度。
  • 初始状态：(-0.56, 0.79, -0.45)
  • 目标状态：(1, 2, -1)

“server处理move action：MoveGroupMoveAction”提到ModelBasedPlanningContext::solve得到规划轨迹，并存放在ompl_simple_setup_。本文叙述在关节规划时，ompl具体怎么处理。

关节规划是用户把关节状态(JointState)作为目标，因为最后发向机械臂的js就是关节状态，因而只要找出中间那些个关节状态就行。对所给示例，文末给出了规划轨迹，一共72个点，索引0就是初始状态，索引71是个和目标状态在容错误差内的值。

由于关节状态不须要转换，在五种规划中，它最简单。按ompl教程(英文)/中文，我把moveit相关编程分为四步，依次是设置目标状态、设置起始状态、求解和读取规划结果。

注：ompl教程也可说是4步，但那里先设置起始状态，然后目标状态，这两操作的次序应该没要求，moveit是先设置目标状态。

 

一、设置目标状态

<libros>/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
------
ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal()
{
  // ******************* set up the goal representation, based on goal constraints

  std::vector<ob::GoalPtr> goals;
  for (kinematic_constraints::KinematicConstraintSetPtr& goal_constraint : goal_constraints_)
  {
    constraint_samplers::ConstraintSamplerPtr constraint_sampler;
    if (spec_.constraint_sampler_manager_)
      constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
                                                                            goal_constraint->getAllConstraints());
    if (constraint_sampler)
    {
      ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler));
      goals.push_back(goal);
    }
  }

  if (!goals.empty())
    return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
  ...
}
图1 constructGoal

constructGoal功能是以KinematicConstraintSet类型的goal_constraints_为输入，生成ConstrainedGoalSampler类型的目标状态器goals，goals[0]就是要设置到ompl的目标状态。图1左侧是goal_constraints_，目标js存在当中的joint_constraints_，右侧是要生成的目标：ConstrainedGoalSampler。bounds_存储着转换后的目标js，说来就是把原js加上tolerance。看下调用constructGoal的setGoalConstraints。

<libros>/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
------
bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints(
    const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
    moveit_msgs::MoveItErrorCodes* error)
{
  // ******************* check if the input is correct
  goal_constraints_.clear();
  for (const moveit_msgs::Constraints& goal_constraint : goal_constraints)
  {
    moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraint, path_constraints);
    kinematic_constraints::KinematicConstraintSetPtr kset(
        new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
    kset->add(constr, getPlanningScene()->getTransforms());
    if (!kset->empty())
      goal_constraints_.push_back(kset);
  }

  ...

  ob::GoalPtr goal = constructGoal();
  ompl_simple_setup_->setGoal(goal);
  return static_cast<bool>(goal);
}

ModelBasedPlanningContext::setGoalConstraints会实时生成goal_constraints_，然后调用constructGoal生成goal，最终ompl_simple_setup_->setGoal(goal)向ompl设置目标状态。

 

二、设置起始状态

<libros>/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
------
void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, bool use_constraints_approximations)
{
  ...

  // convert the input state to the corresponding OMPL state
  ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
  spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
  ompl_simple_setup_->setStartState(ompl_start_state);
  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));

  ...

  useConfig();
  if (ompl_simple_setup_->getGoal())
    ompl_simple_setup_->setup();
}

用spec_.state_space_构造出ompl_start_state，要以getCompleteInitialRobotState()为源向ompl_start_state设置值，这个源就是complete_initial_robot_state_。

const moveit::core::RobotState& getCompleteInitialRobotState() const
{
  return complete_initial_robot_state_;
}
图2 configure

图2左侧是complete_initial_robot_state_，初始js存在当中的position_，右侧是要设置到ompl的初始状态：ompl_start_state，values存储着值，就是原样赋值。得到ompl_start_state后，就调用ompl_simple_setup_->setStartState(ompl_start_state)把初始关态设置到ompl。

complete_initial_robot_state_类型是moveit::core::RobotState，moveit怎么更新当中的position？来自planning_scene->getCurrentStateUpdated(req.start_state)。让看下PlanningContextManager::getPlanningContext，它依次执行了1）得到当前RobotState。2）向ompl设置目标关态。3）向ompl设置初始状态。

ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
    const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req,
    moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, bool use_constraints_approximation) const
{
  ...

  ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req);

  if (context)
  {
    context->clear();

    moveit::core::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state);

得到当前RobotState，后面setCompleteInitialState是把它设置到complete_initial_robot_state_。深入getCurrentStateUpdated会看到源是PlanningScene中的robot_state_，那何时更新robot_state_？——在PlanningScene::getCurrentStateNonConst()。

图3 getCurrentStateNonConst

图3显示了getCurrentStateNonConst()调用时刻。它的上层函数有个叫CurrentStateMonitor::jointStateCallback，该函数的触发时机正是在move_group收到/joint_states话题时。也就是说，move_group向ompl设置的初始状态是实时从joint_state_publiser结点收到的/joint_states。

    // Setup the context
    context->setPlanningScene(planning_scene);
    context->setMotionPlanRequest(req);
    context->setCompleteInitialState(*start_state);

    context->setPlanningVolume(req.workspace_parameters);
    if (!context->setPathConstraints(req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();

    if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();

setGoalConstraints设置目标状态，分析见上面。

    try
    {
      context->configure(nh, use_constraints_approximation);

configure设置起始状态，分析见上面。

      ROS_DEBUG_NAMED(LOGNAME, "%s: New planning context is set.", context->getName().c_str());
      error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    }
    catch (ompl::Exception& ex)
    {
      ROS_ERROR_NAMED(LOGNAME, "OMPL encountered an error: %s", ex.what());
      context.reset();
    }
  }

  return context;
}

 

三、求解

ompl_simple_setup_->solve负责求解，即调用ompl::geometric::SimpleSetup::solve。默认用的路径规划算法是RTTConnect，具体执行的操作是ompl::geometric::RRTConnect::solve(...)，参考“RRTConnect::solve和生成路径”。

 

四、读取规划结果

“server处理move action：MoveGroupMoveAction”中有写，会执行ModelBasedPlanningContext内的“getSolutionPath(*res.trajectory_)”，把存放在ompl_simple_setup_规划出的路径转成RobotTrajectory类型，并放在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();
}

convertPath具体执行转换，第一个参数ompl_simple_setup_->getSolutionPath()得到的就是ompl_simple_setup_中路径，它最终会调用PlannerSolutionSet的getTopSolution。

<libros>/ompl/base/src/ProblemDefinition.cpp
------
class ProblemDefinition::PlannerSolutionSet
{
    PathPtr getTopSolution()
    {
        std::lock_guard<std::mutex> slock(lock_);
        PathPtr copy;
        if (!solutions_.empty())
            copy = solutions_[0].path_;
        return copy;
    }
};
图4 ompl_simple_setup_和solutions_

图4左侧显示了ompl_simple_setup_成员，有个类型ProblemDefinition变量pdef_，顾名思义，它封装了问题描述，成员solutions_存储着规化出的路径。

图4右侧显示solutions_。当中有两个名称是solutions_变量，外层是个PlannerSolutionSet对象，内中有个同名solutions_成员，类型std::vector<PlannerSolution>，这个vector的第一单元便是最后规划出的路径，getTopSolution()中的“Top”就是指vector的第一个。

具体到PlannerSolution，成员path_对应路径。注意，一个path_封装的是一条路径，不是路径中的一个点。path_类型是ompl::base::Path，它是个协议类。对基于Geometric方式的规化方法，像RRTConnect，最终类型是个从Path派生的PathGeometric。在PathGeometric内，路径具体存放在states_成员，其类型std::vector<base::State *>。vector中的一个base::State单元表示路径中的一个点，当然base::State自身是没法存储路径点，而是它的派生类ModelBasedStateSpace::StateType。对应到示例，states_长度是72，意味着规划出路径有72个点。

------size: 72
[0/71] (-0.56,0.79,-0.45)
[1/71] (-0.538027,0.807043,-0.457747)
[2/71] (-0.516054,0.824086,-0.465493)
[3/71] (-0.494081,0.841129,-0.47324)
[4/71] (-0.472108,0.858173,-0.480986)
[5/71] (-0.450135,0.875216,-0.488733)
[6/71] (-0.428162,0.892259,-0.49648)
[7/71] (-0.406189,0.909302,-0.504226)
...
[64/71] (0.846268,1.88076,-0.945782)
[65/71] (0.868241,1.89781,-0.953529)
[66/71] (0.890214,1.91485,-0.961276)
[67/71] (0.912187,1.93189,-0.969022)
[68/71] (0.93416,1.94894,-0.976769)
[69/71] (0.956133,1.96598,-0.984515)
[70/71] (0.978106,1.98302,-0.992262)
[71/71] (1.00008,2.00006,-1.00001)

 

发布于 2022-05-09 09:02:38

全部评论: 0 条

    写评论: