Exemple #1
0
PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) const
{
  PlacePlanPtr p(new PlacePlan(shared_from_this()));
  if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
    p->plan(planning_scene, goal);
  else
    p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);

  if (display_computed_motion_plans_)
  {
    const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
    if (!success.empty())
      visualizePlan(success.back());
  }

  if (display_grasps_)
  {
    const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
    visualizeGrasps(success);
    const std::vector<pick_place::ManipulationPlanPtr> &failed = p->getFailedManipulationPlans();
    visualizeGrasps(failed);
  }

  return p;
}
double constraint_samplers::countSamplesPerSecond(const moveit_msgs::Constraints& constr,
                                                  const planning_scene::PlanningSceneConstPtr& scene,
                                                  const std::string& group)
{
  return countSamplesPerSecond(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr),
                               scene->getCurrentState());
}
Exemple #3
0
  planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                            const planning_interface::MotionPlanRequest& req,
                                                            moveit_msgs::MoveItErrorCodes& error_code) const override
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;

    if (req.group_name.empty())
    {
      ROS_ERROR("No group specified to plan for");
      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      return planning_interface::PlanningContextPtr();
    }

    if (!planning_scene)
    {
      ROS_ERROR("No planning scene supplied as input");
      error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
      return planning_interface::PlanningContextPtr();
    }

    // create PlanningScene using hybrid collision detector
    planning_scene::PlanningScenePtr ps = planning_scene->diff();
    ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorHybrid::create(), true);

    // retrieve and configure existing context
    const CHOMPPlanningContextPtr& context = planning_contexts_.at(req.group_name);
    context->setPlanningScene(ps);
    context->setMotionPlanRequest(req);
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    return context;
  }
  void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const std::string &link_name) const
  {
std::cout << "displayPlannerData called ... " << std::endl;
    const ompl_interface::ModelBasedPlanningContextPtr &pc = ompl_interface_->getLastPlanningContext();
    if (pc)
    {
      ompl::base::PlannerData pd(pc->getOMPLSimpleSetup().getSpaceInformation());
      pc->getOMPLSimpleSetup().getPlannerData(pd);
      robot_state::RobotState kstate = planning_scene->getCurrentState();  
      visualization_msgs::MarkerArray arr; 
      std_msgs::ColorRGBA color;
      color.r = 1.0f;
      color.g = 0.25f;
      color.b = 1.0f;
      color.a = 1.0f;
      unsigned int nv = pd.numVertices();
      for (unsigned int i = 0 ; i < nv ; ++i)
      {
        pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
        kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
        const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
	
        visualization_msgs::Marker mk;
        mk.header.stamp = ros::Time::now();
        mk.header.frame_id = planning_scene->getPlanningFrame();
        mk.ns = "planner_data";
        mk.id = i;
        mk.type = visualization_msgs::Marker::SPHERE;
        mk.action = visualization_msgs::Marker::ADD;
        mk.pose.position.x = pos.x();
        mk.pose.position.y = pos.y();
        mk.pose.position.z = pos.z();
        mk.pose.orientation.w = 1.0;
        mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
        mk.color = color;
        mk.lifetime = ros::Duration(30.0);
        arr.markers.push_back(mk);
      }
      pub_markers_.publish(arr); 
    }
  }
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
                                                               const std::string &group, const std::string &state_space_parameterization,
                                                               const planning_scene::PlanningSceneConstPtr &scene, unsigned int samples, unsigned int edges_per_sample)
{ 
  ConstraintApproximationConstructionResults res;
  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, state_space_parameterization);
  if (pc)
  {                                             
    pc->clear();
    pc->setPlanningScene(scene);
    pc->setCompleteInitialState(scene->getCurrentState());

    std::map<std::string, ConstraintApproximationFactoryPtr>::const_iterator it = constraint_factories_.find(constr_hard.name);
    ConstraintApproximationFactory *fct = NULL;
    ConstraintStateStorageOrderFn order;
    if (it != constraint_factories_.end())
    {
      fct = it->second.get();
      order = fct->getOrderFunction();
    }
    
    ros::WallTime start = ros::WallTime::now();
    ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, order, samples, edges_per_sample, res);
    logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
    if (ss)
    {
      ConstraintApproximationPtr ca;
      if (fct)
        ca = fct->allocApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" + 
                                     boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss);
      else
        ca.reset(new ConstraintApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" + 
                                             boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss));
      if (ca)
      {
        if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
          logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
        constraint_approximations_[ca->getName()] = ca;
        res.approx = ca;
      }
    }
    else
      logError("Unable to construct constraint approximation for group '%s'", group.c_str());
  }
  return res;
}
void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene,
                                              const rviz::Color &default_env_color,
                                              const rviz::Color &default_attached_color,
                                              OctreeVoxelRenderMode octree_voxel_rendering,
                                              OctreeVoxelColorMode octree_color_mode,
                                              float default_scene_alpha)
{
  if (!scene)
    return;

  clear();
  
  if (scene_robot_)
  {
    robot_state::RobotState *rs = new robot_state::RobotState(scene->getCurrentState());
    rs->update();
    
    std_msgs::ColorRGBA color;
    color.r = default_attached_color.r_;
    color.g = default_attached_color.g_;
    color.b = default_attached_color.b_;
    color.a = 1.0f;
    planning_scene::ObjectColorMap color_map;
    scene->getKnownObjectColors(color_map);
    scene_robot_->update(robot_state::RobotStateConstPtr(rs), color, color_map);
  }

  const std::vector<std::string> &ids = scene->getWorld()->getObjectIds();
  for (std::size_t i = 0 ; i < ids.size() ; ++i)
  {
    if (show_object_.find(ids[i]) != show_object_.end() && !show_object_[ids[i]])
      continue;

    collision_detection::CollisionWorld::ObjectConstPtr o = scene->getWorld()->getObject(ids[i]);
    rviz::Color color = default_env_color;
    float alpha = default_scene_alpha;
    if (scene->hasObjectColor(ids[i]))
    {
      const std_msgs::ColorRGBA &c = scene->getObjectColor(ids[i]);
      color.r_ = c.r; color.g_ = c.g; color.b_ = c.b;
      alpha = c.a;
    }
    for (std::size_t j = 0 ; j < o->shapes_.size() ; ++j)
      render_shapes_->renderShape(planning_scene_geometry_node_, o->shapes_[j].get(), o->shape_poses_[j],
                                  octree_voxel_rendering, octree_color_mode, color, alpha);
  }
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(
    const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
    const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
    const ConstraintApproximationConstructionOptions& options)
{
  ConstraintApproximationConstructionResults res;
  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
  if (pc)
  {
    pc->clear();
    pc->setPlanningScene(scene);
    pc->setCompleteInitialState(scene->getCurrentState());

    ros::WallTime start = ros::WallTime::now();
    ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
    ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
                   (ros::WallTime::now() - start).toSec());
    if (ss)
    {
      ConstraintApproximationPtr ca(new ConstraintApproximation(
          group, options.state_space_parameterization, options.explicit_motions, constr_hard,
          group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
              ".ompldb",
          ss, res.milestones));
      if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
        ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
      constraint_approximations_[ca->getName()] = ca;
      res.approx = ca;
    }
    else
      ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
                      group.c_str());
  }
  return res;
}
Exemple #8
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;
}
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;
              if (use)
              {
                usedL[constr.position_constraints[p].link_name] = iks;
                logDebug("Allocated an IK-based sampler for group '%s' satisfying position and orientation constraints on link '%s'",
                         jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
              }
            }
          }
        }
    
    // keep track of links constrained with a full pose
    std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL_fullPose = usedL;
    
    for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
    {   
      // if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint only
      if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
        continue;
      
      boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
      if (pc->configure(constr.position_constraints[p]))
      {
        boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
        if(iks->configure(IKSamplingPose(pc))) 
        {
          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;
          if (use)
          {
            usedL[constr.position_constraints[p].link_name] = iks;
            logDebug("Allocated an IK-based sampler for group '%s' satisfying position constraints on link '%s'", 
                     jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
          }
        }
      }
    }
    
    for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
    {            
      // if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation constraint only
      if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
        continue;
      
      boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
      if (oc->configure(constr.orientation_constraints[o]))
      {
        boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
        if(iks->configure(IKSamplingPose(oc))) 
        {
          bool use = true;
          if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
            if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
              use = false;
          if (use)
          {
            usedL[constr.orientation_constraints[o].link_name] = iks;
            logDebug("Allocated an IK-based sampler for group '%s' satisfying orientation constraints on link '%s'", 
                     jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
          } 
        } 
      }
    }
    
    if (usedL.size() == 1)
    {
      if (samplers.empty())
        return usedL.begin()->second;
      else
      {
        samplers.push_back(usedL.begin()->second);
        return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
      }
    } 
    else 
      if (usedL.size() > 1)
      {
        logDebug("Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", jmg->getName().c_str());
        // find the sampler with the smallest sampling volume; delete the rest
        boost::shared_ptr<IKConstraintSampler> iks = usedL.begin()->second;
        double msv = iks->getSamplingVolume();
        for (std::map<std::string, boost::shared_ptr<IKConstraintSampler> >::const_iterator it = ++usedL.begin() ; it != usedL.end() ; ++it)
        {
          double v = it->second->getSamplingVolume();
          if (v < msv)
          {
            iks = it->second;
            msv = v;
          } 
        }
        if (samplers.empty())
        {
          return iks;
        }
        else
        {
          samplers.push_back(iks);
          return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
        }
      }
  }
  
  // if we got to this point, we have not decided on a sampler.
  // we now check to see if we can use samplers from subgroups
  if (!ik_subgroup_alloc.empty())
  {        
    logDebug("There are IK allocators for subgroups of group '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());

    bool some_sampler_valid = false;
    
    std::set<std::size_t> usedP, usedO;
    for (std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn>::const_iterator it = ik_subgroup_alloc.begin() ; it != ik_subgroup_alloc.end() ; ++it)
    {
      // construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator
      moveit_msgs::Constraints sub_constr;
      for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
        if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
          if (usedP.find(p) == usedP.end())
          {
            sub_constr.position_constraints.push_back(constr.position_constraints[p]);
            usedP.insert(p);
          }
      
      for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)        
        if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
          if (usedO.find(o) == usedO.end())
          {
            sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
            usedO.insert(o);
          }
      
      // if some matching constraints were found, construct the allocator
      if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
      {
        logDebug("Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(), jmg->getName().c_str());
        ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
        if (cs)
        {
          logDebug("Constructed a sampler for the joints corresponding to group '%s', but part of group '%s'", 
                   it->first->getName().c_str(), jmg->getName().c_str());
          some_sampler_valid = true;
          samplers.push_back(cs);
        }
      }
    }
    if (some_sampler_valid)
    {
      logDebug("Constructing sampler for group '%s' as a union of %u samplers", jmg->getName().c_str(), (unsigned int)samplers.size());
      return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
    }
  }

  //if we've gotten here, just return joint sampler
  if (joint_sampler)
  {
    logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
    return joint_sampler;
  }
  
  logDebug("No constraints sampler allocated for group '%s'", jmg->getName().c_str());
  
  return ConstraintSamplerPtr();
}
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);
  }
  
  ROS_INFO("Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_INFO("Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.joint_trajectory.points[goal_index].time_from_start.toSec());
  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec());
  return true;
}
    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);
        }
    }
