Klamp't  0.9.0
Paths.h
1 #ifndef KLAMPT_MODELING_PATH_H
2 #define KLAMPT_MODELING_PATH_H
3 
4 #include "Robot.h"
5 
6 //forward declarations
7 namespace ParabolicRamp { class DynamicPath; }
8 namespace Spline { class PiecewisePolynomialND; }
9 
10 namespace Klampt {
11  using namespace std;
12 
13 //forward declarations
14 class MultiPath;
15 
18 
21 {
22  public:
23  LinearPath();
24  LinearPath(const vector<Real>& times,const vector<Vector>& milestones);
25  bool Save(ostream& out);
26  bool Load(istream& in);
27  Real StartTime() const { return times.front(); }
28  Real EndTime() const { return times.back(); }
29  Real Duration() const { return EndTime()-StartTime(); }
31  void Append(Real t,const Config& x) { times.push_back(t); milestones.push_back(x); }
33  void Clear() { times.resize(0); milestones.resize(0); }
36  void Concat(const LinearPath& path,bool relative=true);
38  void Eval(Real t,Vector& xt) const;
40  void Deriv(Real t,Vector& dxt) const;
42  void Eval(RobotModel& robot,Real t,Vector& xt) const;
44  void Deriv(RobotModel& robot,Real t,Vector& dxt) const;
45 
46  vector<Real> times;
47  vector<Vector> milestones;
48 };
49 
51 void Convert(const LinearPath& in,MultiPath& out);
54 void Convert(const LinearPath& in,ParabolicRamp::DynamicPath& out);
57 void Convert(const LinearPath& in,Spline::PiecewisePolynomialND& out);
60 void Convert(const MultiPath& in,LinearPath& out);
64 void Convert(const MultiPath& in,ParabolicRamp::DynamicPath& out);
68 void Convert(const MultiPath& in,Spline::PiecewisePolynomialND& out);
70 void Convert(const ParabolicRamp::DynamicPath& in,MultiPath& out);
72 void Convert(const ParabolicRamp::DynamicPath& in,Spline::PiecewisePolynomialND& out);
75 //Not implemented yet
76 //void Convert(const Spline::PiecewisePolynomialND& in,ParabolicRamp::DynamicPath& out);
79 void Convert(const Spline::PiecewisePolynomialND& in,MultiPath& out);
80 
82 void Keyframes(const LinearPath& in,vector<Config>& milestones);
83 void Keyframes(const LinearPath& in,vector<Real>& times,vector<Config>& milestones);
85 void Keyframes(const LinearPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
86 void Keyframes(RobotModel& robot,const LinearPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
87 void Keyframes(const MultiPath& in,vector<Config>& milestones);
88 void Keyframes(const MultiPath& in,vector<Real>& times,vector<Config>& milestones);
89 void Keyframes(const MultiPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
90 void Keyframes(RobotModel& robot,const MultiPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
91 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Config>& milestones);
92 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Real>& times,vector<Config>& milestones);
93 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
94 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Config>& milestones);
95 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Real>& times,vector<Config>& milestones);
96 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
97 
99 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,LinearPath& out);
101 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,MultiPath& out);
104 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,Spline::PiecewisePolynomialND& out);
107 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,ParabolicRamp::DynamicPath& out);
109 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,MultiPath& out);
111 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,Spline::PiecewisePolynomialND& out);
113 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,ParabolicRamp::DynamicPath& out);
115 template <class T>
116 inline void Interpolate(const LinearPath& in,T& out) { Interpolate(in.times,in.milestones,out); }
122 void Interpolate(const MultiPath& in,Spline::PiecewisePolynomialND& out);
125 
128 template <class T>
129 void Smooth(const LinearPath& in,T& out)
130 {
131  vector<Real> times;
132  vector<Vector> milestones,dmilestones;
133  Keyframes(in,times,milestones,dmilestones);
134  Interpolate(times,milestones,dmilestones,out);
135 }
136 template <class T>
137 void Smooth(const MultiPath& in,ParabolicRamp::DynamicPath& out)
138 {
139  vector<Real> times;
140  vector<Vector> milestones,dmilestones;
141  Keyframes(in,times,milestones,dmilestones);
142  Interpolate(times,milestones,dmilestones,out);
143 }
144 template <class T>
145 void Smooth(RobotModel& robot,const LinearPath& in,T& out)
146 {
147  vector<Real> times;
148  vector<Vector> milestones,dmilestones;
149  Keyframes(robot,in,times,milestones,dmilestones);
150  Interpolate(times,milestones,dmilestones,out);
151 }
152 template <class T>
153 void Smooth(RobotModel& robot,const MultiPath& in,ParabolicRamp::DynamicPath& out)
154 {
155  vector<Real> times;
156  vector<Vector> milestones,dmilestones;
157  Keyframes(robot,in,times,milestones,dmilestones);
158  Interpolate(times,milestones,dmilestones,out);
159 }
160 
162 void Discretize(const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
163 void Discretize(const MultiPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
164 void Discretize(const ParabolicRamp::DynamicPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
165 void Discretize(const Spline::PiecewisePolynomialND& in,Real res,vector<Real>& times,vector<Config>& milestones);
167 void Discretize(const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
168 void Discretize(const MultiPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
169 void Discretize(const ParabolicRamp::DynamicPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
170 void Discretize(const Spline::PiecewisePolynomialND& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
173 void Discretize(RobotModel& robot,const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
174 void Discretize(RobotModel& robot,const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
175 
176 
179 template <class T1,class T2>
180 void Approximate(const T1& in,Real res,T2& out)
181 {
182  vector<Real> times;
183  vector<Vector> milestones,dmilestones;
184  Discretize(in,res,times,milestones,dmilestones);
185  Interpolate(times,milestones,dmilestones,out);
186 }
189 template <class T1>
190 void Approximate(const T1& in,Real res,LinearPath& out)
191 {
192  vector<Real> times;
193  vector<Vector> milestones;
194  Discretize(in,res,times,milestones);
195  Interpolate(times,milestones,out);
196 }
197 
200 } //namespace Klampt
201 
202 #endif
void Smooth(const LinearPath &in, T &out)
Definition: Paths.h:129
void Clear()
clears the path
Definition: Paths.h:33
A very general multi-section path container.
Definition: MultiPath.h:31
void Convert(const Spline::PiecewisePolynomialND &in, MultiPath &out)
Definition: Config.h:15
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
void Discretize(const LinearPath &in, Real res, vector< Real > &times, vector< Config > &milestones)
Split up the path into keyframes at a given resolution.
Definition: Paths.h:8
void Interpolate(const ParabolicRamp::DynamicPath &in, MultiPath &out)
Create a representation that matches the keyframes of the input path.
The main robot type used in RobotSim.
Definition: Robot.h:83
void Keyframes(const LinearPath &in, vector< Config > &milestones)
Extract keyframes from the path representation.
void Approximate(const T1 &in, Real res, LinearPath &out)
Definition: Paths.h:190
void Append(Real t, const Config &x)
Appends a new milestone at the given time.
Definition: Paths.h:31
A piecewise linear path.
Definition: Paths.h:20
Definition: ContactDistance.h:6