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 <boost/math/constants/constants.hpp>
00039 #include <moveit/trajectory_processing/trajectory_tools.h>
00040 #include <moveit/robot_state/conversions.h>
00041 #include <class_loader/class_loader.h>
00042 #include <ros/ros.h>
00043
00044 namespace default_planner_request_adapters
00045 {
00046
00047 class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdapter
00048 {
00049 public:
00050
00051 static const std::string BOUNDS_PARAM_NAME;
00052 static const std::string DT_PARAM_NAME;
00053
00054 FixStartStateBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
00055 {
00056 if (!nh_.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
00057 {
00058 bounds_dist_ = 0.05;
00059 ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was not set. Using default value: " << bounds_dist_);
00060 }
00061 else
00062 ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was set to " << bounds_dist_);
00063
00064 if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
00065 {
00066 max_dt_offset_ = 0.5;
00067 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
00068 }
00069 else
00070 ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
00071 }
00072
00073 virtual std::string getDescription() const { return "Fix Start State Bounds"; }
00074
00075
00076 virtual bool adaptAndPlan(const PlannerFn &planner,
00077 const planning_scene::PlanningSceneConstPtr& planning_scene,
00078 const planning_interface::MotionPlanRequest &req,
00079 planning_interface::MotionPlanResponse &res,
00080 std::vector<std::size_t> &added_path_index) const
00081 {
00082 ROS_DEBUG("Running '%s'", getDescription().c_str());
00083
00084
00085 robot_state::RobotState start_state = planning_scene->getCurrentState();
00086 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
00087
00088 const std::vector<const robot_model::JointModel*> &jmodels =
00089 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
00090 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
00091 planning_scene->getRobotModel()->getJointModels();
00092
00093 bool change_req = false;
00094 for (std::size_t i = 0 ; i < jmodels.size() ; ++i)
00095 {
00096
00097
00098
00099
00100
00101
00102 const robot_model::JointModel* jm = jmodels[i];
00103 if (jm->getType() == robot_model::JointModel::REVOLUTE)
00104 {
00105 if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
00106 {
00107 double initial = start_state.getJointPositions(jm)[0];
00108 start_state.enforceBounds(jm);
00109 double after = start_state.getJointPositions(jm)[0];
00110 if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
00111 change_req = true;
00112 }
00113 }
00114 else
00115
00116 if (jm->getType() == robot_model::JointModel::PLANAR)
00117 {
00118 const double *p = start_state.getJointPositions(jm);
00119 double copy[3] = {p[0], p[1], p[2]};
00120 if (static_cast<const robot_model::PlanarJointModel*>(jm)->normalizeRotation(copy))
00121 {
00122 start_state.setJointPositions(jm, copy);
00123 change_req = true;
00124 }
00125 }
00126 else
00127
00128 if (jm->getType() == robot_model::JointModel::FLOATING)
00129 {
00130 const double *p = start_state.getJointPositions(jm);
00131 double copy[7] = {p[0], p[1], p[2], p[3], p[4], p[5], p[6]};
00132 if (static_cast<const robot_model::FloatingJointModel*>(jm)->normalizeRotation(copy))
00133 {
00134 start_state.setJointPositions(jm, copy);
00135 change_req = true;
00136 }
00137 }
00138 }
00139
00140
00141 robot_state::RobotStatePtr prefix_state;
00142 for (std::size_t i = 0 ; i < jmodels.size() ; ++i)
00143 {
00144 if (!start_state.satisfiesBounds(jmodels[i]))
00145 {
00146 if (start_state.satisfiesBounds(jmodels[i], bounds_dist_))
00147 {
00148 if (!prefix_state)
00149 prefix_state.reset(new robot_state::RobotState(start_state));
00150 start_state.enforceBounds(jmodels[i]);
00151 change_req = true;
00152 ROS_INFO("Starting state is just outside bounds (joint '%s'). Assuming within bounds.", jmodels[i]->getName().c_str());
00153 }
00154 else
00155 {
00156 std::stringstream joint_values;
00157 std::stringstream joint_bounds_low;
00158 std::stringstream joint_bounds_hi;
00159 const double *p = start_state.getJointPositions(jmodels[i]);
00160 for (std::size_t k = 0 ; k < jmodels[i]->getVariableCount() ; ++k)
00161 joint_values << p[k] << " ";
00162 const robot_model::JointModel::Bounds &b = jmodels[i]->getVariableBounds();
00163 for (std::size_t k = 0 ; k < b.size() ; ++k)
00164 {
00165 joint_bounds_low << b[k].min_position_ << " ";
00166 joint_bounds_hi << b[k].max_position_ << " ";
00167 }
00168 ROS_WARN_STREAM("Joint '" << jmodels[i]->getName() << "' from the starting state is outside bounds by a significant margin: [ " << joint_values.str() << "] should be in the range [ " << joint_bounds_low.str() <<
00169 "], [ " << joint_bounds_hi.str() << "] but the error above the ~" << BOUNDS_PARAM_NAME << " parameter (currently set to " << bounds_dist_ << ")");
00170 }
00171 }
00172 }
00173
00174 bool solved;
00175
00176 if (change_req)
00177 {
00178 planning_interface::MotionPlanRequest req2 = req;
00179 robot_state::robotStateToRobotStateMsg(start_state, req2.start_state, false);
00180 solved = planner(planning_scene, req2, res);
00181 }
00182 else
00183 solved = planner(planning_scene, req, res);
00184
00185
00186 if (prefix_state && res.trajectory_ && !res.trajectory_->empty())
00187 {
00188
00189 res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
00190 res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
00191
00192 for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
00193 added_path_index[i]++;
00194 added_path_index.push_back(0);
00195 }
00196
00197 return solved;
00198 }
00199
00200 private:
00201
00202 ros::NodeHandle nh_;
00203 double bounds_dist_;
00204 double max_dt_offset_;
00205 };
00206
00207
00208 const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error";
00209 const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt";
00210
00211 }
00212
00213 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds,
00214 planning_request_adapter::PlanningRequestAdapter);