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