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
00046 class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter
00047 {
00048 public:
00049
00050 static const std::string DT_PARAM_NAME;
00051 static const std::string JIGGLE_PARAM_NAME;
00052 static const std::string ATTEMPTS_PARAM_NAME;
00053
00054 FixStartStateCollision() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
00055 {
00056 if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
00057 {
00058 max_dt_offset_ = 0.5;
00059 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
00060 }
00061 else
00062 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
00063
00064 if (!nh_.getParam(JIGGLE_PARAM_NAME, jiggle_fraction_))
00065 {
00066 jiggle_fraction_ = 0.02;
00067 ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was not set. Using default value: " << jiggle_fraction_);
00068 }
00069 else
00070 ROS_INFO_STREAM("Param '" << JIGGLE_PARAM_NAME << "' was set to " << jiggle_fraction_);
00071
00072 if (!nh_.getParam(ATTEMPTS_PARAM_NAME, sampling_attempts_))
00073 {
00074 sampling_attempts_ = 100;
00075 ROS_INFO_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' was not set. Using default value: " << sampling_attempts_);
00076 }
00077 else
00078 {
00079 if (sampling_attempts_ < 1)
00080 {
00081 sampling_attempts_ = 1;
00082 ROS_WARN_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' needs to be at least 1.");
00083 }
00084 ROS_INFO_STREAM("Param '" << ATTEMPTS_PARAM_NAME << "' was set to " << sampling_attempts_);
00085 }
00086
00087 }
00088
00089 virtual std::string getDescription() const { return "Fix Start State In Collision"; }
00090
00091 virtual bool adaptAndPlan(const PlannerFn &planner,
00092 const planning_scene::PlanningSceneConstPtr& planning_scene,
00093 const planning_interface::MotionPlanRequest &req,
00094 planning_interface::MotionPlanResponse &res,
00095 std::vector<std::size_t> &added_path_index) const
00096 {
00097 ROS_DEBUG("Running '%s'", getDescription().c_str());
00098
00099
00100 robot_state::RobotState start_state = planning_scene->getCurrentState();
00101 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
00102
00103 collision_detection::CollisionRequest creq;
00104 creq.group_name = req.group_name;
00105 collision_detection::CollisionResult cres;
00106 planning_scene->checkCollision(creq, cres, start_state);
00107 if (cres.collision)
00108 {
00109
00110 collision_detection::CollisionRequest vcreq = creq;
00111 collision_detection::CollisionResult vcres;
00112 vcreq.verbose = true;
00113 planning_scene->checkCollision(vcreq, vcres, start_state);
00114
00115 if (creq.group_name.empty())
00116 ROS_INFO("Start state appears to be in collision");
00117 else
00118 ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);
00119
00120 robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
00121 random_numbers::RandomNumberGenerator &rng = prefix_state->getRandomNumberGenerator();
00122
00123 const std::vector<const robot_model::JointModel*> &jmodels =
00124 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
00125 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
00126 planning_scene->getRobotModel()->getJointModels();
00127
00128 bool found = false;
00129 for (int c = 0 ; !found && c < sampling_attempts_ ; ++c)
00130 {
00131 for (std::size_t i = 0 ; !found && i < jmodels.size() ; ++i)
00132 {
00133 std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
00134 const double *original_values = prefix_state->getJointPositions(jmodels[i]);
00135 jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, 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", prefix_state->distance(start_state), c);
00143 }
00144 }
00145 }
00146
00147 if (found)
00148 {
00149 planning_interface::MotionPlanRequest req2 = req;
00150 robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
00151 bool solved = planner(planning_scene, req2, res);
00152 if (solved && !res.trajectory_->empty())
00153 {
00154
00155 res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
00156 res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
00157
00158 for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
00159 added_path_index[i]++;
00160 added_path_index.push_back(0);
00161 }
00162 return solved;
00163 }
00164 else
00165 {
00166 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.",
00167 jiggle_fraction_, sampling_attempts_);
00168 return planner(planning_scene, req, res);
00169 }
00170 }
00171 else
00172 {
00173 if (creq.group_name.empty())
00174 ROS_DEBUG("Start state is valid");
00175 else
00176 ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
00177 return planner(planning_scene, req, res);
00178 }
00179 }
00180
00181 private:
00182
00183 ros::NodeHandle nh_;
00184 double max_dt_offset_;
00185 double jiggle_fraction_;
00186 int sampling_attempts_;
00187 };
00188
00189 const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt";
00190 const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction";
00191 const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts";
00192
00193 }
00194
00195 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision,
00196 planning_request_adapter::PlanningRequestAdapter);