Klamp't  0.8.1
Paths.h
1 #ifndef KLAMPT_MODELING_PATH_H
2 #define KLAMPT_MODELING_PATH_H
3 
4 #include "Robot.h"
5 class MultiPath;
6 namespace ParabolicRamp { class DynamicPath; }
7 namespace Spline { class PiecewisePolynomialND; }
8 
11 
14 {
15  public:
16  LinearPath();
17  LinearPath(const vector<Real>& times,const vector<Vector>& milestones);
18  bool Save(ostream& out);
19  bool Load(istream& in);
20  Real StartTime() const { return times.front(); }
21  Real EndTime() const { return times.back(); }
22  Real Duration() const { return EndTime()-StartTime(); }
24  void Append(Real t,const Config& x) { times.push_back(t); milestones.push_back(x); }
26  void Clear() { times.resize(0); milestones.resize(0); }
29  void Concat(const LinearPath& path,bool relative=true);
31  void Eval(Real t,Vector& xt) const;
33  void Deriv(Real t,Vector& dxt) const;
35  void Eval(Robot& robot,Real t,Vector& xt) const;
37  void Deriv(Robot& robot,Real t,Vector& dxt) const;
38 
39  vector<Real> times;
40  vector<Vector> milestones;
41 };
42 
44 void Convert(const LinearPath& in,MultiPath& out);
47 void Convert(const LinearPath& in,ParabolicRamp::DynamicPath& out);
50 void Convert(const LinearPath& in,Spline::PiecewisePolynomialND& out);
53 void Convert(const MultiPath& in,LinearPath& out);
57 void Convert(const MultiPath& in,ParabolicRamp::DynamicPath& out);
61 void Convert(const MultiPath& in,Spline::PiecewisePolynomialND& out);
63 void Convert(const ParabolicRamp::DynamicPath& in,MultiPath& out);
65 void Convert(const ParabolicRamp::DynamicPath& in,Spline::PiecewisePolynomialND& out);
68 //Not implemented yet
69 //void Convert(const Spline::PiecewisePolynomialND& in,ParabolicRamp::DynamicPath& out);
72 void Convert(const Spline::PiecewisePolynomialND& in,MultiPath& out);
73 
75 void Keyframes(const LinearPath& in,vector<Config>& milestones);
76 void Keyframes(const LinearPath& in,vector<Real>& times,vector<Config>& milestones);
78 void Keyframes(const LinearPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
79 void Keyframes(Robot& robot,const LinearPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
80 void Keyframes(const MultiPath& in,vector<Config>& milestones);
81 void Keyframes(const MultiPath& in,vector<Real>& times,vector<Config>& milestones);
82 void Keyframes(const MultiPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
83 void Keyframes(Robot& robot,const MultiPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
84 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Config>& milestones);
85 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Real>& times,vector<Config>& milestones);
86 void Keyframes(const ParabolicRamp::DynamicPath& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
87 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Config>& milestones);
88 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Real>& times,vector<Config>& milestones);
89 void Keyframes(const Spline::PiecewisePolynomialND& in,vector<Real>& times,vector<Config>& milestones,vector<Config>& dmilestones);
90 
92 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,LinearPath& out);
94 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,MultiPath& out);
97 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,Spline::PiecewisePolynomialND& out);
100 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,ParabolicRamp::DynamicPath& out);
102 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,MultiPath& out);
104 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,Spline::PiecewisePolynomialND& out);
106 void Interpolate(const vector<Real>& times,const vector<Config>& milestones,const vector<Vector>& dmilestones,ParabolicRamp::DynamicPath& out);
108 template <class T>
109 inline void Interpolate(const LinearPath& in,T& out) { Interpolate(in.times,in.milestones,out); }
115 void Interpolate(const MultiPath& in,Spline::PiecewisePolynomialND& out);
118 
121 template <class T>
122 void Smooth(const LinearPath& in,T& out)
123 {
124  vector<Real> times;
125  vector<Vector> milestones,dmilestones;
126  Keyframes(in,times,milestones,dmilestones);
127  Interpolate(times,milestones,dmilestones,out);
128 }
129 template <class T>
130 void Smooth(const MultiPath& in,ParabolicRamp::DynamicPath& out)
131 {
132  vector<Real> times;
133  vector<Vector> milestones,dmilestones;
134  Keyframes(in,times,milestones,dmilestones);
135  Interpolate(times,milestones,dmilestones,out);
136 }
137 template <class T>
138 void Smooth(Robot& robot,const LinearPath& in,T& out)
139 {
140  vector<Real> times;
141  vector<Vector> milestones,dmilestones;
142  Keyframes(robot,in,times,milestones,dmilestones);
143  Interpolate(times,milestones,dmilestones,out);
144 }
145 template <class T>
146 void Smooth(Robot& robot,const MultiPath& in,ParabolicRamp::DynamicPath& out)
147 {
148  vector<Real> times;
149  vector<Vector> milestones,dmilestones;
150  Keyframes(robot,in,times,milestones,dmilestones);
151  Interpolate(times,milestones,dmilestones,out);
152 }
153 
155 void Discretize(const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
156 void Discretize(const MultiPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
157 void Discretize(const ParabolicRamp::DynamicPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
158 void Discretize(const Spline::PiecewisePolynomialND& in,Real res,vector<Real>& times,vector<Config>& milestones);
160 void Discretize(const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
161 void Discretize(const MultiPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
162 void Discretize(const ParabolicRamp::DynamicPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
163 void Discretize(const Spline::PiecewisePolynomialND& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
166 void Discretize(Robot& robot,const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones);
167 void Discretize(Robot& robot,const LinearPath& in,Real res,vector<Real>& times,vector<Config>& milestones,vector<Vector>& dmilestones);
168 
169 
172 template <class T1,class T2>
173 void Approximate(const T1& in,Real res,T2& out)
174 {
175  vector<Real> times;
176  vector<Vector> milestones,dmilestones;
177  Discretize(in,res,times,milestones,dmilestones);
178  Interpolate(times,milestones,dmilestones,out);
179 }
182 template <class T1>
183 void Approximate(const T1& in,Real res,LinearPath& out)
184 {
185  vector<Real> times;
186  vector<Vector> milestones;
187  Discretize(in,res,times,milestones);
188  Interpolate(times,milestones,out);
189 }
190 
193 #endif
A very general multi-section path container.
Definition: MultiPath.h:29
The main robot type used in RobotSim.
Definition: Robot.h:79
Definition: Config.h:15
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
void Smooth(const LinearPath &in, T &out)
Definition: Paths.h:122
A piecewise linear path.
Definition: Paths.h:13
void Keyframes(const LinearPath &in, vector< Config > &milestones)
Extract keyframes from the path representation.
void Interpolate(const vector< Real > &times, const vector< Config > &milestones, LinearPath &out)
Create a representation that matches the input keyframes.
Definition: Paths.h:7
void Clear()
clears the path
Definition: Paths.h:26
void Append(Real t, const Config &x)
Appends a new milestone at the given time.
Definition: Paths.h:24
void Approximate(const T1 &in, Real res, T2 &out)
Definition: Paths.h:173
void Discretize(const LinearPath &in, Real res, vector< Real > &times, vector< Config > &milestones)
Split up the path into keyframes at a given resolution.
void Convert(const LinearPath &in, MultiPath &out)
Exact, direct conversion from LinearPath to MultiPath.