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