void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan &plan, const moveit_msgs::PlanningScene &scene_diff, const Options &opt) { if (planning_scene::PlanningScene::isEmpty(scene_diff)) planAndExecute(plan, opt); else { plan.planning_scene_monitor_ = planning_scene_monitor_; { planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_); // lock the scene so that it does not modify the world representation while diff() is called plan.planning_scene_ = lscene->diff(scene_diff); } planAndExecuteHelper(plan, opt); } }
void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal) { planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_); if (lscene->hasObjectType(goal.target_name)) { // const object_recognition_msgs::ObjectType &ot = lscene->getObjectType(goal->target_name); // need to call the grasp planner here } if (goal.possible_grasps.empty()) { // add a number of default grasp points // \todo add more! manipulation_msgs::Grasp g; g.grasp_pose.header.frame_id = goal.target_name; g.grasp_pose.pose.position.x = 0.0; g.grasp_pose.pose.position.y = 0.0; g.grasp_pose.pose.position.z = 0.0; g.grasp_pose.pose.orientation.x = 0.0; g.grasp_pose.pose.orientation.y = 0.0; g.grasp_pose.pose.orientation.z = 0.0; g.grasp_pose.pose.orientation.w = 1.0; g.approach.direction.vector.x = 1.0; g.approach.min_distance = 0.1; g.approach.desired_distance = 0.3; g.retreat.direction.vector.z = 1.0; g.retreat.direction.header.frame_id = lscene->getPlanningFrame(); g.retreat.min_distance = 0.1; g.retreat.desired_distance = 0.3; if (lscene->getRobotModel()->hasEndEffector(goal.end_effector)) { g.pre_grasp_posture.name = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames(); g.pre_grasp_posture.position.resize(g.pre_grasp_posture.name.size(), std::numeric_limits<double>::max()); g.grasp_posture.name = g.pre_grasp_posture.name; g.grasp_posture.position.resize(g.grasp_posture.name.size(), -std::numeric_limits<double>::max()); } goal.possible_grasps.push_back(g); } }
void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal) { planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); ROS_DEBUG_NAMED("manipulation", "Using default grasp poses"); goal.minimize_object_distance = true; // add a number of default grasp points // \todo add more! moveit_msgs::Grasp g; g.grasp_pose.header.frame_id = goal.target_name; g.grasp_pose.pose.position.x = -0.2; g.grasp_pose.pose.position.y = 0.0; g.grasp_pose.pose.position.z = 0.0; g.grasp_pose.pose.orientation.x = 0.0; g.grasp_pose.pose.orientation.y = 0.0; g.grasp_pose.pose.orientation.z = 0.0; g.grasp_pose.pose.orientation.w = 1.0; g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame(); g.pre_grasp_approach.direction.vector.x = 1.0; g.pre_grasp_approach.min_distance = 0.1; g.pre_grasp_approach.desired_distance = 0.2; g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame(); g.post_grasp_retreat.direction.vector.z = 1.0; g.post_grasp_retreat.min_distance = 0.1; g.post_grasp_retreat.desired_distance = 0.2; if (lscene->getRobotModel()->hasEndEffector(goal.end_effector)) { g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames(); g.pre_grasp_posture.points.resize(1); g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(), std::numeric_limits<double>::max()); g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names; g.grasp_posture.points.resize(1); g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max()); } goal.possible_grasps.push_back(g); }
bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan &plan, const std::pair<int, int> &path_segment) { if (path_segment.first >= 0 && path_segment.second >= 0 && plan.plan_components_[path_segment.first].trajectory_monitoring_) { planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // lock the scene so that it does not modify the world representation while isStateValid() is called const robot_trajectory::RobotTrajectory &t = *plan.plan_components_[path_segment.first].trajectory_; const collision_detection::AllowedCollisionMatrix *acm = plan.plan_components_[path_segment.first].allowed_collision_matrix_.get(); std::size_t wpc = t.getWayPointCount(); collision_detection::CollisionRequest req; req.group_name = t.getGroupName(); for (std::size_t i = std::max(path_segment.second - 1, 0) ; i < wpc ; ++i) { collision_detection::CollisionResult res; if (acm) plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); else plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i)); if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false)) { // Dave's debacle ROS_INFO("Trajectory component '%s' is invalid", plan.plan_components_[path_segment.first].description_.c_str()); // call the same functions again, in verbose mode, to show what issues have been detected plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true); req.verbose = true; res.clear(); if (acm) plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); else plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i)); return false; } } } return true; }
bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan, const ExecutableMotionPlanComputationFn& motion_planner, unsigned int max_look_attempts, double max_safe_path_cost) { if (max_safe_path_cost <= std::numeric_limits<double>::epsilon()) max_safe_path_cost = default_max_safe_path_cost_; if (max_look_attempts == 0) max_look_attempts = default_max_look_attempts_; double previous_cost = 0.0; unsigned int look_attempts = 0; // this flag is set to true when all conditions for looking around are met, and the command is sent. // the intention is for the planning looop not to terminate when having just looked around bool just_looked_around = false; // this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that // maybe some information was gained anyway (the sensor moved part of the way) and we try to plan one more time. // If we have two sensor pointing failures in a row, we fail bool look_around_failed = false; // there can be a maximum number of looking attempts as well that lead to replanning, if the cost // of the path is above a maximum threshold. do { bool solved = motion_planner(plan); if (!solved || plan.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) return solved; // determine the sources of cost for this path std::set<collision_detection::CostSource> cost_sources; { planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // it is ok if // planning_scene_monitor_ is // null; there just will be // no locking done for (std::size_t i = 0; i < plan.plan_components_.size(); ++i) { std::set<collision_detection::CostSource> cost_sources_i; plan.planning_scene_->getCostSources(*plan.plan_components_[i].trajectory_, max_cost_sources_, plan.plan_components_[i].trajectory_->getGroupName(), cost_sources_i, discard_overlapping_cost_sources_); cost_sources.insert(cost_sources_i.begin(), cost_sources_i.end()); if (cost_sources.size() > max_cost_sources_) { std::set<collision_detection::CostSource> other; other.swap(cost_sources); std::size_t j = 0; for (std::set<collision_detection::CostSource>::iterator it = other.begin(); j < max_cost_sources_; ++it, ++j) cost_sources.insert(*it); } } } // display the costs if needed if (display_cost_sources_) { visualization_msgs::MarkerArray arr; collision_detection::getCostMarkers(arr, plan.planning_scene_->getPlanningFrame(), cost_sources); cost_sources_publisher_.publish(arr); } double cost = collision_detection::getTotalCost(cost_sources); ROS_DEBUG("The total cost of the trajectory is %lf.", cost); if (previous_cost > 0.0) ROS_DEBUG("The change in the trajectory cost is %lf after the perception step.", cost - previous_cost); if (cost > max_safe_path_cost && look_attempts < max_look_attempts) { ++look_attempts; ROS_INFO("The cost of the trajectory is %lf, which is above the maximum safe cost of %lf. Attempt %u (of at most " "%u) at looking around.", cost, max_safe_path_cost, look_attempts, max_look_attempts); bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame()); if (looked_at_result) ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan."); else { if (look_around_failed) ROS_WARN("Looking around seems to keep failing. Giving up."); else ROS_WARN("Looking around seems to have failed. Attempting to recompute a motion plan anyway."); } if (looked_at_result || !look_around_failed) { previous_cost = cost; just_looked_around = true; } look_around_failed = !looked_at_result; // if we are unable to look, let this loop continue into the next if statement if (just_looked_around) continue; } if (cost > max_safe_path_cost) { plan.error_code_.val = moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA; return true; } else return true; } while (true); return false; }