Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/trajectory_processing/trajectory_tools.h>
00040 #include <class_loader/class_loader.h>
00041 #include <ros/ros.h>
00042
00043 namespace default_planner_request_adapters
00044 {
00045 class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter
00046 {
00047 public:
00048 static const std::string DT_PARAM_NAME;
00049 static const std::string JIGGLE_PARAM_NAME;
00050 static const std::string ATTEMPTS_PARAM_NAME;
00051
00052 FixStartStateCollision() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
00053 {
00054 if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
00055 {
00056 max_dt_offset_ = 0.5;
00057 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
00058 }
00059 else
00060 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
00061
00062 if (!nh_.getParam(JIGGLE_PARAM_NAME, jiggle_fraction_))
00063 {
00064 jiggle_fraction_ = 0.02;
00065 ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was not set. Using default value: " << jiggle_fraction_);
00066 }
00067 else
00068 ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was set to " << jiggle_fraction_);
00069
00070 if (!nh_.getParam(ATTEMPTS_PARAM_NAME, sampling_attempts_))
00071 {
00072 sampling_attempts_ = 100;
00073 ROS_INFO_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' was not set. Using default value: " << sampling_attempts_);
00074 }
00075 else
00076 {
00077 if (sampling_attempts_ < 1)
00078 {
00079 sampling_attempts_ = 1;
00080 ROS_WARN_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' needs to be at least 1.");
00081 }
00082 ROS_INFO_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' was set to " << sampling_attempts_);
00083 }
00084 }
00085
00086 virtual std::string getDescription() const
00087 {
00088 return "Fix Start State In Collision";
00089 }
00090
00091 virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00092 const planning_interface::MotionPlanRequest& req,
00093 planning_interface::MotionPlanResponse& res,
00094 std::vector<std::size_t>& added_path_index) const
00095 {
00096 ROS_DEBUG("Running '%s'", getDescription().c_str());
00097
00098
00099 robot_state::RobotState start_state = planning_scene->getCurrentState();
00100 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
00101
00102 collision_detection::CollisionRequest creq;
00103 creq.group_name = req.group_name;
00104 collision_detection::CollisionResult cres;
00105 planning_scene->checkCollision(creq, cres, start_state);
00106 if (cres.collision)
00107 {
00108
00109 collision_detection::CollisionRequest vcreq = creq;
00110 collision_detection::CollisionResult vcres;
00111 vcreq.verbose = true;
00112 planning_scene->checkCollision(vcreq, vcres, start_state);
00113
00114 if (creq.group_name.empty())
00115 ROS_INFO("Start state appears to be in collision");
00116 else
00117 ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);
00118
00119 robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
00120 random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator();
00121
00122 const std::vector<const robot_model::JointModel*>& jmodels =
00123 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
00124 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
00125 planning_scene->getRobotModel()->getJointModels();
00126
00127 bool found = false;
00128 for (int c = 0; !found && c < sampling_attempts_; ++c)
00129 {
00130 for (std::size_t i = 0; !found && i < jmodels.size(); ++i)
00131 {
00132 std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
00133 const double* original_values = prefix_state->getJointPositions(jmodels[i]);
00134 jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values,
00135 jmodels[i]->getMaximumExtent() * jiggle_fraction_);
00136 start_state.setJointPositions(jmodels[i], sampled_variable_values);
00137 collision_detection::CollisionResult cres;
00138 planning_scene->checkCollision(creq, cres, start_state);
00139 if (!cres.collision)
00140 {
00141 found = true;
00142 ROS_INFO("Found a valid state near the start state at distance %lf after %d attempts",
00143 prefix_state->distance(start_state), c);
00144 }
00145 }
00146 }
00147
00148 if (found)
00149 {
00150 planning_interface::MotionPlanRequest req2 = req;
00151 robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
00152 bool solved = planner(planning_scene, req2, res);
00153 if (solved && !res.trajectory_->empty())
00154 {
00155
00156
00157 res.trajectory_->setWayPointDurationFromPrevious(
00158 0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
00159 res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
00160
00161 for (std::size_t i = 0; i < added_path_index.size(); ++i)
00162 added_path_index[i]++;
00163 added_path_index.push_back(0);
00164 }
00165 return solved;
00166 }
00167 else
00168 {
00169 ROS_WARN("Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling "
00170 "attempts). Passing the original planning request to the planner.",
00171 jiggle_fraction_, sampling_attempts_);
00172 return planner(planning_scene, req, res);
00173 }
00174 }
00175 else
00176 {
00177 if (creq.group_name.empty())
00178 ROS_DEBUG("Start state is valid");
00179 else
00180 ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
00181 return planner(planning_scene, req, res);
00182 }
00183 }
00184
00185 private:
00186 ros::NodeHandle nh_;
00187 double max_dt_offset_;
00188 double jiggle_fraction_;
00189 int sampling_attempts_;
00190 };
00191
00192 const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt";
00193 const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction";
00194 const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts";
00195 }
00196
00197 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision,
00198 planning_request_adapter::PlanningRequestAdapter);