KrisLibrary  1.0.0
InterpolatorHelpers.h
1 #ifndef PLANNING_INTERPOLATOR_HELPERS_H
2 #define PLANNING_INTERPOLATOR_HELPERS_H
3 
4 #include "Interpolator.h"
5 #include "GeodesicSpace.h"
6 #include <memory>
8 #include <KrisLibrary/spline/Hermite.h>
9 #include <KrisLibrary/spline/PiecewisePolynomial.h>
10 
11 
16 {
17 public:
18  LinearInterpolator(Real ax,Real bx,Real ta=0,Real tb=1);
19  LinearInterpolator(const Config& a,const Config& b,Real ta=0,Real tb=1);
20  virtual ~LinearInterpolator() {}
21  virtual void Eval(Real u,Config& x) const;
22  virtual Real Length() const { return a.distance(b); }
23  virtual const Config& Start() const { return a; }
24  virtual const Config& End() const { return b; }
25  virtual Real ParamStart() const { return ta; }
26  virtual Real ParamEnd() const { return tb; }
27 
28  Config a, b;
29  Real ta, tb;
30 };
31 
36 {
37 public:
38  GeodesicInterpolator(GeodesicSpace* space,const Config& a,const Config& b);
39  virtual ~GeodesicInterpolator() {}
40  virtual void Eval(Real u,Config& x) const { space->Interpolate(a,b,u,x); }
41  virtual Real Length() const { return space->Distance(a,b); }
42  virtual const Config& Start() const { return a; }
43  virtual const Config& End() const { return b; }
44 
45  GeodesicSpace* space;
46  Config a, b;
47 };
48 
53 {
54 public:
55  CSpaceInterpolator(CSpace* space,const Config& a,const Config& b);
56  virtual ~CSpaceInterpolator() {}
57  virtual void Eval(Real u,Config& x) const { space->Interpolate(a,b,u,x); }
58  virtual Real Length() const { return space->Distance(a,b); }
59  virtual const Config& Start() const { return a; }
60  virtual const Config& End() const { return b; }
61 
62  CSpace* space;
63  Config a, b;
64 };
65 
72 {
73 public:
74  TimeRemappedInterpolator(const InterpolatorPtr& base,Real a,Real b,Real pstart=0,Real pend=1);
75  virtual ~TimeRemappedInterpolator() {}
76  virtual void Eval(Real u,Config& x) const;
77  virtual Real Length() const;
78  virtual const Config& Start() const { return base->End(); }
79  virtual const Config& End() const { return base->Start(); }
80  virtual Real ParamStart() const { return pstart; }
81  virtual Real ParamEnd() const { return pend; }
82 
83  InterpolatorPtr base;
84  Real a,b;
85  Real pstart,pend;
86 };
87 
93 {
94 public:
96  PathInterpolator(const InterpolatorPtr& interp);
97  PathInterpolator(const std::vector<InterpolatorPtr > & segments,Real totalTime=1.0);
98  virtual ~PathInterpolator() {}
100  void Append(const InterpolatorPtr& interp,Real duration=0);
101  void Concat(const PathInterpolator& path);
102  void ScaleDuration(Real scale);
103  void SetTotalTime(Real ttotal);
104  virtual void Eval(Real u,Config& x) const;
105  virtual Real Length() const;
106  virtual const Config& Start() const { return segments.front()->Start(); }
107  virtual const Config& End() const { return segments.back()->End(); }
108  virtual Real ParamStart() const { return times.empty() ? 0 : times.front(); }
109  virtual Real ParamEnd() const { return times.empty() ? 1 : times.back(); }
110 
111  std::vector<InterpolatorPtr > segments;
112  std::vector<Real> durations;
113  std::vector<Real> times;
114 };
115 
117 {
118 public:
119  PiecewiseLinearInterpolator(const std::vector<Config>& configs);
120  PiecewiseLinearInterpolator(const std::vector<Config>& configs,const std::vector<Real>& times);
121  virtual ~PiecewiseLinearInterpolator() {}
122  virtual void Eval(Real u,Config& x) const;
123  virtual Real Length() const;
124  virtual const Config& Start() const { return configs.front(); }
125  virtual const Config& End() const { return configs.back(); }
126  virtual Real ParamStart() const { return times.empty() ? 0 : times.front(); }
127  virtual Real ParamEnd() const { return times.empty() ? 1 : times.back(); }
128 
129  std::vector<Config> configs;
130  std::vector<Real> times;
131 };
132 
134 {
135 public:
136  PiecewiseLinearCSpaceInterpolator(CSpace* space,const std::vector<Config>& configs);
137  PiecewiseLinearCSpaceInterpolator(CSpace* space,const std::vector<Config>& configs,const std::vector<Real>& times);
139  virtual void Eval(Real u,Config& x) const;
140  virtual Real Length() const;
141 
142  CSpace* space;
143 };
144 
146 {
147 public:
149  virtual ~PiecewisePolynomialInterpolator() {}
150  virtual void Eval(Real u,Config& x) const;
151  virtual Real Length() const;
152  virtual const Config& Start() const { return start; }
153  virtual const Config& End() const { return end; }
154  virtual Real ParamStart() const { return path.StartTime(); }
155  virtual Real ParamEnd() const { return path.EndTime(); }
156 
158  Vector start,end;
159 };
160 
161 
166 {
167 public:
168  MultiInterpolator(const InterpolatorPtr& component1,const InterpolatorPtr& component2);
169  MultiInterpolator(const std::vector<InterpolatorPtr > & components);
170  virtual ~MultiInterpolator() {}
171  void Split(const Vector& x,std::vector<Vector>& items) const;
172  void Join(const std::vector<Vector>& items,Vector& x) const;
173  virtual void Eval(Real u,Config& x) const;
174  virtual Real Length() const;
175  virtual const Config& Start() const { return a; }
176  virtual const Config& End() const { return b; }
177  virtual Real ParamStart() const { return components[0]->ParamStart(); }
178  virtual Real ParamEnd() const { return components[0]->ParamEnd(); }
179 
180  std::vector<InterpolatorPtr > components;
181  Config a,b;
182 };
183 
185 {
186 public:
187  SubsetInterpolator(const InterpolatorPtr& base,int start,int end);
188  virtual ~SubsetInterpolator() {}
189  virtual void Eval(Real u,Config& x) const;
190  virtual Real Length() const { return base->Length(); }
191  virtual const Config& Start() const { return a; }
192  virtual const Config& End() const { return b; }
193  virtual Real ParamStart() const { return base->ParamStart(); }
194  virtual Real ParamEnd() const { return base->ParamEnd(); }
195 
196  InterpolatorPtr base;
197  int start,end;
198  Config a,b;
199 };
200 
201 #endif
A space with geodesics and (optionally) geodesic derivatives.
Definition: GeodesicSpace.h:31
An interpolator that uses a GeodesicSpace&#39;s Interpolate function.
Definition: InterpolatorHelpers.h:35
An interpolator that uses a CSpace&#39;s Interpolate function.
Definition: InterpolatorHelpers.h:52
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Motion planning configuration space base class. The configuration space implements an interpolation s...
Definition: CSpace.h:34
An interpolator that goes between two Configs.
Definition: InterpolatorHelpers.h:15
A cartesian product of interpolators.
Definition: InterpolatorHelpers.h:165
An interpolator that remaps a time interval of another interpolator. Maps the parameter range [pstart...
Definition: InterpolatorHelpers.h:71
Definition: InterpolatorHelpers.h:184
Definition: InterpolatorHelpers.h:116
Definition: InterpolatorHelpers.h:145
virtual Real Distance(const Config &x, const Config &y)
optionally overrideable (default uses euclidean space)
Definition: CSpace.h:56
A base class for all 1D interpolators.
Definition: Interpolator.h:10
An interpolator that concatenates a list of other interpolators. The interpolators may have nonunifor...
Definition: InterpolatorHelpers.h:92
Definition: InterpolatorHelpers.h:133
Definition: PiecewisePolynomial.h:105
A vector over the field T.
Definition: function.h:9
Convenience functions for linear interpolation of vectors and matrices.