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
14struct Point {
15 double x;
16 double y;
17};
18
19namespace PatternGeneratorJRL {
20
22class 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;
81
82 std::vector<std::vector<double> > m_basis_functions;
83 std::vector<double> m_basis_functions_derivative;
85
86 std::deque<double> m_knot;
87};
88
90class 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
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_*/
Bsplines used for Z trajectory of stair steps.
Definition: Bsplines.hh:90
void ComputeControlPointFrom4DataPoint()
Definition: Bsplines.cpp:1334
void GetParameters(double &FT, double &IP, double &FP, std::vector< double > &ToMP, std::vector< double > &MP)
Definition: Bsplines.cpp:3442
int Compute(double t, double &x, double &dx, double &ddx)
Definition: Bsplines.cpp:281
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
std::vector< double > ToMP()
Definition: Bsplines.hh:135
~BSplinesFoot()
Definition: Bsplines.cpp:279
void FT(double ft)
Definition: Bsplines.hh:139
double IP()
Definition: Bsplines.hh:141
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
double FP()
Definition: Bsplines.hh:143
void SetParametersWithoutMPAndToMP(double FT, double IP, double FP, double IS, double IA, double FS, double FA)
Definition: Bsplines.cpp:3451
std::vector< double > MP()
Definition: Bsplines.hh:133
void SetParametersWithInitFinalPose(double FT, double IP, double FP, std::vector< double > &ToMP, std::vector< double > &MP)
Definition: Bsplines.cpp:3466
void ComputeControlPointFrom2DataPoint()
Definition: Bsplines.cpp:382
double FT()
Definition: Bsplines.hh:137
void ComputeControlPointFrom3DataPoint()
Definition: Bsplines.cpp:579
Definition: Bsplines.hh:22
void SetKnotVector(std::deque< double > &knot_vector)
Definition: Bsplines.cpp:237
long int GetDegree() const
Definition: Bsplines.cpp:241
std::vector< double > m_sec_derivative_control_points
Definition: Bsplines.hh:80
int ComputeBasisFunctions(double t)
Definition: Bsplines.cpp:28
void SetControlPoints(std::vector< double > &control_points)
Definition: Bsplines.cpp:229
~Bsplines()
Definition: Bsplines.cpp:17
void PrintDegree() const
Definition: Bsplines.cpp:265
std::vector< std::vector< double > > m_basis_functions
Definition: Bsplines.hh:82
std::vector< double > m_control_points
Definition: Bsplines.hh:78
std::deque< double > m_knot
Definition: Bsplines.hh:86
void PrintControlPoints() const
Definition: Bsplines.cpp:257
long int m_degree
Definition: Bsplines.hh:76
std::deque< double > GetKnotVector() const
Definition: Bsplines.cpp:247
void PrintKnotVector() const
Definition: Bsplines.cpp:249
Bsplines(long int degree)
Definition: Bsplines.cpp:11
void SetDegree(long int degree)
Definition: Bsplines.cpp:227
std::vector< double > m_basis_functions_sec_derivative
Definition: Bsplines.hh:84
std::vector< double > m_basis_functions_derivative
Definition: Bsplines.hh:83
void GenerateDegree()
Definition: Bsplines.cpp:19
Bsplines DerivativeBsplines()
Definition: Bsplines.cpp:197
std::vector< double > GetControlPoints() const
Definition: Bsplines.cpp:243
double Nij_t(int i, int j, double t, std::deque< double > &knot)
Definition: Bsplines.cpp:155
double ComputeBsplines(double t)
Definition: Bsplines.cpp:183
int ComputeBasisFunctionsRecursively(double t, std::deque< double > &knot, unsigned int degree)
Definition: Bsplines.cpp:144
std::vector< double > m_derivative_control_points
Definition: Bsplines.hh:79
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
doublereal * x
Definition: qld.cpp:386
Definition: Bsplines.hh:14
double x
Definition: Bsplines.hh:15
double y
Definition: Bsplines.hh:16