AnalyticalZMPCOGTrajectory.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2008, 2009, 2010,
3  *
4  * Alireza Nakhaei
5  * Olivier Stasse
6  *
7  * JRL, CNRS/AIST
8  *
9  * This file is part of walkGenJrl.
10  * walkGenJrl is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU Lesser General Public License as published by
12  * the Free Software Foundation, either version 3 of the License, or
13  * (at your option) any later version.
14  *
15  * walkGenJrl is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Lesser Public License for more details.
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
21  *
22  * Research carried out within the scope of the
23  * Joint Japanese-French Robotics Laboratory (JRL)
24  */
28 #ifndef _ANALYTICAL_COG_H_
29 #define _ANALYTICAL_COG_H_
30 
31 #include <iostream>
32 #include <vector>
33 
34 #include <Mathematics/Polynome.hh>
35 
36 namespace PatternGeneratorJRL {
37 
43 
44 public:
46  AnalyticalZMPCOGTrajectory(int lNbOfIntervals = 0);
47 
50 
58  bool ComputeCOM(double t, double &r);
59 
67  bool ComputeCOMSpeed(double t, double &r);
68 
79  bool ComputeCOM(double t, double &r, int i);
80 
91  bool ComputeCOMSpeed(double t, double &r, int i);
92 
103  bool ComputeCOMAcceleration(double t, double &r, int j);
104 
112  bool ComputeZMP(double t, double &r);
113 
121  bool ComputeZMPSpeed(double t, double &r);
122 
131  bool ComputeZMP(double t, double &r, int i);
132 
137  void SetNumberOfIntervals(unsigned int lNbOfIntervals);
138 
141  void SetCoGHyperbolicCoefficients(std::vector<double> &lV,
142  std::vector<double> &lW);
143 
145  void SetStartingTimeIntervalsAndHeightVariation(std::vector<double> &lDeltaTj,
146  std::vector<double> &lomegaj);
147 
152  void SetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegree);
153 
156  void GetPolynomialDegrees(std::vector<unsigned int> &lPolynomialDegree) const;
157 
160  void GetNumberOfIntervals(unsigned int &lNbOfIntervals) const;
161 
164  void GetHyperbolicCoefficients(std::vector<double> &lV,
165  std::vector<double> &lW) const;
166 
168  void GetStartingPointAndHeightVariation(std::vector<double> &lTj,
169  std::vector<double> &lomegaj);
170 
176  bool GetFromListOfCOGPolynomials(unsigned int j, Polynome *&aPoly) const;
177 
182  bool GetFromListOfZMPPolynomials(unsigned int j, Polynome *&aPoly) const;
183 
189  void
190  TransfertCoefficientsFromCOGTrajectoryToZMPOne(std::vector<double> &lCOMZ,
191  std::vector<double> &lZMPZ);
192 
199  unsigned int IntervalIndex, double &lCOMZ, double &lZMPZ);
200 
204  void Building3rdOrderPolynomial(unsigned int anIntervalj, double pjTjm1,
205  double pjTj);
206 
211  double FluctuationMaximal();
212 
213  friend std::ostream &operator<<(std::ostream &os,
214  const AnalyticalZMPCOGTrajectory &obj);
215 
218 
220  void SetAbsoluteTimeReference(double anAbsoluteTimeReference) {
221  m_AbsoluteTimeReference = anAbsoluteTimeReference;
222  }
223 
225  bool GetIntervalIndexFromTime(double t, unsigned int &j);
226 
229  bool GetIntervalIndexFromTime(double t, unsigned int &j,
230  unsigned int &prev_j);
231 
232 protected:
235 
238  std::vector<double> m_V;
239 
242  std::vector<double> m_W;
243 
245  std::vector<double> m_DeltaTj;
246 
249  std::vector<double> m_omegaj;
250 
252  std::vector<double> m_RefTime;
253 
255  std::vector<unsigned int> m_PolynomialDegree;
256 
258  std::vector<Polynome *> m_ListOfCOGPolynomials;
259 
261  std::vector<Polynome *> m_ListOfZMPPolynomials;
262 
264  void FreePolynomes();
265 
268 
269  /* \brien Sensitivity to numerical noise. */
271 };
272 
273 std::ostream &operator<<(std::ostream &os,
274  const AnalyticalZMPCOGTrajectory &obj);
275 } // namespace PatternGeneratorJRL
276 #endif /* _ANALYTICAL_COG_H_ */
PatternGeneratorJRL::Polynome
Definition: Polynome.hh:42
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_RefTime
std::vector< double > m_RefTime
Definition: AnalyticalZMPCOGTrajectory.hh:252
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::SetStartingTimeIntervalsAndHeightVariation
void SetStartingTimeIntervalsAndHeightVariation(std::vector< double > &lDeltaTj, std::vector< double > &lomegaj)
Set the starting point and the height variation.
Definition: AnalyticalZMPCOGTrajectory.cpp:269
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::ComputeCOM
bool ComputeCOM(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:79
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_W
std::vector< double > m_W
Definition: AnalyticalZMPCOGTrajectory.hh:242
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne
void TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(unsigned int IntervalIndex, double &lCOMZ, double &lZMPZ)
Transfert the coefficients from the COG trajectory to the ZMP.
Definition: AnalyticalZMPCOGTrajectory.cpp:353
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_AbsoluteTimeReference
double m_AbsoluteTimeReference
Definition: AnalyticalZMPCOGTrajectory.hh:267
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_DeltaTj
std::vector< double > m_DeltaTj
Definition: AnalyticalZMPCOGTrajectory.hh:245
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetAbsoluteTimeReference
double GetAbsoluteTimeReference() const
Absolute Time reference of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.hh:217
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetFromListOfZMPPolynomials
bool GetFromListOfZMPPolynomials(unsigned int j, Polynome *&aPoly) const
Definition: AnalyticalZMPCOGTrajectory.cpp:341
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::SetNumberOfIntervals
void SetNumberOfIntervals(unsigned int lNbOfIntervals)
Set the number of Intervals for this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:60
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_PolynomialDegree
std::vector< unsigned int > m_PolynomialDegree
Definition: AnalyticalZMPCOGTrajectory.hh:255
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetNumberOfIntervals
void GetNumberOfIntervals(unsigned int &lNbOfIntervals) const
Set the number of Intervals for this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:313
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_ListOfCOGPolynomials
std::vector< Polynome * > m_ListOfCOGPolynomials
Definition: AnalyticalZMPCOGTrajectory.hh:258
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetPolynomialDegrees
void GetPolynomialDegrees(std::vector< unsigned int > &lPolynomialDegree) const
Get the degree of each polynomials for the CoG.
Definition: AnalyticalZMPCOGTrajectory.cpp:308
PatternGeneratorJRL::operator<<
std::ostream & operator<<(std::ostream &os, const COMPosition_s &aCp)
Definition: pgtypes.hh:63
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::AnalyticalZMPCOGTrajectory
AnalyticalZMPCOGTrajectory(int lNbOfIntervals=0)
Definition: AnalyticalZMPCOGTrajectory.cpp:37
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::SetCoGHyperbolicCoefficients
void SetCoGHyperbolicCoefficients(std::vector< double > &lV, std::vector< double > &lW)
Set the coefficients for the sinuse and cosinues function.
Definition: AnalyticalZMPCOGTrajectory.cpp:261
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::ComputeZMP
bool ComputeZMP(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:185
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::operator<<
friend std::ostream & operator<<(std::ostream &os, const AnalyticalZMPCOGTrajectory &obj)
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_V
std::vector< double > m_V
Definition: AnalyticalZMPCOGTrajectory.hh:238
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetStartingPointAndHeightVariation
void GetStartingPointAndHeightVariation(std::vector< double > &lTj, std::vector< double > &lomegaj)
Get the starting point and the height variation.
Definition: AnalyticalZMPCOGTrajectory.cpp:324
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_omegaj
std::vector< double > m_omegaj
Definition: AnalyticalZMPCOGTrajectory.hh:249
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_ListOfZMPPolynomials
std::vector< Polynome * > m_ListOfZMPPolynomials
Definition: AnalyticalZMPCOGTrajectory.hh:261
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::ComputeZMPSpeed
bool ComputeZMPSpeed(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:218
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::TransfertCoefficientsFromCOGTrajectoryToZMPOne
void TransfertCoefficientsFromCOGTrajectoryToZMPOne(std::vector< double > &lCOMZ, std::vector< double > &lZMPZ)
Transfert the coefficients from the COG trajectory to the ZMP for all intervals.
Definition: AnalyticalZMPCOGTrajectory.cpp:378
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::SetPolynomialDegrees
void SetPolynomialDegrees(std::vector< unsigned int > &lPolynomialDegree)
Set the degree of each polynomials for the CoG Remark: the size of the vector of degrees should match...
Definition: AnalyticalZMPCOGTrajectory.cpp:288
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_NbOfIntervals
int m_NbOfIntervals
Definition: AnalyticalZMPCOGTrajectory.hh:234
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::SetAbsoluteTimeReference
void SetAbsoluteTimeReference(double anAbsoluteTimeReference)
Set Absolute time reference of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.hh:220
Polynome.hh
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::m_Sensitivity
double m_Sensitivity
Definition: AnalyticalZMPCOGTrajectory.hh:270
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetHyperbolicCoefficients
void GetHyperbolicCoefficients(std::vector< double > &lV, std::vector< double > &lW) const
Get the coefficients for the sinuse and cosinues function.
Definition: AnalyticalZMPCOGTrajectory.cpp:318
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetFromListOfCOGPolynomials
bool GetFromListOfCOGPolynomials(unsigned int j, Polynome *&aPoly) const
Get the polynomial at interval j for the CoG Remark: The call to this function assume that the method...
Definition: AnalyticalZMPCOGTrajectory.cpp:330
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::ComputeCOMSpeed
bool ComputeCOMSpeed(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:117
PatternGeneratorJRL
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::ComputeCOMAcceleration
bool ComputeCOMAcceleration(double t, double &r, int j)
Definition: AnalyticalZMPCOGTrajectory.cpp:173
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory
Definition: AnalyticalZMPCOGTrajectory.hh:42
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::FreePolynomes
void FreePolynomes()
Definition: AnalyticalZMPCOGTrajectory.cpp:45
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::~AnalyticalZMPCOGTrajectory
~AnalyticalZMPCOGTrajectory()
Definition: AnalyticalZMPCOGTrajectory.cpp:43
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime
bool GetIntervalIndexFromTime(double t, unsigned int &j)
Get the index of the interval according to the time.
Definition: AnalyticalZMPCOGTrajectory.cpp:430
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::Building3rdOrderPolynomial
void Building3rdOrderPolynomial(unsigned int anIntervalj, double pjTjm1, double pjTj)
Definition: AnalyticalZMPCOGTrajectory.cpp:388
PatternGeneratorJRL::AnalyticalZMPCOGTrajectory::FluctuationMaximal
double FluctuationMaximal()
Returns the maximal fluctuation for the first segment of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:417