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;
}
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;
}
// ******************************************************************************************
// 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;
}