C++ PlanningSceneConstPtr::getRobotModel方法代码示例

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::GetMotionPlan::Request &req, 
                         const chomp::ChompParameters& params,
                         moveit_msgs::GetMotionPlan::Response &res) const
{
  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(),
                             3.0,
                             .03,
                             req.motion_plan_request.group_name);
  jointStateToArray(planning_scene->getRobotModel(),
                    req.motion_plan_request.start_state.joint_state, 
                    req.motion_plan_request.group_name,
                    trajectory.getTrajectoryPoint(0));

  int goal_index = trajectory.getNumPoints()- 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for(unsigned int i = 0; i < req.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) {
    js.name.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name);
    js.position.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
    ROS_INFO_STREAM("Setting joint " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name
                    << " to position " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
  }
  jointStateToArray(planning_scene->getRobotModel(),
                    js, 
                    req.motion_plan_request.group_name, 
                    trajectory.getTrajectoryPoint(goal_index));
  const planning_models::RobotModel::JointModelGroup* model_group = 
    planning_scene->getRobotModel()->getJointModelGroup(req.motion_plan_request.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getJointModels().size(); i++)
  {
    const planning_models::RobotModel::JointModel* model = model_group->getJointModels()[i];
    const planning_models::RobotModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::RobotModel::RevoluteJointModel*>(model);

    if (revolute_joint != NULL)
    {
      if(revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }
  
  // fill in an initial quintic spline trajectory
  trajectory.fillInMinJerk();

  // optimize!
  planning_models::RobotState *start_state(planning_scene->getCurrentState());
  planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
    
  ros::WallTime create_time = ros::WallTime::now();
  ChompOptimizer optimizer(&trajectory, 
                           planning_scene, 
                           req.motion_plan_request.group_name,
                           &params,
                           start_state);
  if(!optimizer.isInitialized()) {
    ROS_WARN_STREAM("Could not initialize optimizer");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
    return false;
  }
  ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  optimizer.optimize();
  ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec());
  create_time = ros::WallTime::now();
  // assume that the trajectory is now optimized, fill in the output structure:

  ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints());

  // fill in joint names:
  res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints());
  for (size_t i = 0; i < model_group->getJointModels().size(); i++)
  {
    res.trajectory.joint_trajectory.joint_names[i] = model_group->getJointModels()[i]->getName();
  }

  res.trajectory.joint_trajectory.header = req.motion_plan_request.start_state.joint_state.header; // @TODO this is probably a hack

  // fill in the entire trajectory
  res.trajectory.joint_trajectory.points.resize(trajectory.getNumPoints());
  for (int i=0; i < trajectory.getNumPoints(); i++)
  {
    res.trajectory.joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
    for (size_t j=0; j < res.trajectory.joint_trajectory.points[i].positions.size(); j++)
    {
      res.trajectory.joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
      if(i == trajectory.getNumPoints()-1) {
        ROS_INFO_STREAM("Joint " << j << " " << res.trajectory.joint_trajectory.points[i].positions[j]);
      }
    }
    // Setting invalid timestamps.
    // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
    res.trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
  }
//.........这里部分代码省略.........

constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene,
                                                                                                              const std::string &group_name,
                                                                                                              const moveit_msgs::Constraints &constr)
{
  const robot_model::JointModelGroup *jmg = scene->getRobotModel()->getJointModelGroup(group_name);
  if (!jmg)
    return constraint_samplers::ConstraintSamplerPtr();
  std::stringstream ss; ss << constr;
  logDebug("Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str());
  
  ConstraintSamplerPtr joint_sampler;
  // if there are joint constraints, we could possibly get a sampler from those
  if (!constr.joint_constraints.empty())
  {    
    logDebug("There are joint constraints specified. Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str());

    std::map<std::string, bool> joint_coverage;
    for(std::size_t i = 0; i < jmg->getVariableNames().size() ; ++i)
      joint_coverage[jmg->getVariableNames()[i]] = false;
    
    // construct the constraints
    std::vector<kinematic_constraints::JointConstraint> jc;
    for (std::size_t i = 0 ; i < constr.joint_constraints.size() ; ++i)
    {
      kinematic_constraints::JointConstraint j(scene->getRobotModel(), scene->getTransforms());
      if (j.configure(constr.joint_constraints[i]))
      {
        if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
        {
          joint_coverage[j.getJointVariableName()] = true;
          jc.push_back(j);
        }
      }
    }

    bool full_coverage = true;
    for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it) 
      if (!it->second) 
      {
        full_coverage = false;
        break;
      }
    
    // if we have constrained every joint, then we just use a sampler using these constraints
    if (full_coverage)
    {
      boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
      if (sampler->configure(jc))
      {
        logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
        return sampler;
      }
    }
    else
      // if a smaller set of joints has been specified, keep the constraint sampler around, but use it only if no IK sampler has been specified.
      if (!jc.empty())
      {
        boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
        if (sampler->configure(jc))
        {
          logDebug("Temporary sampler satisfying joint constraints for group '%s' allocated. Looking for different types of constraints before returning though.", jmg->getName().c_str());
          joint_sampler = sampler;
        }
      }
  }
  
  std::vector<ConstraintSamplerPtr> samplers;
  if (joint_sampler) 
    samplers.push_back(joint_sampler);
  
  // read the ik allocators, if any
  robot_model::SolverAllocatorFn ik_alloc = jmg->getSolverAllocators().first;
  std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn> ik_subgroup_alloc = jmg->getSolverAllocators().second;
  
  // if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints should be used
  if (ik_alloc)
  {
    logDebug("There is an IK allocator for '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());
    
    // keep track of which links we constrained
    std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL;
    
    // if we have position and/or orientation constraints on links that we can perform IK for,
    // we will use a sampleable goal region that employs IK to sample goals;
    // if there are multiple constraints for the same link, we keep the one with the smallest 
    // volume for sampling
    for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
      for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
        if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
        {
          boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
          boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
          if (pc->configure(constr.position_constraints[p]) && oc->configure(constr.orientation_constraints[o]))
          {        
            boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
            if(iks->configure(IKSamplingPose(pc, oc))) {
              bool use = true;
              if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
                if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
                  use = false;
//.........这里部分代码省略.........

bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
{
  double timeout = goal.allowed_planning_time;
  ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
  std::string attached_object_name = goal.attached_object_name;
  const robot_model::JointModelGroup *jmg = NULL;
  const robot_model::JointModelGroup *eef = NULL;

  // if the group specified is actually an end-effector, we use it as such
  if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
  {
    eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
    if (eef)
    { // if we correctly found the eef, then we try to find out what the planning group is
      const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
      if (eef_parent.empty())
      {
        ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
        error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
        return false;
      }
      else
        jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
    }
  }
  else
  {
    // if a group name was specified, try to use it
    jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
    if (jmg)
    {
      // we also try to find the corresponding eef
      const std::vector<std::string> &eef_names = jmg->getAttachedEndEffectorNames();  
      if (eef_names.empty())
      {
        ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name << "'");
        error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
        return false;
      }
      else
        // check to see if there is an end effector that has attached objects associaded, so we can complete the place
        for (std::size_t i = 0 ; i < eef_names.size() ; ++i) 
        {
          std::vector<const robot_state::AttachedBody*> attached_bodies;
          const robot_model::JointModelGroup *eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
          if (eg)
          {
            // see if there are objects attached to links in the eef
            planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
            
            // is is often possible that the objects are attached to the same link that the eef itself is attached,
            // so we check for attached bodies there as well
            const robot_model::LinkModel *attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
            if (attached_link_model)
            {
              std::vector<const robot_state::AttachedBody*> attached_bodies2;
              planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
              attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
            }
          }
          
          // if this end effector has attached objects, we go on
          if (!attached_bodies.empty())
          {
            // if the user specified the name of the attached object to place, we check that indeed
            // the group contains this attachd body
            if (!attached_object_name.empty())
            {
              bool found = false;
              for (std::size_t j = 0 ; j < attached_bodies.size() ; ++j)
                if (attached_bodies[j]->getName() == attached_object_name)
                {
                  found = true;
                  break;
                }
              // if the attached body this group has is not the same as the one specified,
              // we cannot use this eef
              if (!found)
                continue;
            }
            
            // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
            if (eef)
            {
              ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '" << goal.group_name <<
                                     "' that are currently holding objects. It is ambiguous which end-effector to use. Please specify it explicitly.");
              error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
              return false;
            }
            // set the end effector (this was initialized to NULL above)
            eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
          }
        }
    }
  }
  
  // if we know the attached object, but not the eef, we can try to identify that
  if (!attached_object_name.empty() && !eef)
  {
    const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
//.........这里部分代码省略.........

  virtual bool adaptAndPlan(const PlannerFn &planner,
                            const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest &req, 
                            planning_interface::MotionPlanResponse &res,
                            std::vector<std::size_t> &added_path_index) const
  {
    ROS_DEBUG("Running '%s'", getDescription().c_str());
    
    // get the specified start state
    robot_state::RobotState start_state = planning_scene->getCurrentState();
    robot_state::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.start_state, start_state);

    const std::vector<robot_state::JointState*> &jstates = 
      planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? 
      start_state.getJointStateGroup(req.group_name)->getJointStateVector() : 
      start_state.getJointStateVector(); 
    
    bool change_req = false;
    for (std::size_t i = 0 ; i < jstates.size() ; ++i)
    { 
      // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
      // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around. 
      // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
      // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
      // joints, and we un-do them when the plan comes from the planner
      
      const robot_model::JointModel* jm = jstates[i]->getJointModel();
      if (jm->getType() == robot_model::JointModel::REVOLUTE)
      {
        if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
        {
          double initial = jstates[i]->getVariableValues()[0];
          jstates[i]->enforceBounds();
          double after = jstates[i]->getVariableValues()[0];
          if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
            change_req = true;
        } 
      }
      else
        // Normalize yaw; no offset needs to be remembered
        if (jm->getType() == robot_model::JointModel::PLANAR)
        {   
          double initial = jstates[i]->getVariableValues()[2];
          if (static_cast<const robot_model::PlanarJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues()))
            change_req = true;
        }
        else
          // Normalize quaternions
          if (jm->getType() == robot_model::JointModel::FLOATING)
          {
            if (static_cast<const robot_model::FloatingJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues()))
              change_req = true;
          }
    }
    
    // pointer to a prefix state we could possibly add, if we detect we have to make changes
    robot_state::RobotStatePtr prefix_state;
    for (std::size_t i = 0 ; i < jstates.size() ; ++i)
    {   
      if (!jstates[i]->satisfiesBounds())
      {    
        if (jstates[i]->satisfiesBounds(bounds_dist_))
        {
          if (!prefix_state)
            prefix_state.reset(new robot_state::RobotState(start_state));
          jstates[i]->enforceBounds();
          change_req = true;
          ROS_INFO("Starting state is just outside bounds (joint '%s'). Assuming within bounds.", jstates[i]->getName().c_str());
        }
        else
        {
          std::stringstream joint_values;
          std::stringstream joint_bounds_low;
          std::stringstream joint_bounds_hi;
          for (std::size_t k = 0 ; k < jstates[i]->getVariableValues().size() ; ++k)
            joint_values << jstates[i]->getVariableValues()[k] << " ";
          for (std::size_t k = 0 ; k < jstates[i]->getVariableBounds().size() ; ++k)
          {
            joint_bounds_low << jstates[i]->getVariableBounds()[k].first << " ";
            joint_bounds_hi << jstates[i]->getVariableBounds()[k].second << " ";
          }
          ROS_WARN_STREAM("Joint '" << jstates[i]->getName() << "' from the starting state is outside bounds by a significant margin: [ " << joint_values.str() << "] should be in the range [ " << joint_bounds_low.str() <<
                          "], [ " << joint_bounds_hi.str() << "] but the error above the ~" << BOUNDS_PARAM_NAME << " parameter (currently set to " << bounds_dist_ << ")");
        }
      }
    }

    bool solved;
    // if we made any changes, use them
    if (change_req)
    {
      planning_interface::MotionPlanRequest req2 = req;
      robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
      solved = planner(planning_scene, req2, res);
    }
    else
      solved = planner(planning_scene, req, res);

    // re-add the prefix state, if it was constructed
    if (prefix_state && res.trajectory_)
//.........这里部分代码省略.........

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
                         moveit_msgs::MotionPlanDetailedResponse& res) const
{
  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    return false;
  }

  if (req.start_state.joint_state.position.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

  jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
                    trajectory.getTrajectoryPoint(0));

  if (req.goal_constraints.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  if (req.goal_constraints[0].joint_constraints.empty())
  {
    ROS_ERROR_STREAM("Only joint-space goals are supported");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  int goal_index = trajectory.getNumPoints() - 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
  {
    js.name.push_back(joint_constraint.joint_name);
    js.position.push_back(joint_constraint.position);
    ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position "
                                                            << joint_constraint.position);
  }
  jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));

  const moveit::core::JointModelGroup* model_group =
      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
  {
    const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
    const moveit::core::RevoluteJointModel* revolute_joint =
        dynamic_cast<const moveit::core::RevoluteJointModel*>(model);

    if (revolute_joint != nullptr)
    {
      if (revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }

  const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
  const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
  moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
  goal_robot_state.setVariablePositions(
      active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));

  if (not goal_robot_state.satisfiesBounds())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  // fill in an initial trajectory based on user choice from the chomp_config.yaml file
  if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
    trajectory.fillInMinJerk();
  else if (params.trajectory_initialization_method_.compare("linear") == 0)
    trajectory.fillInLinearInterpolation();
  else if (params.trajectory_initialization_method_.compare("cubic") == 0)
    trajectory.fillInCubicInterpolation();
  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
  {
//.........这里部分代码省略.........

    virtual bool adaptAndPlan(const PlannerFn &planner,
                              const planning_scene::PlanningSceneConstPtr& planning_scene,
                              const planning_interface::MotionPlanRequest &req,
                              planning_interface::MotionPlanResponse &res,
                              std::vector<std::size_t> &added_path_index) const
    {
        ROS_DEBUG("Running '%s'", getDescription().c_str());

        // get the specified start state
        robot_state::RobotState start_state = planning_scene->getCurrentState();
        robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);

        collision_detection::CollisionRequest creq;
        creq.group_name = req.group_name;
        collision_detection::CollisionResult cres;
        planning_scene->checkCollision(creq, cres, start_state);
        if (cres.collision)
        {
            // Rerun in verbose mode
            collision_detection::CollisionRequest vcreq = creq;
            collision_detection::CollisionResult vcres;
            vcreq.verbose = true;
            planning_scene->checkCollision(vcreq, vcres, start_state);

            if (creq.group_name.empty())
                ROS_INFO("Start state appears to be in collision");
            else
                ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);

            robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
            random_numbers::RandomNumberGenerator &rng = prefix_state->getRandomNumberGenerator();

            const std::vector<const robot_model::JointModel*> &jmodels =
                planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
                planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
                planning_scene->getRobotModel()->getJointModels();

            bool found = false;
            for (int c = 0 ; !found && c < sampling_attempts_ ; ++c)
            {
                for (std::size_t i = 0 ; !found && i < jmodels.size() ; ++i)
                {
                    std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
                    const double *original_values = prefix_state->getJointPositions(jmodels[i]);
                    jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, jmodels[i]->getMaximumExtent() * jiggle_fraction_);
                    start_state.setJointPositions(jmodels[i], sampled_variable_values);
                    collision_detection::CollisionResult cres;
                    planning_scene->checkCollision(creq, cres, start_state);
                    if (!cres.collision)
                    {
                        found = true;
                        ROS_INFO("Found a valid state near the start state at distance %lf after %d attempts", prefix_state->distance(start_state), c);
                    }
                }
            }

            if (found)
            {
                planning_interface::MotionPlanRequest req2 = req;
                robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
                bool solved = planner(planning_scene, req2, res);
                if (solved && !res.trajectory_->empty())
                {
                    // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
                    res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
                    res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
                    // we add a prefix point, so we need to bump any previously added index positions
                    for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
                        added_path_index[i]++;
                    added_path_index.push_back(0);
                }
                return solved;
            }
            else
            {
                ROS_WARN("Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling attempts). Passing the original planning request to the planner.",
                         jiggle_fraction_, sampling_attempts_);
                return planner(planning_scene, req, res);
            }
        }
        else
        {
            if (creq.group_name.empty())
                ROS_DEBUG("Start state is valid");
            else
                ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
            return planner(planning_scene, req, res);
        }
    }

bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
{
  double timeout = goal.allowed_planning_time;
  ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);

  std::string planning_group = goal.group_name;
  std::string end_effector = goal.end_effector;
  if (end_effector.empty() && !planning_group.empty())
  {
    const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
    if (!jmg)
    {
      error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      return false;
    }
    const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames();
    if (!eefs.empty())
    {
      end_effector = eefs.front();
      if (eefs.size() > 1)
        ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" << planning_group << "' is ambiguous. Assuming '" << end_effector << "'");
    }
  }
  else
    if (!end_effector.empty() && planning_group.empty())
    {
      const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
      if (!jmg)
      {
        error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
        return false;
      }
      planning_group = jmg->getEndEffectorParentGroup().first;
      if (planning_group.empty())
      {   
        ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << end_effector << "'. Please define a parent group in the SRDF.");
        error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
        return false;
      }
      else
        ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'");
    }
  const robot_model::JointModelGroup *eef = end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
  if (!eef)
  {
    ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
    error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
    return false;
  }
  const std::string &ik_link = eef->getEndEffectorParentGroup().second;

  ros::WallTime start_time = ros::WallTime::now();

  // construct common data for possible manipulation plans
  ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
  ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
  plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
  plan_data->end_effector_group_ = eef;
  plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
  plan_data->timeout_ = endtime;
  plan_data->path_constraints_ = goal.path_constraints;
  plan_data->planner_id_ = goal.planner_id;
  plan_data->minimize_object_distance_ = goal.minimize_object_distance;
  plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
  moveit_msgs::AttachedCollisionObject &attach_object_msg = plan_data->diff_attached_object_;

  // construct the attached object message that will change the world to what it would become after a pick
  attach_object_msg.link_name = ik_link;
  attach_object_msg.object.id = goal.target_name;
  attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
  attach_object_msg.touch_links = goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
  collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));

  // we are allowed to touch the target object
  approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
  // we are allowed to touch certain other objects with the gripper
  approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
  if (!goal.support_surface_name.empty())
  {
    // we are allowed to have contact between the target object and the support surface before the grasp
    approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);

    // optionally, it may be allowed to touch the support surface with the gripper
    if (goal.allow_gripper_support_collision)
      approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
  }

  // configure the manipulation pipeline
  pipeline_.reset();
  ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
  pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);

  initialize();
  pipeline_.start();

  // order the grasps by quality
  std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
  for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
//.........这里部分代码省略.........