Exemplo n.º 1
0
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);
    if (attached_body)
    {
      // get the robot model link this attached body is associated to
      const robot_model::LinkModel *link = attached_body->getAttachedLink();
      // check to see if there is a unique end effector containing the link
      const std::vector<const robot_model::JointModelGroup*> &eefs = planning_scene->getRobotModel()->getEndEffectors();
      for (std::size_t i = 0 ; i < eefs.size() ; ++i)
        if (eefs[i]->hasLinkModel(link->getName()))
        {
          if (eef)
          {
            ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '" << link->getName() <<
                                   "' which is where the body '" << attached_object_name << "' is attached. It is unclear which end-effector to use.");
            error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
            return false;
          }
          eef = eefs[i];
        }
    }
    // if the group is also unknown, but we just found out the eef
    if (!jmg && eef)
    {
      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);
    }
  }
  
  if (!jmg || !eef)
  { 
    error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
    return false;
  }
  
  // try to infer attached body name if possible
  int loop_count = 0;
  while (attached_object_name.empty() && loop_count < 2)
  {
    // in the first try, look for objects attached to the eef, if the eef is known;
    // otherwise, look for attached bodies in the planning group itself
    std::vector<const robot_state::AttachedBody*> attached_bodies;
    planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
    
    loop_count++;
    if (attached_bodies.size() > 1)
    {
      ROS_ERROR_NAMED("manipulation", "Multiple attached bodies for group '%s' but no explicit attached object to place was specified", goal.group_name.c_str());
      error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
      return false;
    }
    else
      attached_object_name = attached_bodies[0]->getName();
  }

  const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
  if (!attached_body)
  {
    ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action");
    error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
    return false;
  }

  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_ = jmg;
  plan_data->end_effector_group_ = eef;
  plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
  
  plan_data->timeout_ = endtime;
  plan_data->path_constraints_ = goal.path_constraints;
  plan_data->planner_id_ = goal.planner_id;
  plan_data->minimize_object_distance_ = false;
  plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
  moveit_msgs::AttachedCollisionObject &detach_object_msg = plan_data->diff_attached_object_;

  // construct the attached object message that will change the world to what it would become after a placement
  detach_object_msg.link_name = attached_body->getAttachedLinkName();
  detach_object_msg.object.id = attached_object_name;
  detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;

  collision_detection::AllowedCollisionMatrixPtr approach_place_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));

  // we are allowed to touch certain other objects with the gripper
  approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);

  // we are allowed to touch the target object slightly while retreating the end effector
  std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
  approach_place_acm->setEntry(attached_object_name, touch_links, true);

  if (!goal.support_surface_name.empty())
  {
    // we are allowed to have contact between the target object and the support surface before the place
    approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);

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


  // configure the manipulation pipeline
  pipeline_.reset();

  ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
  pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);

  initialize();

  pipeline_.start();

  // add possible place locations
  for (std::size_t i = 0 ; i < goal.place_locations.size() ; ++i)
  {
    ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
    const moveit_msgs::PlaceLocation &pl = goal.place_locations[i];
    
    if (goal.place_eef)
      p->goal_pose_ = pl.place_pose;
    else
      // The goals are specified for the attached body
      // but we want to transform them into goals for the end-effector instead
      if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
      {    
        p->goal_pose_ = pl.place_pose;
        ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the end-effector");
      }
    
    p->approach_ = pl.pre_place_approach;
    p->retreat_ = pl.post_place_retreat;
    p->retreat_posture_ = pl.post_place_posture;
    p->id_ = i;
    if (p->retreat_posture_.joint_names.empty())
      p->retreat_posture_ = attached_body->getDetachPosture();
    pipeline_.push(p);
  }
  ROS_INFO_NAMED("manipulation", "Added %d place locations", (int) goal.place_locations.size());

  // wait till we're done
  waitForPipeline(endtime);

  pipeline_.stop();

  last_plan_time_ = (ros::WallTime::now() - start_time).toSec();

  if (!getSuccessfulManipulationPlans().empty())
    error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  else
  {
    if (last_plan_time_ > timeout)
      error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
    else
    {
      error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      if (goal.place_locations.size() > 0)
      {
        ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode.");
        // everything failed. we now start the pipeline again in verbose mode for one grasp
        initialize();
        pipeline_.setVerbose(true);
        pipeline_.start();
        pipeline_.reprocessLastFailure();
        waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
        pipeline_.stop();
        pipeline_.setVerbose(false);
      }
    }
  }
  ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_);

  return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
Exemplo n.º 2
0
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)
    grasp_order[i] = i;
  OrderGraspQuality oq(goal.possible_grasps);
  std::sort(grasp_order.begin(), grasp_order.end(), oq);

  // feed the available grasps to the stages we set up
  for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
  {
    ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
    const moveit_msgs::Grasp &g = goal.possible_grasps[grasp_order[i]];
    p->approach_ = g.pre_grasp_approach;
    p->retreat_ = g.post_grasp_retreat;
    p->goal_pose_ = g.grasp_pose;
    p->id_ = grasp_order[i];
    // if no frame of reference was specified, assume the transform to be in the reference frame of the object
    if (p->goal_pose_.header.frame_id.empty())
      p->goal_pose_.header.frame_id = goal.target_name;
    p->approach_posture_ = g.pre_grasp_posture;
    p->retreat_posture_ = g.grasp_posture;
    pipeline_.push(p);
  }

  // wait till we're done
  waitForPipeline(endtime);
  pipeline_.stop();

  last_plan_time_ = (ros::WallTime::now() - start_time).toSec();

  if (!getSuccessfulManipulationPlans().empty())
    error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  else
  {
    if (last_plan_time_ > timeout)
      error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
    else
    {
      error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      if (goal.possible_grasps.size() > 0)
      {
        ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
        // everything failed. we now start the pipeline again in verbose mode for one grasp
        initialize();
        pipeline_.setVerbose(true);
        pipeline_.start();
        pipeline_.reprocessLastFailure();
        waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
        pipeline_.stop();
        pipeline_.setVerbose(false);
      }
    }
  }
  ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);

  return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}