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()); }
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; }
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, ¶ms, 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; }
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; }
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, ¶ms_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; }
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; }