Bsplines.hh
Go to the documentation of this file.
1 
5 #ifndef _BSPLINES_H_
6 #define _BSPLINES_H_
7 
8 #include <math.h>
9 
10 #include <deque>
11 #include <iostream>
12 #include <vector>
13 
14 struct Point {
15  double x;
16  double y;
17 };
18 
19 namespace PatternGeneratorJRL {
20 
22 class Bsplines {
23  public:
25  Bsplines(long int degree);
26 
28  ~Bsplines();
29 
31  void GenerateDegree();
32 
35  // void GenerateKnotVector(std::string method);
36 
39 
41  int ComputeBasisFunctions(double t);
42 
43  // computes the basis function without the derivatives
44  int ComputeBasisFunctionsRecursively(double t, std::deque<double> &knot,
45  unsigned int degree);
46  double Nij_t(int i, int j, double t, std::deque<double> &knot);
47 
49  double ComputeBsplines(double t);
50 
52  void SetDegree(long int degree);
53 
55  void SetControlPoints(std::vector<double> &control_points);
56 
58  void SetKnotVector(std::deque<double> &knot_vector);
59 
61  long int GetDegree() const;
62 
64  std::vector<double> GetControlPoints() const;
65 
67  std::deque<double> GetKnotVector() const;
68 
69  void PrintKnotVector() const;
70 
71  void PrintControlPoints() const;
72 
73  void PrintDegree() const;
74 
75  protected:
76  long int m_degree;
77 
78  std::vector<double> m_control_points;
79  std::vector<double> m_derivative_control_points;
80  std::vector<double> m_sec_derivative_control_points;
81 
82  std::vector<std::vector<double> > m_basis_functions;
83  std::vector<double> m_basis_functions_derivative;
84  std::vector<double> m_basis_functions_sec_derivative;
85 
86  std::deque<double> m_knot;
87 };
88 
90 class BSplinesFoot : public Bsplines {
91  public:
97  BSplinesFoot(double FT = 1.0, double IP = 0.0, double FP = 0.0,
98  std::vector<double> ToMP = std::vector<double>(),
99  std::vector<double> MP = std::vector<double>(), double IS = 0.0,
100  double IA = 0.0, double FS = 0.0, double FA = 0.0);
101 
103  ~BSplinesFoot();
104 
113  void SetParameters(double FT, double IP, double FP, std::vector<double> ToMP,
114  std::vector<double> MP, double IS = 0.0, double IA = 0.0,
115  double FS = 0.0, double FA = 0.0);
116  void SetParametersWithoutMPAndToMP(double FT, double IP, double FP, double IS,
117  double IA, double FS, double FA);
118 
120  int Compute(double t, double &x, double &dx, double &ddx);
121 
129 
130  void GetParameters(double &FT, double &IP, double &FP,
131  std::vector<double> &ToMP, std::vector<double> &MP);
132 
133  std::vector<double> MP() { return m_MP; }
134 
135  std::vector<double> ToMP() { return m_ToMP; }
136 
137  double FT() { return m_FT; }
138 
139  void FT(double ft) { m_FT = ft; }
140 
141  double IP() { return m_IP; }
142 
143  double FP() { return m_FP; }
144 
145  void SetParametersWithInitFinalPose(double FT, double IP, double FP,
146  std::vector<double> &ToMP,
147  std::vector<double> &MP);
148 
149  private:
150  double m_FT; // final time
151  double m_IP; // Initial Position
152  double m_IS; // Initial Speed
153  double m_IA; // Initial Acceleration
154  double m_FP; // Final Position
155  double m_FS; // Final Speed
156  double m_FA; // Final Acceleration
157  std::vector<double>
158  m_ToMP; // times to reach the middle (intermediate) positions
159  std::vector<double> m_MP; // middle (intermediate) positions
160 };
161 
162 } // namespace PatternGeneratorJRL
163 #endif /* _BSPLINES_H_*/
std::vector< std::vector< double > > m_basis_functions
Definition: Bsplines.hh:82
std::deque< double > GetKnotVector() const
Definition: Bsplines.cpp:247
int ComputeBasisFunctionsRecursively(double t, std::deque< double > &knot, unsigned int degree)
Definition: Bsplines.cpp:144
double y
Definition: Bsplines.hh:16
Bsplines(long int degree)
Definition: Bsplines.cpp:11
void GetParameters(double &FT, double &IP, double &FP, std::vector< double > &ToMP, std::vector< double > &MP)
Definition: Bsplines.cpp:3442
void ComputeControlPointFrom3DataPoint()
Definition: Bsplines.cpp:579
void SetKnotVector(std::deque< double > &knot_vector)
Definition: Bsplines.cpp:237
double x
Definition: Bsplines.hh:15
void PrintKnotVector() const
Definition: Bsplines.cpp:249
double FP()
Definition: Bsplines.hh:143
void ComputeControlPointFrom2DataPoint()
Definition: Bsplines.cpp:382
std::vector< double > m_control_points
Definition: Bsplines.hh:78
doublereal * x
Definition: qld.cpp:386
~BSplinesFoot()
Definition: Bsplines.cpp:279
Bsplines used for Z trajectory of stair steps.
Definition: Bsplines.hh:90
BSplinesFoot(double FT=1.0, double IP=0.0, double FP=0.0, std::vector< double > ToMP=std::vector< double >(), std::vector< double > MP=std::vector< double >(), double IS=0.0, double IA=0.0, double FS=0.0, double FA=0.0)
Definition: Bsplines.cpp:272
void FT(double ft)
Definition: Bsplines.hh:139
void SetParametersWithInitFinalPose(double FT, double IP, double FP, std::vector< double > &ToMP, std::vector< double > &MP)
Definition: Bsplines.cpp:3466
void PrintControlPoints() const
Definition: Bsplines.cpp:257
void SetParametersWithoutMPAndToMP(double FT, double IP, double FP, double IS, double IA, double FS, double FA)
Definition: Bsplines.cpp:3451
std::vector< double > GetControlPoints() const
Definition: Bsplines.cpp:243
Definition: Bsplines.hh:22
double IP()
Definition: Bsplines.hh:141
void SetDegree(long int degree)
Definition: Bsplines.cpp:227
void SetControlPoints(std::vector< double > &control_points)
Definition: Bsplines.cpp:229
int ComputeBasisFunctions(double t)
Definition: Bsplines.cpp:28
std::vector< double > m_basis_functions_sec_derivative
Definition: Bsplines.hh:84
double FT()
Definition: Bsplines.hh:137
std::vector< double > m_basis_functions_derivative
Definition: Bsplines.hh:83
void SetParameters(double FT, double IP, double FP, std::vector< double > ToMP, std::vector< double > MP, double IS=0.0, double IA=0.0, double FS=0.0, double FA=0.0)
Definition: Bsplines.cpp:298
Definition: Bsplines.hh:14
std::vector< double > m_derivative_control_points
Definition: Bsplines.hh:79
void GenerateDegree()
Definition: Bsplines.cpp:19
~Bsplines()
Definition: Bsplines.cpp:17
std::vector< double > ToMP()
Definition: Bsplines.hh:135
double Nij_t(int i, int j, double t, std::deque< double > &knot)
Definition: Bsplines.cpp:155
long int GetDegree() const
Definition: Bsplines.cpp:241
Bsplines DerivativeBsplines()
Definition: Bsplines.cpp:197
int Compute(double t, double &x, double &dx, double &ddx)
Definition: Bsplines.cpp:281
std::vector< double > m_sec_derivative_control_points
Definition: Bsplines.hh:80
double ComputeBsplines(double t)
Definition: Bsplines.cpp:183
Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
void PrintDegree() const
Definition: Bsplines.cpp:265
void ComputeControlPointFrom4DataPoint()
Definition: Bsplines.cpp:1334
long int m_degree
Definition: Bsplines.hh:76
std::vector< double > MP()
Definition: Bsplines.hh:133
std::deque< double > m_knot
Definition: Bsplines.hh:86