fix_start_state_bounds.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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     // get the specified start state
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       // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
00096       // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around.
00097       // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
00098       // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
00099       // joints, and we un-do them when the plan comes from the planner
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           // Normalize yaw; no offset needs to be remembered
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           // Normalize quaternions
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     // pointer to a prefix state we could possibly add, if we detect we have to make changes
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     // if we made any changes, use them
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     // re-add the prefix state, if it was constructed
00189     if (prefix_state && res.trajectory_ && !res.trajectory_->empty())
00190     {
00191       // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to
00192       // the computed trajectory)
00193       res.trajectory_->setWayPointDurationFromPrevious(
00194           0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
00195       res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
00196       // we add a prefix point, so we need to bump any previously added index positions
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);


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16