Bsplines.hh
Go to the documentation of this file.
1 
5 #ifndef _BSPLINES_H_
6 #define _BSPLINES_H_
7 
8 #include <deque>
9 #include <iostream>
10 #include <math.h>
11 #include <vector>
12 
13 struct Point {
14  double x;
15  double y;
16 };
17 
18 namespace PatternGeneratorJRL {
19 
21 class Bsplines {
22 
23 public:
25  Bsplines(long int degree);
26 
28  ~Bsplines();
29 
31  void GenerateDegree();
32 
35  // void GenerateKnotVector(std::string method);
36 
38  Bsplines DerivativeBsplines();
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 
126  void ComputeControlPointFrom2DataPoint();
127  void ComputeControlPointFrom3DataPoint();
128  void ComputeControlPointFrom4DataPoint();
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
double y
Definition: Bsplines.hh:15
double x
Definition: Bsplines.hh:14
double FP()
Definition: Bsplines.hh:143
std::vector< double > m_control_points
Definition: Bsplines.hh:78
Bsplines used for Z trajectory of stair steps.
Definition: Bsplines.hh:90
void FT(double ft)
Definition: Bsplines.hh:139
Definition: Bsplines.hh:21
double IP()
Definition: Bsplines.hh:141
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
Definition: Bsplines.hh:13
std::vector< double > m_derivative_control_points
Definition: Bsplines.hh:79
std::vector< double > ToMP()
Definition: Bsplines.hh:135
std::vector< double > m_sec_derivative_control_points
Definition: Bsplines.hh:80
Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
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