Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeSpacePlannerXYTheta.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef _SHAPE_SPACE_PLANNER_XYTHETA_H_
00003 #define _SHAPE_SPACE_PLANNER_XYTHETA_H_
00004 
00005 #include "Motion/Kinematics.h"
00006 
00007 #include "Planners/RRT/ShapeSpacePlannerBase.h"
00008 
00009 class ShapeSpacePlannerXYTheta;
00010 
00011 //================ RRTNodeXYTheta ================
00012 
00013 class RRTNodeXYTheta : public RRTNodeBase {
00014 public:
00015   
00016   struct NodeValue_t {
00017     float x;            //!< The x-coordinate of this position
00018     float y;            //!< The y-coordinate of this position
00019     AngTwoPi theta;     //!< Heading that will take us from parent nodes's position to this position (x,y) 
00020     AngSignTwoPi turn;  //!< Left or right turn that takes us to this node's desired heading theta
00021     NodeValue_t() : x(0), y(0), theta(0), turn(0) {}
00022     NodeValue_t(float _x, float _y, AngTwoPi _theta, AngSignTwoPi _turn=0) :
00023       x(_x), y(_y), theta(_theta), turn(_turn) {}
00024   };
00025 
00026   NodeValue_t q;
00027   
00028   //! Constructor
00029   RRTNodeXYTheta(const NodeValue_t &_q, unsigned int _parent) : RRTNodeBase(_parent), q(_q) {};
00030   
00031   class CollisionChecker : public ShapeSpaceCollisionCheckerBase<2> {
00032   public:
00033 
00034     //! Constructor
00035     CollisionChecker(DualCoding::ShapeSpace & shs,
00036                      const DualCoding::Shape<DualCoding::PolygonData> &_worldBounds,
00037                      float _inflation) :
00038       ShapeSpaceCollisionCheckerBase<2>(shs, _worldBounds, _inflation), body() {
00039       for (unsigned int i = 0; i < obstacles.size(); i++) {
00040         if (obstacles[i]->isBodyObstacle())
00041           body.add(dynamic_cast<PlannerObstacle<2>*>(obstacles[i]->clone()));
00042       }
00043     }
00044       
00045     HierarchicalObstacle body;
00046     
00047     virtual bool collides(const NodeValue_t &qnew, GenericRRTBase::PlannerResult2D* result=NULL);
00048 
00049     std::vector<PlannerObstacle2D*> colliders(const NodeValue_t &q);
00050   };
00051   
00052   static const unsigned int maxInterpolations = 10; //!< Maximum number of interpolation steps in interpolate() when @a truncate is true
00053 
00054   static const AngSignPi turnLimit; //!< Maximum theta difference between adjacent nodes without interpolated collision checking
00055   
00056   virtual float distance(const NodeValue_t &target);
00057 
00058   static void generateSample(const NodeValue_t &lower, const NodeValue_t &upper, NodeValue_t &sample);
00059 
00060   static Interp_t interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interpStep,
00061                               bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool searchingBackwards);
00062 
00063   static bool safeTurn(const NodeValue_t &start, const AngSignPi headingChange, AngSignTwoPi &turn, CollisionChecker *cc);
00064 
00065   //! Interpolation function for potential short-circuiting of segments of a path
00066   static Interp_t snipInterpolate(const NodeValue_t &start, const NodeValue_t &target, const AngTwoPi nextTheta,
00067                                   const NodeValue_t &interpStep, CollisionChecker *cc,
00068                                   NodeValue_t &reached, AngSignTwoPi &nextTurn);
00069   
00070   virtual std::string toString() const;
00071 };
00072 
00073 ostream& operator<<(ostream &os, const RRTNodeXYTheta::NodeValue_t q);
00074 
00075 
00076 //================ ShapeSpacePlannerXYTheta ================
00077 
00078 //! Plans a path in a 2D linear space with smooth angle changes (if turnLimit is small), using multiple bounding boxes for collision checking
00079 class ShapeSpacePlannerXYTheta : public GenericRRT<RRTNodeXYTheta, 2> {
00080 public:
00081   typedef RRTNodeXYTheta NodeType_t;
00082   typedef NodeType_t::NodeValue_t NodeValue_t;
00083   
00084   ShapeSpacePlannerXYTheta(DualCoding::ShapeSpace &shs,
00085                            const DualCoding::Shape<DualCoding::PolygonData> &worldBounds = Shape<PolygonData>(),
00086                            float inflation = 0);
00087   
00088   virtual ~ShapeSpacePlannerXYTheta() {}
00089   
00090   virtual void initialize(const NodeValue_t &start, std::vector<NodeType_t> &treeStartResult,
00091                           const NodeValue_t &end, std::vector<NodeType_t> &treeEndResult);
00092 
00093   //! Calculate the heading necessary to point the baseOffset at the target.
00094   /*! This is the slope of the tangent line from the target to a
00095     circle centered on the baseFrame with radius equal to the y
00096     component of the baseOffset.
00097   */
00098   AngTwoPi tangentHeading(const NodeValue_t &start, const NodeValue_t &end) const;
00099 
00100   virtual void buildPath(const std::vector<NodeType_t> *treeStart,
00101                          const std::vector<NodeType_t> *treeEnd, 
00102                          std::vector<NodeValue_t> &path);
00103   
00104   static unsigned int const numDivisions = 18; //!< Number of divisions of the circle to try when targetHeading unspecified
00105 
00106   using GenericRRT<NodeType_t, 2>::planPath;
00107 
00108   //! Plan a robot path from @a startPoint to @a endPoint with optional @a targetHeading at the end
00109   /*!
00110     @param startPoint Starting location of the robot
00111     @param baseOffset Offset from the base frame, e.g., if we want a path that brings the @e gripper (rather than the base) to a particular location
00112     @param endPoint Desired ending point
00113     @param initialHeading The robot's heading at the start of the path
00114     @param targetHeading Desired final heading; if unspecified, the planner generates a collection of possible target headings to try for
00115     @param maxIterations Maximum number of iterations for the RRT to search before giving up
00116     @param pathResult Stores the final, smoothed path computed by the planner
00117     @param treeStartResult Stores the start search tree; only used for debugging or teaching
00118     @param treeEndResult Stores the end search tree; only used for debugging or teaching
00119   */
00120   GenericRRTBase::PlannerResult2D
00121   planPath(const Point &startPoint,
00122      const fmat::Column<3> &_baseOffset,
00123            float gateLength,
00124            const Point &endPoint,
00125            const AngTwoPi initialHeading,
00126            const AngTwoPi _targetHeading,
00127            unsigned int _maxIterations=4000,
00128            std::vector<NodeValue_t> *pathResult=NULL,
00129            std::vector<NodeType_t> *treeStartResult=NULL,
00130            std::vector<NodeType_t> *treeEndResult=NULL);
00131   
00132   static void plotPath(const std::vector<NodeValue_t> &path,
00133                        Shape<GraphicsData> &graphics,
00134                        rgb color = rgb(0,0,255));
00135 
00136   static void plotTree(const std::vector<NodeType_t> &tree,
00137                        Shape<GraphicsData> &graphics,
00138                        rgb color = rgb(0,0,255));
00139 
00140 private:
00141   float targetHeading;  //!< Set by planPath() and used by initialize()
00142   fmat::Column<3> baseOffset;  //!< Set by planPath() and used by initialize()
00143   float gateLength; //!< Set by planPath() and used by initialize()
00144   
00145 };
00146 
00147 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:51 2016 by Doxygen 1.6.3