// ******************************************************************************************
// Generates an adjacency list of links that are always and never in collision, to speed up collision detection
// ******************************************************************************************
LinkPairMap
computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int * progress,
                         const bool include_never_colliding, const unsigned int num_trials, const double min_collision_fraction,
                         const bool verbose)
{
  // Create new instance of planning scene using pointer
  planning_scene::PlanningScenePtr scene = parent_scene->diff();

  // Map of disabled collisions that contains a link as a key and an ordered list of links that are connected.
  LinkPairMap link_pairs;

  // Track unique edges that have been found to be in collision in some state
  StringPairSet links_seen_colliding;

  // LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second
  LinkGraph link_graph;

  //ROS_INFO_STREAM("Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() );

  // 0. GENERATE ALL POSSIBLE LINK PAIRS -------------------------------------------------------------
  // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically.
  // There should be n choose 2 pairs
  computeLinkPairs( *scene, link_pairs );
  *progress = 1;

  // 1. FIND CONNECTING LINKS ------------------------------------------------------------------------
  // For each link, compute the set of other links it connects to via a single joint (adjacent links)
  // or via a chain of joints with intermediate links with no geometry (like a socket joint)

  // Create Connection Graph
  computeConnectionGraph(scene->getRobotModel()->getRootLink(), link_graph);
  *progress = 2; // Progress bar feedback

  // 2. DISABLE ALL ADJACENT LINK COLLISIONS ---------------------------------------------------------
  // if 2 links are adjacent, or adjacent with a zero-shape between them, disable collision checking for them
  unsigned int num_adjacent = disableAdjacentLinks( *scene, link_graph, link_pairs);
  *progress = 4; // Progress bar feedback

  // 3. INITIAL CONTACTS TO CONSIDER GUESS -----------------------------------------------------------
  // Create collision detection request object
  collision_detection::CollisionRequest req;
  req.contacts = true;
  // max number of contacts to compute. initial guess is number of links on robot
  req.max_contacts = int(link_graph.size());
  req.max_contacts_per_pair = 1;
  req.verbose = false;

  // 4. DISABLE "DEFAULT" COLLISIONS --------------------------------------------------------
  // Disable all collision checks that occur when the robot is started in its default state
  unsigned int num_default = disableDefaultCollisions(*scene, link_pairs, req);
  *progress = 6; // Progress bar feedback

  // 5. ALWAYS IN COLLISION --------------------------------------------------------------------
  // Compute the links that are always in collision
  unsigned int num_always = disableAlwaysInCollision(*scene, link_pairs, req, links_seen_colliding, min_collision_fraction);
  //ROS_INFO("Links seen colliding total = %d", int(links_seen_colliding.size()));
  *progress = 8; // Progress bar feedback

  // 6. NEVER IN COLLISION -------------------------------------------------------------------
  // Get the pairs of links that are never in collision
  unsigned int num_never = 0;
  if (include_never_colliding) // option of function
  {
    num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress);
  }

  //ROS_INFO("Link pairs seen colliding ever: %d", int(links_seen_colliding.size()));

  if(verbose)
  {
    // Calculate number of disabled links:
    unsigned int num_disabled = 0;
    for ( LinkPairMap::const_iterator pair_it = link_pairs.begin() ; pair_it != link_pairs.end() ; ++pair_it)
    {
      if( pair_it->second.disable_check ) // has a reason to be disabled
        ++num_disabled;
    }

    ROS_INFO("-------------------------------------------------------------------------------");
    ROS_INFO("Statistics:");
    unsigned int num_links = int(link_graph.size());
    double num_possible = boost::math::binomial_coefficient<double>(num_links, 2); // n choose 2
    unsigned int num_sometimes = num_possible - num_disabled;

    ROS_INFO("%6d : %s",   num_links, "Total Links");
    ROS_INFO("%6.0f : %s", num_possible, "Total possible collisions");
    ROS_INFO("%6d : %s",   num_always, "Always in collision");
    ROS_INFO("%6d : %s",   num_never, "Never in collision");
    ROS_INFO("%6d : %s",   num_default, "Default in collision");
    ROS_INFO("%6d : %s",   num_adjacent, "Adjacent links disabled");
    ROS_INFO("%6d : %s",   num_sometimes, "Sometimes in collision");
    ROS_INFO("%6d : %s",   num_disabled, "TOTAL DISABLED");

    /*ROS_INFO("Copy to Spreadsheet:");
    ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never
                    << "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes
                    << "\t" << num_disabled);
    */
  }

  *progress = 100; // end the status bar

  return link_pairs;
}
Exemple #13
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;
}
Exemple #14
0
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)
  {
    if (!(trajectory.fillInFromTrajectory(res)))
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
                                              "trajectory must contain at least start and goal state");
      return false;
    }
  }
  else
    ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");

  ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                 (params.trajectory_initialization_method_).c_str());

  // optimize!
  moveit::core::RobotState start_state(planning_scene->getCurrentState());
  moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
  start_state.update();

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

  int replan_count = 0;
  bool replan_flag = false;
  double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
  int org_max_iterations = 200;

  // storing the initial chomp parameters values
  org_learning_rate = params.learning_rate_;
  org_ridge_factor = params.ridge_factor_;
  org_planning_time_limit = params.planning_time_limit_;
  org_max_iterations = params.max_iterations_;

  std::unique_ptr<ChompOptimizer> optimizer;

  // create a non_const_params variable which stores the non constant version of the const params variable
  ChompParameters params_nonconst = params;

  // while loop for replanning (recovery behaviour) if collision free optimized solution not found
  while (true)
  {
    if (replan_flag)
    {
      // increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
      // additional secs in hope to find a solution; increase maximum iterations
      params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
                                        params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
    }

    // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
    // case of a recovery behaviour
    optimizer.reset(new ChompOptimizer(&trajectory, planning_scene, req.group_name, &params_nonconst, start_state));
    if (!optimizer->isInitialized())
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      return false;
    }

    ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
                    (ros::WallTime::now() - create_time).toSec());

    bool optimization_result = optimizer->optimize();

    // replan with updated parameters if no solution is found
    if (params_nonconst.enable_failure_recovery_)
    {
      ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
                                      "planning_time_limit, max_iterations), attempt: # %d ",
                     (replan_count + 1));
      ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
                     params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
                     params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);

      if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
      {
        replan_count++;
        replan_flag = true;
      }
      else
      {
        break;
      }
    }
    else
      break;
  }  // end of while loop

  // resetting the CHOMP Parameters to the original values after a successful plan
  params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);

  ROS_DEBUG_NAMED("chomp_planner", "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_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());

  res.trajectory.resize(1);

  res.trajectory[0].joint_trajectory.joint_names = active_joint_names;

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

  // fill in the entire trajectory
  res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
  for (int i = 0; i < trajectory.getNumPoints(); i++)
  {
    res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
    for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
    {
      res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
    }
    // Setting invalid timestamps.
    // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
    res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
  }

  ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
                  (ros::WallTime::now() - start_time).toSec(),
                  res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());

  // report planning failure if path has collisions
  if (not optimizer->isCollisionFree())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // check that final state is within goal tolerances
  kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
  robot_state::RobotState last_state(start_state);
  last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names,
                                  res.trajectory[0].joint_trajectory.points.back().positions);

  bool constraints_are_ok = true;
  for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
  {
    constraints_are_ok = constraints_are_ok and jc.configure(constraint);
    constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
    if (not constraints_are_ok)
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
      res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
      return false;
    }
  }

  return true;
}
Exemple #15
0
bool StompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                   const moveit_msgs::MotionPlanRequest &req,
                   moveit_msgs::MotionPlanDetailedResponse &res) const
{
  ros::WallTime start_time = ros::WallTime::now();
  boost::shared_ptr<StompOptimizationTask> stomp_task;
  boost::shared_ptr<stomp::STOMP> stomp;

  int max_rollouts;
  STOMP_VERIFY(node_handle_.getParam("max_rollouts", max_rollouts));

  // prepare the collision checkers
  boost::shared_ptr<const collision_detection::CollisionRobot> collision_robot; /**< standard robot collision checker */
  boost::shared_ptr<const collision_detection::CollisionWorld> collision_world; /**< standard robot -> world collision checker */
  boost::shared_ptr<collision_detection::CollisionRobotDistanceField> collision_robot_df;    /**< distance field robot collision checker */
  boost::shared_ptr<collision_detection::CollisionWorldDistanceField> collision_world_df;    /**< distance field robot -> world collision checker */

  collision_robot = planning_scene->getCollisionRobot();
  collision_world = planning_scene->getCollisionWorld();
  std::map<std::string, std::vector<collision_detection::CollisionSphere> > link_body_decompositions;
  bool use_signed_distance_field = true;
  double padding = 0.0;
  double scale = 1.0;

  // TODO: remove this disgusting hack!
  link_body_decompositions["r_shoulder_pan_link"] = std::vector<collision_detection::CollisionSphere>();
  link_body_decompositions["r_shoulder_lift_link"] = std::vector<collision_detection::CollisionSphere>();

  collision_robot_df.reset(new collision_detection::CollisionRobotDistanceField(kinematic_model_,
                                                                                 link_body_decompositions,
                                                                                 df_size_x_, df_size_y_, df_size_z_,
                                                                                 use_signed_distance_field,
                                                                                 df_resolution_, df_collision_tolerance_,
                                                                                 df_max_propagation_distance_,
                                                                                 padding, scale));
  collision_world_df.reset(new collision_detection::CollisionWorldDistanceField(df_size_x_, df_size_y_, df_size_z_,
                                                                                 use_signed_distance_field,
                                                                                 df_resolution_, df_collision_tolerance_,
                                                                                 df_max_propagation_distance_));
  copyObjects(collision_world, collision_world_df);


  // first setup the task
  stomp_task.reset(new StompOptimizationTask(node_handle_, req.group_name,
                                             kinematic_model_,
                                             collision_robot, collision_world,
                                             collision_robot_df, collision_world_df));

  int num_threads=1;
  STOMP_VERIFY(stomp_task->initialize(num_threads, max_rollouts));

  XmlRpc::XmlRpcValue features_xml;
  STOMP_VERIFY(node_handle_.getParam("features", features_xml));
  stomp_task->setFeaturesFromXml(features_xml);
  stomp_task->setControlCostWeight(0.00001);
  stomp_task->setTrajectoryVizPublisher(const_cast<ros::Publisher&>(trajectory_viz_pub_));
  stomp_task->setDistanceFieldVizPublisher((const_cast<ros::Publisher&>(trajectory_viz_pub_)));
  stomp_task->setRobotBodyVizPublisher((const_cast<ros::Publisher&>(robot_body_viz_pub_)));
  stomp_task->setMotionPlanRequest(planning_scene, req);

  stomp.reset(new stomp::STOMP());
  stomp->initialize(node_handle_, stomp_task);

  // TODO: don't hardcode these params
  bool success = stomp->runUntilValid(100, 10);

  std::vector<Eigen::VectorXd> best_params;
  double best_cost;
  stomp->getBestNoiselessParameters(best_params, best_cost);
  stomp_task->publishTrajectoryMarkers(const_cast<ros::Publisher&>(trajectory_viz_pub_), best_params);
  res.trajectory_start = req.start_state;
  res.group_name = req.group_name;
  res.trajectory.resize(1);
  stomp_task->parametersToJointTrajectory(best_params, res.trajectory[0].joint_trajectory);
  res.description.resize(1);
  res.description[0] = "STOMP";
  res.processing_time.resize(1);
  ros::WallDuration wd = ros::WallTime::now() - start_time;
  res.processing_time[0] = ros::Duration(wd.sec, wd.nsec);

  if (!success)
  {
    ROS_ERROR("STOMP: failed to find a collision-free plan");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
    return true;
  }

  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;

  // res.trajectory
  // res.description
  // res.processing_time
  // res.error_code

  return true;
}
bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const moveit_msgs::GetMotionPlan::Request &req,
                          moveit_msgs::GetMotionPlan::Response &res,
                          const PlanningParameters& params) const
{
    res.trajectory.joint_trajectory.points.clear();
    (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = PlanningStatistics();
    planning_models::RobotState *start_state(planning_scene->getCurrentState());
    planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);

    ros::WallTime wt = ros::WallTime::now();
    boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene));
    if(!env_chain->setupForMotionPlan(planning_scene,
                                      req,
                                      res,
                                      params)) {
        //std::cerr << "Env chain setup failing" << std::endl;
        return false;
    }
    //std::cerr << "Creation with params " << params.use_bfs_ << " took " << (ros::WallTime::now()-wt).toSec() << std::endl;
    boost::this_thread::interruption_point();

    //DummyEnvironment* dummy_env = new DummyEnvironment();
    boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true));
    planner->set_initialsolution_eps(100.0);
    planner->set_search_mode(true);
    planner->force_planning_from_scratch();
    planner->set_start(env_chain->getPlanningData().start_hash_entry_->stateID);
    planner->set_goal(env_chain->getPlanningData().goal_hash_entry_->stateID);
    //std::cerr << "Creation took " << (ros::WallTime::now()-wt) << std::endl;
    std::vector<int> solution_state_ids;
    int solution_cost;
    wt = ros::WallTime::now();
    //CALLGRIND_START_INSTRUMENTATION;
    bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost);
    //CALLGRIND_STOP_INSTRUMENTATION;
    double el = (ros::WallTime::now()-wt).toSec();
    std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl;
    std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_
              << " average time " << (env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
              << " hz " << 1.0/(env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
              << std::endl;
    std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz " << 1.0/(env_chain->getPlanningStatistics().total_coll_check_time_.toSec()/(env_chain->getPlanningStatistics().coll_checks_*1.0)) << std::endl;
    std::cerr << "Path length is " << solution_state_ids.size() << std::endl;
    if(!b_ret) {
        res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
        return false;
    }
    if(solution_state_ids.size() == 0) {
        std::cerr << "Success but no path" << std::endl;
        res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
        return false;
    }
    if(!env_chain->populateTrajectoryFromStateIDSequence(solution_state_ids,
            res.trajectory.joint_trajectory)) {
        std::cerr << "Success but path bad" << std::endl;
        res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
        return false;
    }
    ros::WallTime pre_short = ros::WallTime::now();
    //std::cerr << "Num traj points before " << res.trajectory.joint_trajectory.points.size() << std::endl;
    trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory;
    env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory);
    //std::cerr << "Num traj points after " << res.trajectory.joint_trajectory.points.size() << std::endl;
    //std::cerr << "Time " << (ros::WallTime::now()-pre_short).toSec() << std::endl;
    //env_chain->getPlaneBFSMarker(mark, env_chain->getGoalPose().translation().z());
    res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    PlanningStatistics stats = env_chain->getPlanningStatistics();
    stats.total_planning_time_ = ros::WallDuration(el);
    (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = stats;
    return true;
}
  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);

    // if the start state is otherwise valid but does not meet path constraints
    if (planning_scene->isStateValid(start_state, req.group_name) &&
        !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
    {
      ROS_INFO("Path constraints not satisfied for start state...");
      planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
      ROS_INFO("Planning to path constraints...");

      planning_interface::MotionPlanRequest req2 = req;
      req2.goal_constraints.resize(1);
      req2.goal_constraints[0] = req.path_constraints;
      req2.path_constraints = moveit_msgs::Constraints();
      planning_interface::MotionPlanResponse res2;
      // we call the planner for this additional request, but we do not want to include potential 
      // index information from that call
      std::vector<std::size_t> added_path_index_temp;
      added_path_index_temp.swap(added_path_index);
      bool solved1 = planner(planning_scene, req2, res2);
      added_path_index_temp.swap(added_path_index);
      
      if (solved1)
      {
        planning_interface::MotionPlanRequest req3 = req;
        ROS_INFO("Planned to path constraints. Resuming original planning request.");

        // extract the last state of the computed motion plan and set it as the new start state
        robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
        bool solved2 = planner(planning_scene, req3, res);
        res.planning_time_ += res2.planning_time_;

        if (solved2)
        {
          // since we add a prefix, we need to correct any existing index positions
          for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
            added_path_index[i] += res2.trajectory_->getWayPointCount();
          
          // we mark the fact we insert a prefix path (we specify the index position we just added)
          for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
            added_path_index.push_back(i);
          
          // we need to append the solution paths.
          res2.trajectory_->append(*res.trajectory_, 0.0);
          res2.trajectory_->swap(*res.trajectory_);
          return true;
        }
        else
          return false;
      }
      else
      {
        ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
        bool result = planner(planning_scene, req, res);
        res.planning_time_ += res2.planning_time_;
        return result;
      }
    }
    else
    {
      ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
      return planner(planning_scene, req, res);
    }
  }
