Klamp't  0.8.1
DynamicPath.h
1 /*****************************************************************************
2  *
3  * Copyright (c) 2010-2011, the Trustees of Indiana University
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Indiana University nor the
14  * names of its contributors may be used to endorse or promote products
15  * derived from this software without specific prior written permission.
16 
17  * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
27  * THE POSSIBILITY OF SUCH DAMAGE.
28  *
29  ***************************************************************************/
30 
31 #ifndef DYNAMIC_PATH_H
32 #define DYNAMIC_PATH_H
33 
34 #include "ParabolicRamp.h"
35 #include <KrisLibrary/math/random.h>
36 
37 namespace ParabolicRamp {
38 
42 {
43  public:
44  virtual ~FeasibilityCheckerBase() {}
45  virtual bool ConfigFeasible(const Vector& x)=0;
46  virtual bool SegmentFeasible(const Vector& a,const Vector& b)=0;
47 };
48 
56 {
57  public:
58  virtual ~DistanceCheckerBase() {}
59  virtual Real ObstacleDistanceNorm() const { return Math::Inf; }
60  virtual Real ObstacleDistance(const Vector& x)=0;
61 };
62 
64 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters);
65 
68 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol);
69 
70 
83 {
84  public:
87  bool Check(const ParabolicRampND& x);
88 
90  Real tol;
91  DistanceCheckerBase* distance;
92  int maxiters;
93 };
94 
99 {
100  public:
101  virtual Real Rand() { return Math::Rand(); }
102 };
103 
104 
115 {
116  public:
117  DynamicPath();
118  void Init(const Vector& velMax,const Vector& accMax);
119  void SetJointLimits(const Vector& qMin,const Vector& qMax);
120  inline void Clear() { ramps.clear(); }
121  inline bool Empty() const { return ramps.empty(); }
122  inline const Vector& StartConfig() const { return ramps.front().x0; }
123  inline const Vector& EndConfig() const { return ramps.back().x1; }
124  inline const Vector& StartVelocity() const { return ramps.front().dx0; }
125  inline const Vector& EndVelocity() const { return ramps.back().dx1; }
126  Real GetTotalTime() const;
127  int GetSegment(Real t,Real& u) const;
128  void Evaluate(Real t,Vector& x) const;
129  void Derivative(Real t,Vector& dx) const;
130  void Accel(Real t,Vector& ddx) const;
131  bool SolveMinTime(const Vector& x0,const Vector& dx0,const Vector& x1,const Vector& dx1);
132  bool SolveMinAccel(const Vector& x0,const Vector& dx0,const Vector& x1,const Vector& dx1,Real endTime);
133  bool SetMilestones(const std::vector<Vector>& x);
134  bool SetMilestones(const std::vector<Vector>& x,const std::vector<Vector>& dx);
135  void GetMilestones(std::vector<Vector>& x,std::vector<Vector>& dx) const;
136  void Append(const Vector& x);
137  void Append(const Vector& x,const Vector& dx);
138  void Concat(const DynamicPath& suffix);
139  void Split(Real t,DynamicPath& before,DynamicPath& after) const;
140  bool TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check);
141  int Shortcut(int numIters,RampFeasibilityChecker& check);
142  int Shortcut(int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
143  int ShortCircuit(RampFeasibilityChecker& check);
146  int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check);
147  int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
148 
149  //Shortcutting technique with wrapping ranges, useful for angles. Set
150  //modulus = Inf to indicate that the range should not be wrapped.
151  //Note: does not allow intermediate segments to wrap twice, even if that
152  //would be more efficient
153  int WrappedShortcut(const std::vector<Real>& modulus,int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
154 
155  bool IsValid() const;
156 
158  Vector xMin,xMax,velMax,accMax;
160  std::vector<ParabolicRampND> ramps;
161 };
162 
163 } //namespace ParabolicRamp
164 
165 #endif
Solves for optimal trajectores for a velocity-bounded ND system.
Definition: ParabolicRamp.h:110
Definition: Config.h:15
std::vector< ParabolicRampND > ramps
The path is stored as a series of ramps.
Definition: DynamicPath.h:160
A custom random number generator that can be provided to DynamicPath::Shortcut()
Definition: DynamicPath.h:98
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
A base class for a distance checker. ObstacleDistance returns the radius of a L-z norm guaranteed to ...
Definition: DynamicPath.h:55
Vector xMin
The joint limits (optional), velocity bounds, and acceleration bounds.
Definition: DynamicPath.h:158
A base class for a feasibility checker.
Definition: DynamicPath.h:41
Functions for optimal acceleration-bounded trajectories.
A class that encapsulates feaibility checking of a ParabolicRampND.
Definition: DynamicPath.h:82