void constraint_samplers::visualizeDistribution(const moveit_msgs::Constraints& constr,
                                                const planning_scene::PlanningSceneConstPtr& scene,
                                                const std::string& group, const std::string& link_name,
                                                unsigned int sample_count, visualization_msgs::MarkerArray& markers)
{
  visualizeDistribution(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState(),
                        link_name, sample_count, markers);
}
bool ConvexConstraintSolver::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                   const moveit_msgs::GetMotionPlan::Request &req,
                   moveit_msgs::GetMotionPlan::Response &res) const
{
  // Need to add in joint limit constraints
  // Get the position constraints from the collision detector

  // The raw algorithm laid out in Chan et. al. is as follows:
  //  1. Compute unconstrained motion to go from proxy to goal.
  //  2. Get the current contact set by moving the proxy toward the goal by some epsilon and get all collision points.
  //  3. Compute constrained motion (convex solver).
  //  4. Compute collisions along the constrained motion path.
  //  5. Set proxy to stop at the first new contact along path.


  // In our framework this will look more like this:
  // Outside ths planner:
  //  1. Compute error between cartesian end effector and cartesian goal.
  //  2. Cap pose error (based on some heuristic?) because we are about to make a linear approximation.
  //  3. Use Jinverse to compute the joint deltas for the pose error (or perhaps we should frame this as a velocity problem).
  //  4. Add the joint deltas to the start state to get a goal state.

  // Inside the planner:
  //  1. Get the "current" contact set by moving the proxy toward the goal by some epsilon and get all collision points.
  //  2. Compute constrained motion (convex solver).
  //  3. Subdivide motion subject to some sort of minimum feature size.
  //  4. Move proxy in steps, checking for colliding state along the way. Optionally use interval bisection to refine.

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -/

  std::string group_name = req.motion_plan_request.group_name;
  std::string ee_group_name;
  std::string ee_control_frame;
  if(group_name == "right_arm"){ ee_group_name = "r_end_effector"; ee_control_frame = "r_wrist_roll_link"; }
  if(group_name == "left_arm") { ee_group_name = "l_end_effector"; ee_control_frame = "l_wrist_roll_link"; }


  ROS_WARN("Solving for group [%s], end-effector [%s], control frame [%s] using ConvexConstraintSolver!", group_name.c_str(), ee_group_name.c_str(), ee_control_frame.c_str());



  // We "getCurrentState" just to populate the structure with auxiliary info, then copy in the transform info from the planning request.
  planning_models::KinematicState start_state = planning_scene->getCurrentState();
  planning_models::robotStateToKinematicState(*(planning_scene->getTransforms()), req.motion_plan_request.start_state, start_state);

  // constrained_goal_state is the optimization output before interval stepping, proxy_state will be used as we step around.
  planning_models::KinematicState proxy_state = start_state;
  planning_models::KinematicState constrained_goal_state = start_state;

  std::string planning_frame = ros::names::resolve("/", planning_scene->getPlanningFrame());


    const planning_models::KinematicState::JointStateGroup * jsg = proxy_state.getJointStateGroup(group_name);
    const planning_models::KinematicModel::JointModelGroup * jmg = planning_scene->getKinematicModel()->getJointModelGroup(group_name);
//    std::string end_effector_group_name = jmg->getAttachedEndEffectorGroupName();
//    const std::vector<std::string>& subgroups = jmg->getSubgroupNames();

//    ROS_INFO("End-effector group name is [%s].", end_effector_group_name.c_str());
//    for(size_t i = 0; i< subgroups.size(); i++)
//      ROS_INFO("Subgroup [%zd] is [%s].", i, subgroups[i].c_str());
    const planning_models::KinematicModel::JointModelGroup * ee_jmg = planning_scene->getKinematicModel()->getJointModelGroup(ee_group_name);


//    const std::vector<const planning_models::KinematicModel::JointModel*>& joint_models = jmg->getJointModels();
//    std::map<std::string, unsigned int> joint_index_map;
//    for(size_t i = 0; i < joint_models.size(); i++)
//    {
//      //ROS_INFO("Group [%s] has joint %d: [%s]", req.motion_plan_request.group_name.c_str(), i, joint_models[i]->getName().c_str());
//      joint_index_map[joint_models[i]->getName()] = i;
//    }

    const std::map<std::string, unsigned int>& joint_index_map = jmg->getJointVariablesIndexMap();

    std::vector<double> limits_min, limits_max, joint_vector;
    std::vector<std::string> joint_names;

    limits_min.resize(7);
    limits_max.resize(7);
    joint_vector.resize(7);
    joint_names.resize(7);


    {
      const moveit_msgs::Constraints &c = req.motion_plan_request.goal_constraints[1];
      for (std::size_t i = 0 ; i < c.joint_constraints.size() ; ++i)
      {
        std::string joint_name = c.joint_constraints[i].joint_name;
        if(joint_index_map.find(joint_name) == joint_index_map.end())
        {
          ROS_WARN("Didin't find [%s] in the joint map, ignoring...", joint_name.c_str());
          continue;
        }
        unsigned int joint_index = joint_index_map.find(joint_name)->second;

        std::vector<moveit_msgs::JointLimits> limits = planning_scene->getKinematicModel()->getJointModel(joint_name)->getLimits();
        moveit_msgs::JointLimits limit = limits[0];
        if(limit.has_position_limits)
        {
          limits_min[joint_index] = limit.min_position;
          limits_max[joint_index] = limit.max_position;
        }
        else
        {
          limits_min[joint_index] = -1E3;
          limits_max[joint_index] =  1E3;
        }
        joint_vector[joint_index] = start_state.getJointState(joint_name)->getVariableValues()[0];
        joint_names[joint_index] = joint_name;
        //ROS_INFO("Joint [%d] [%s] has min %.2f, value %.2f, max %.2f", joint_index, joint_name.c_str(), limits_min[joint_index], joint_vector[joint_index], limits_max[joint_index]);
      }
    }

// ======== Extract all contact points and normals from previous collision state, get associated Jacobians ========

    std::vector<Eigen::MatrixXd> contact_jacobians;
    std::vector<Eigen::Vector3d> contact_normals;

    for( collision_detection::CollisionResult::ContactMap::const_iterator it = last_collision_result.contacts.begin(); it != last_collision_result.contacts.end(); ++it)
    {
      std::string contact1 = it->first.first;
      std::string contact2 = it->first.second;
      const std::vector<collision_detection::Contact>& vec = it->second;

      for(size_t contact_index = 0; contact_index < vec.size(); contact_index++)
      {
        Eigen::Vector3d point =     vec[contact_index].pos;
        Eigen::Vector3d normal =  vec[contact_index].normal;
        double depth = vec[contact_index].depth;
        ROS_INFO("Contact between [%s] and [%s] point: %.2f %.2f %.2f normal: %.2f %.2f %.2f depth: %.3f",
                 contact1.c_str(), contact2.c_str(),
                 point(0), point(1), point(2),
                 normal(0), normal(1), normal(2),
                 depth);
        // Contact point needs to be expressed with respect to the link; normals should stay in the common frame
        std::string group_contact;
        if(      jmg->hasLinkModel(contact1) || ee_jmg->hasLinkModel(contact1) ) group_contact = contact1;
        else if( jmg->hasLinkModel(contact2) || ee_jmg->hasLinkModel(contact2) ) group_contact = contact2;
        else
        {
          ROS_WARN("Contact isn't on group [%s], skipping...", req.motion_plan_request.group_name.c_str());
          continue;
        }
        planning_models::KinematicState::LinkState *link_state = start_state.getLinkState(group_contact);
        Eigen::Affine3d link_T_world = link_state->getGlobalCollisionBodyTransform().inverse();
        point = link_T_world*point;

        Eigen::MatrixXd jacobian;
        if(jsg->getJacobian(group_contact, point, jacobian))
        {
          contact_jacobians.push_back(jacobian);
          contact_normals.push_back(normal);
        }
      }
    }


// ======== Extract goal "constraints" ========

  const moveit_msgs::Constraints &c = req.motion_plan_request.goal_constraints[0];

  // Position and Orientation
  if(c.position_constraints.size() != 1 || c.orientation_constraints.size() != 1)
  {
    ROS_ERROR("Currently require exactly one position and orientation constraint. Aborting...");
    return false;
  }
  moveit_msgs::PositionConstraint    pc = c.position_constraints[0];
  moveit_msgs::OrientationConstraint oc = c.orientation_constraints[0];
  pc.header.frame_id = ros::names::resolve("/", pc.header.frame_id);
  oc.header.frame_id = ros::names::resolve("/", oc.header.frame_id);

  if(pc.link_name != oc.link_name)
  {
    ROS_ERROR("Currently can't support position and orientation goals that are not for the same link. Aborting...");
    return false;
  }
  if(pc.constraint_region.primitive_poses.size() == 0)
  {
    ROS_ERROR("Need to specify a single primitive_pose for position constraint region. Aborting...");
    return false;
  }
  if(pc.constraint_region.primitive_poses.size() != 1)
  {
    ROS_ERROR("Need exactly one 'pose' for the end-effector goal region. Aborting...");
  }


  if(pc.header.frame_id != planning_frame)
    ROS_WARN("The position goal header [%s] and planning_frame [%s] don't match, things are probably all wrong!",
             pc.header.frame_id.c_str(), planning_frame.c_str() );
  if(oc.header.frame_id != planning_frame)
    ROS_WARN("The orientation goal header [%s] and planning_frame [%s] don't match, things are probably all wrong!",
             oc.header.frame_id.c_str(), planning_frame.c_str() );

  Eigen::Vector3d goal_point;
  Eigen::Quaterniond goal_quaternion_e;
  { // scoped so we don't pollute function scope with these message temps
    geometry_msgs::Point msg_goal_point = pc.constraint_region.primitive_poses[0].position;
    geometry_msgs::Quaternion msg_goal_orientation = oc.orientation;
    goal_point = Eigen::Vector3d(msg_goal_point.x, msg_goal_point.y, msg_goal_point.z);
    goal_quaternion_e = Eigen::Quaterniond(msg_goal_orientation.w, msg_goal_orientation.x, msg_goal_orientation.y, msg_goal_orientation.z);
  }


  //ROS_INFO("Computing position and orientation error...");
  planning_models::KinematicState::LinkState *link_state = start_state.getLinkState(pc.link_name);
  Eigen::Affine3d planning_T_link = link_state->getGlobalLinkTransform();

  Eigen::Vector3d ee_point_in_ee_frame = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
  Eigen::Vector3d ee_point_in_planning_frame = planning_T_link*ee_point_in_ee_frame;

  // TODO need to make sure these are expressed in the same frame.
  Eigen::Vector3d x_error = goal_point - ee_point_in_planning_frame;
  Eigen::Vector3d delta_x = x_error;
  double x_error_mag = x_error.norm();
  double LINEAR_CLIP = 0.02;
  if(x_error_mag > LINEAR_CLIP)
    delta_x = x_error/x_error_mag*LINEAR_CLIP;


  // = = = = Rotations are gross = = = =
  Eigen::Quaterniond link_quaternion_e = Eigen::Quaterniond(planning_T_link.rotation());
  tf::Quaternion link_quaternion_tf, goal_quaternion_tf;
  tf::RotationEigenToTF( link_quaternion_e, link_quaternion_tf );
  tf::RotationEigenToTF( goal_quaternion_e, goal_quaternion_tf );

  tf::Quaternion delta_quaternion = link_quaternion_tf.inverse()*goal_quaternion_tf;
  double rotation_angle = delta_quaternion.getAngle();
  //tf::Vector3 rotation_axis = delta_quaternion.getAxis();
  //ROS_INFO("Delta is [%.3f] radians about [%.3f, %.3f, %.3f]", rotation_angle, rotation_axis.x(), rotation_axis.y(), rotation_axis.z());
  double ANGLE_CLIP = 0.2;
  double clipped_rotation_fraction = std::min<double>(1.0, ANGLE_CLIP/fabs(rotation_angle));
  if(clipped_rotation_fraction < 0) ROS_ERROR("Clipped rotation fraction < 0, look into this!");

  tf::Matrix3x3 clipped_delta_matrix;
  tf::Quaternion clipped_goal_quaternion = link_quaternion_tf.slerp(goal_quaternion_tf, clipped_rotation_fraction);
  clipped_delta_matrix.setRotation( link_quaternion_tf.inverse()*clipped_goal_quaternion );


  tf::Vector3 delta_euler;
  clipped_delta_matrix.getRPY(delta_euler[0], delta_euler[1], delta_euler[2]);
  //ROS_INFO("Delta euler in link frame = [%.3f, %.3f, %.3f]", delta_euler[0], delta_euler[1], delta_euler[2]);
  tf::Matrix3x3 link_matrix(link_quaternion_tf);
  delta_euler = link_matrix * delta_euler;
  //ROS_INFO("Delta euler in planning_frame = [%.3f, %.3f, %.3f]", delta_euler[0], delta_euler[1], delta_euler[2]);


  // Get end-effector Jacobian
//  std::string ee_control_frame = "r_wrist_roll_link";
//  if(false && jmg->isChain())
//    ee_control_frame = planning_scene->getKinematicModel()->getJointModelGroup(jmg->getAttachedEndEffectorGroupName())->getEndEffectorParentGroup().second;
//  else
//    ROS_WARN("Using r_wrist_roll_link as HARD_CODED value.");
  if(ee_control_frame != pc.link_name)
    ROS_WARN("ee_control_frame [%s] and position_goal link [%s] aren't the same, this could be bad!", ee_control_frame.c_str(), pc.link_name.c_str());


//  ROS_INFO("Getting end-effector Jacobian for local point %.3f, %.3f, %.3f on link [%s]",
//           ee_point_in_ee_frame(0),
//           ee_point_in_ee_frame(1),
//           ee_point_in_ee_frame(2),
//           ee_control_frame.c_str());
  Eigen::MatrixXd ee_jacobian;
  if(!jsg->getJacobian(ee_control_frame , ee_point_in_ee_frame , ee_jacobian))
  {
    ROS_ERROR("Unable to get end-effector Jacobian! Can't plan, exiting...");
    return false;
  }

  //ROS_INFO_STREAM("End-effector jacobian in planning frame is: \n" << ee_jacobian);


// ======== Pack into solver data structure, run solver. ========

  //ROS_INFO("Packing data into the cvx solver...");
//  Vars vars;
//  Params params;
//  Workspace work;
//  Settings settings;

  // CVX Settings
  cvx.set_defaults();
  cvx.setup_indexing();
  cvx.settings.verbose = 0;

  // - - - - - - - load all problem instance data - - - - - - - //

  unsigned int N = 7; // number of joints in the chain

  // end-effector Jacobian
  // TODO magic numbers (though I suppose the CVX solver is already hard-coded)
  for(unsigned int row = 0; row < 3; row++ )
  {
    for(unsigned int col = 0; col < N; col++ )
    {
      // CVX matrices are COLUMN-MAJOR!!!
      cvx.params.J_v[col*3 + row] = ee_jacobian(row,   col);
      cvx.params.J_w[col*3 + row] = ee_jacobian(row+3, col);
    }
  }

  // Set weights for objective terms
  cvx.params.weight_x[0] = 1.0; // translational error
  cvx.params.weight_w[0] = 0.1; // angular error (error in radians is numerically much larger than error in meters)
  cvx.params.weight_q[0] = 0.001; // only want to barely encourage values to stay small...

  // set up constraints from contact set
  unsigned int MAX_CONSTRAINTS = 25;
  unsigned int constraint_count = std::min<size_t>(MAX_CONSTRAINTS, contact_normals.size());
  for(unsigned int constraint = 0; constraint < MAX_CONSTRAINTS; constraint++)
  {
    if(constraint < constraint_count)
    {
      // CVX matrices are COLUMN-MAJOR!!!
      for (int j = 0; j < 3*7; j++) {
        cvx.params.J_c[constraint][j] = contact_jacobians[constraint](j%3, j/3);
      }
      for (int j = 0; j < 3; j++) {
        cvx.params.normal[constraint][j] = contact_normals[constraint][j];
      }
    }
    else{
      //printf("setting to zero\n");
      for (int j = 0; j < 3*7; j++) {
        cvx.params.J_c[constraint][j] = 0;
      }
      for (int j = 0; j < 3; j++) {
        cvx.params.normal[constraint][j] = 0;
      }
    }
  }

  for(unsigned int index = 0; index < N; index++ )
  {
    cvx.params.q[index] = joint_vector[index];
    cvx.params.q_min[index] = limits_min[index];
    cvx.params.q_max[index] = limits_max[index];
  }

  // TODO should these be clipped down at all? :)
  for(unsigned int index = 0; index < 3; index++ )
  {
    cvx.params.x_d[index] = delta_x(index);  // TODO magic minus sign!!
    cvx.params.w_d[index] = delta_euler[index];
  }



  // - - - - - - - Solve our problem at high speed! - - - - - - - //
  long num_iters = 0;
  num_iters = cvx.solve();
  if(!cvx.work.converged)
  {
    printf("solving failed to converge in %ld iterations.\n", num_iters);
    return false;
  }

// ======== Unpack solver result into constrained goal state. ========



  ROS_INFO("Finished solving in %ld iterations, unpacking data...", num_iters);
  Eigen::VectorXd joint_deltas(N);
  std::map<std::string, double> goal_update;
  for(size_t joint_index = 0; joint_index < joint_names.size(); joint_index++)
  {
    std::string joint_name = joint_names[joint_index];
    joint_deltas(joint_index) = cvx.vars.q_d[joint_index];
    goal_update[joint_name] = cvx.vars.q_d[joint_index] + start_state.getJointState(joint_name)->getVariableValues()[0];
    //ROS_INFO("Updated joint [%zd] [%s]: %.3f + %.3f", joint_index, joint_name.c_str(), start_state.getJointState(joint_name)->getVariableValues()[0], cvx.vars.q_d[joint_index]);
  }
  Eigen::VectorXd cartesian_deltas = ee_jacobian*joint_deltas;

  //ROS_INFO("Raw Error: translate [%.3f, %.3f, %.3f]",
  //         x_error(0), x_error(1), x_error(2));
  ROS_INFO("ClipError: translate [%.3f, %.3f, %.3f]  euler [%.2f, %.2f, %.2f]",
           delta_x(0), delta_x(1), delta_x(2),
           delta_euler[0], delta_euler[1], delta_euler[2]);
  ROS_INFO("Output:    translate [%.3f, %.3f, %.3f]  euler [%.2f, %.2f, %.2f]",
           cartesian_deltas(0), cartesian_deltas(1), cartesian_deltas(2),
           cartesian_deltas(3), cartesian_deltas(4), cartesian_deltas(5));


  //goal_update[vars.qdd_c[index] + ]
  constrained_goal_state.setStateValues(goal_update);


// ======== Step toward constrained goal, checking for new collisions along the way ========

  //double proxy_goal_tolerance = 0.1;
  double interpolation_progress = 0.0;
  double interpolation_step = 0.34;
  collision_detection::CollisionResult collision_result;
  while(interpolation_progress <= 1.0)
  {
    //ROS_INFO("Doing interpolation, with progress %.2f ", interpolation_progress);

    // TODO this interpolation scheme might not allow the arm to slide along contacts very well...
    planning_models::KinematicStatePtr point;
    point.reset(new planning_models::KinematicState(constrained_goal_state));
    start_state.interpolate(constrained_goal_state, interpolation_progress, *point);

    // get contact set
    collision_detection::CollisionRequest collision_request;
    // TODO magic number
    collision_request.max_contacts = 50;
    collision_request.contacts = true;
    collision_request.distance = false;
    collision_request.verbose = false;

    collision_result.clear();
    planning_scene->checkCollision(collision_request, collision_result, *point);
    if(collision_result.collision)
    {
      break;
    }
    else
    {
      proxy_state = *point;
    }
    // TODO Use proxy_goal_tolerance to exit if we are close enough!
    interpolation_progress += interpolation_step;
  }
  // Store the last collision result!
  last_collision_result = collision_result;

// ======== Convert proxy state to a "trajectory" ========
  //ROS_INFO("Converting proxy to a trajectory, and returning...");

  moveit_msgs::RobotTrajectory rt;
  trajectory_msgs::JointTrajectory traj;
  trajectory_msgs::JointTrajectoryPoint pt;
  sensor_msgs::JointState js;

  planning_models::kinematicStateToJointState(proxy_state, js);
  //const planning_models::KinematicModel::JointModelGroup *jmg = planning_scene->getKinematicModel()->getJointModelGroup(req.motion_plan_request.group_name);

  // getJointNames
  for(size_t i = 0 ; i < js.name.size(); i++)
  {
    std::string name = js.name[i];
    if( !jmg->hasJointModel(name) ) continue;

    traj.joint_names.push_back(js.name[i]);
    pt.positions.push_back(js.position[i]);
    if(js.velocity.size()) pt.velocities.push_back(js.velocity[i]);
  }
  pt.time_from_start = ros::Duration(req.motion_plan_request.allowed_planning_time*1.0);
  traj.points.push_back(pt);

  traj.header.stamp = ros::Time::now();
  traj.header.frame_id = "odom_combined";

  res.trajectory.joint_trajectory = traj;

  return true;
}
  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_)
    {    
      if (!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);
      added_path_index.push_back(0);
    }

    return solved;
  }