#include <iostream>
#include <fstream>
#include <math.h>
#include <Mathematics/AnalyticalZMPCOGTrajectory.hh>
#include <Debug.hh>
void multiBody(bool multiBody)
Definition: rigid-body-system.hh:152
char * iciunit
Definition: qld.cpp:174
void SetHeightOfCoM(double lZc)
Definition: PreviewControl.cpp:125
static integer nact
Definition: qld.cpp:405
StepOverSpline()
Definition: StepOverPolynome.cpp:268
static const int ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:200
ftnlen infmtlen
Definition: qld.cpp:226
int stepType
Definition: pgtypes.hh:105
doublereal eps
Definition: qld.cpp:337
double x
Definition: pgtypes.hh:209
goto L910
Definition: qld.cpp:1224
double y
Definition: pgtypes.hh:143
double y[3]
Definition: pgtypes.hh:54
static integer kflag
Definition: qld.cpp:658
static doublereal res
Definition: qld.cpp:676
void SetPreviewControlTime(double lPreviewControlTime)
Definition: PreviewControl.cpp:113
ftnint informlen
Definition: qld.cpp:228
static integer iws
Definition: qld.cpp:677
static integer irb
Definition: qld.cpp:673
~OptimalControllerSolver()
Definition: OptimalControllerSolver.cpp:128
double dy
Definition: pgtypes.hh:145
static const int ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:197
double weight
Ponderation.
Definition: intermediate-qp-matrices.hh:89
Polynome for the hip trajectory.
Definition: StepOverPolynome.hh:105
#define MAL_VECTOR(name, type)
int Interpolation(std::deque< COMState > &COMStates, std::deque< ZMPPosition > &ZMPRefPositions, int CurrentPosition, double CX, double CY)
Interpolation during a simulation period with control parameters.
Definition: LinearizedInvertedPendulum2D.cpp:157
const RigidBody & LeftFoot() const
Definition: rigid-body-system.hh:111
Definition: intermediate-qp-matrices.hh:86
int Degree()
Definition: Polynome.hh:69
static doublereal fdiffa
Definition: qld.cpp:669
void CoM(const RigidBody &CoM)
Definition: rigid-body-system.hh:108
ComputeConvexHull()
Definition: ConvexHull.cpp:77
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:412
IntermedQPMat()
Definition: intermediate-qp-matrices.cpp:34
~ZMPPreviewControlWithMultiBodyZMP()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:98
ftnint ciunit
Definition: qld.cpp:165
integer * liwar
Definition: qld.cpp:391
static const unsigned int MODE_WITHOUT_INITIALPOS
Definition: OptimalControllerSolver.hh:143
void DisplayWeights()
Definition: OptimalControllerSolver.cpp:354
std::vector< CH_Point > ConvexHullList
Definition: ConvexHull.hh:44
Polynome used for Z trajectory during stepover.
Definition: StepOverPolynome.hh:47
Definition: FootHalfSize.hh:34
double ComputeAlpha(vector< unsigned int > &NewActivatedConstraints, vector< int > &SimilarConstraint)
Definition: PLDPSolver.cpp:534
ftnint cirec
Definition: qld.cpp:168
static doublereal step
Definition: qld.cpp:653
double m_SSPeriod
Definition: SupportFSM_backup.hh:52
Polynome5(double FT, double FP)
Constructor: FT: Final time FP: Final position.
Definition: PolynomeFoot.cpp:159
double ddomega2
Definition: pgtypes.hh:147
static integer mn
Definition: qld.cpp:409
FootHalfSize()
Definition: FootHalfSize.cpp:32
double getHalfHeight() const
Definition: FootHalfSize.cpp:86
doublereal * c
Definition: qld.cpp:388
~StepOverClampedCubicSpline()
Definition: StepOverPolynome.cpp:544
SupportFSM()
Constructor.
Definition: SupportFSM.cpp:34
double CompareCBRep(CH_Point &s1, CH_Point &s2)
Definition: ConvexHull.cpp:66
static integer il
Definition: qld.cpp:670
int EvaluateStartingState(MAL_VECTOR(&, double) BodyAngles, MAL_S3_VECTOR(&, double) aStartingCOMState, MAL_S3_VECTOR(&, double) aStartingZMPPosition, MAL_VECTOR(&, double) aStartingWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:797
void CoMHeight(double Height)
Definition: rigid-body-system.hh:147
virtual ~PatternGeneratorInterface()
Definition: patterngeneratorinterface.hh:66
int * me
Definition: qld.cpp:387
static doublereal ga
Definition: qld.cpp:667
static doublereal sumx
Definition: qld.cpp:655
double pz
Definition: pgtypes.hh:122
int ForwardSubstitution()
Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1.
Definition: PLDPHerdt.cpp:384
static doublereal diagr
Definition: qld.cpp:659
StepOverPolynomeFoot()
Definition: StepOverPolynome.cpp:40
#define cmache_1
Definition: qld.cpp:341
static integer iwa
Definition: qld.cpp:673
void DSSSPeriod(const double DSSSPeriod)
Definition: SupportFSM.hh:83
rigid_body_state_t & State()
Definition: rigid-body.hh:154
static integer iy
Definition: qld.cpp:670
const trunk_t & Trunk() const
Definition: intermediate-qp-matrices.hh:128
static integer jl
Definition: qld.cpp:670
void WriteCurrentZMPSolution(string filename, double *XkYk)
Definition: PLDPHerdt.cpp:1073
static integer lflag
Definition: qld.cpp:658
RelativeFeetInequalities(SimplePluginManager *aSPM, CjrlHumanoidDynamicRobot *aHS)
Definition: relative-feet-inequalities.cpp:42
static doublereal temp
Definition: qld.cpp:653
boost_ublas::vector< double > Z
Definition: rigid-body.hh:44
static integer lw
Definition: qld.cpp:409
long flag
Definition: qld.cpp:157
double pitch[3]
Definition: pgtypes.hh:82
~IntermedQPMat()
Definition: intermediate-qp-matrices.cpp:48
virtual void getAnklePositionInLocalFrame(vector3d &outCoordinates) const=0
void SetStrategyForPCStages(int Strategy)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:829
void SetSimulationControlPeriod(const double &)
Definition: LinearizedInvertedPendulum2D.cpp:78
const double & GetSimulationControlPeriod() const
Definition: LinearizedInvertedPendulum2D.cpp:73
int ComputeProjectedDescentDirection(unsigned int NumberSteps)
Compute Projected descent direction.
Definition: PLDPHerdt.cpp:446
#define VOID
Definition: qld.cpp:237
goto L570
Definition: qld.cpp:1517
const linear_inequality_t & Inequalities(ineq_e type) const
Definition: intermediate-qp-matrices.cpp:86
std::ostream & print(std::ostream &o) const
Definition: intermediate-qp-matrices.cpp:173
std::vector< unsigned int > m_PolynomialDegree
Definition: AnalyticalZMPCOGTrajectory.hh:249
boost_ublas::vector< double > Yaw
Definition: rigid-body.hh:50
void SetParameters(double FT, double FP)
Set the parameters.
Definition: PolynomeFoot.cpp:174
std::vector< int > SimilarConstraints
Definition: pgtypes.hh:173
doublereal * b
Definition: qld.cpp:388
unsigned int m_NbOfStepsSSDS
Definition: SupportFSM_backup.hh:55
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:362
static integer iflag
Definition: qld.cpp:658
virtual bool currentConfiguration(const vectorN &inConfig)=0
real r
Definition: qld.cpp:242
double sy
Definition: pgtypes.hh:102
Long * dims
Definition: qld.cpp:255
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:75
double y
Definition: pgtypes.hh:192
void SamplingPeriodSim(double T)
Definition: rigid-body.hh:133
PLDPSolver(unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ)
Constructor.
Definition: PLDPSolver.cpp:58
static integer ipp
Definition: qld.cpp:677
Structure to store each of the ZMP value, with a given direction at a certain time.
Definition: pgtypes.hh:120
doublereal d__3
Definition: qld.cpp:646
integer * iwar
Definition: qld.cpp:391
logical(* L_fp)()
Definition: qld.cpp:297
Generate a stack of inequalities relative to feet centers for the whole preview window.
Definition: relative-feet-inequalities.hh:51
boost_ublas::vector< double > Y
Definition: rigid-body.hh:43
void AllocateMemoryForSolver(unsigned int NumberSteps)
Definition: PLDPHerdt.cpp:111
ftnlen inunflen
Definition: qld.cpp:230
Definition: ConvexHull.hh:47
MAL_VECTOR(Center, double)
ftnlen infilen
Definition: qld.cpp:212
static integer in
Definition: qld.cpp:409
bool ComputeZMP(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:198
double dtheta
Definition: pgtypes.hh:145
~Polynome6()
Destructor.
Definition: PolynomeFoot.cpp:285
void GetDifferenceBetweenComAndWaist(double lComAndWaist[3])
bool IsCoherent()
Indicates if the weights are coherent with the parameters.
Definition: PreviewControl.cpp:137
int * mnn
Definition: qld.cpp:387
static doublereal vfact
Definition: qld.cpp:661
void updateHalfHeightDS(double DSFeetDistance)
Definition: FootHalfSize.cpp:71
Definition: AnalyticalZMPCOGTrajectory.hh:45
integer * iact
Definition: qld.cpp:637
void LeftFoot(const RigidBody &LeftFoot)
Definition: rigid-body-system.hh:115
virtual bool setProperty(std::string &, const std::string &)
#define MAL_MATRIX_DIM(name, type, nb_rows, nb_cols)
void setHalfSizeInit(double lHalfWidth, double lHalfHeight, double DSFeetDistance)
Definition: FootHalfSize.cpp:51
doublereal(* D_fp)()
Definition: qld.cpp:294
static doublereal xmag
Definition: qld.cpp:653
double yaw[3]
Definition: pgtypes.hh:81
Polynome4(double FT, double MP)
Constructor: FT: Final time MP: Middle position.
Definition: PolynomeFoot.cpp:95
doublereal * a
Definition: qld.cpp:388
static doublereal one
Definition: qld.cpp:674
doublereal d
Definition: qld.cpp:243
friend std::ostream & operator<<(std::ostream &os, const AnalyticalZMPCOGTrajectory &obj)
Definition: AnalyticalZMPCOGTrajectory.cpp:509
i__2
Definition: qld.cpp:939
int interpolate(std::deque< COMState > &COMStates, std::deque< ZMPPosition > &ZMPRefPositions, int CurrentPosition, double CX, double CY)
Interpolate.
StepOverPolynomeFootZtoX()
Definition: StepOverPolynome.cpp:100
Polynome used for X trajectory in function of time to combine with StepOverPolynomeFootZtoX.
Definition: StepOverPolynome.hh:87
objective_e type
Minimization objective type.
Definition: intermediate-qp-matrices.hh:92
Definition: intermediate-qp-matrices.hh:51
char * infmt
Definition: qld.cpp:225
static doublereal sum
Definition: qld.cpp:678
double time
Definition: pgtypes.hh:124
bool ComputeCOMSpeed(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:134
double StepPeriod() const
Definition: SupportFSM.hh:71
double DStime
Definition: pgtypes.hh:104
int * mmax
Definition: qld.cpp:387
void SetParameters(MAL_VECTOR(Zpos, double), std::vector< double > Xpos)
Definition: StepOverPolynome.cpp:105
int SetupFirstPhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:531
double DSPeriod() const
Definition: SupportFSM.hh:76
goto L440
Definition: qld.cpp:1234
const double & Mass() const
Definition: rigid-body.hh:146
int ql0001_(int *m, int *me, int *mmax, int *n, int *nmax, int *mnn, double *c, double *d, double *a, double *b, double *xl, double *xu, double *x, double *u, int *iout, int *ifail, int *iprint, double *war, int *lwar, int *iwar, int *liwar, double *eps1)
c_offset
Definition: qld.cpp:432
~FootHalfSize()
Definition: FootHalfSize.cpp:47
#define MAL_MATRIX_FILL(matrix, value)
void update_vel_reference(reference_t &Ref, const support_state_t &CurrentSupport)
Update the velocity reference after a pure rotation.
Definition: SupportFSM.cpp:58
#define max(a, b)
Definition: qld.cpp:270
~StepOverPolynomeHip4()
Definition: StepOverPolynome.cpp:257
void print()
Overloading of << operator.
Definition: PreviewControl.cpp:487
ftnint inunit
Definition: qld.cpp:210
static integer kk
Definition: qld.cpp:670
MAL_MATRIX(Coefficients, double)
shortlogical(* K_fp)()
Definition: qld.cpp:298
Definition: rigid-body-system.hh:37
double row
Definition: ConvexHull.hh:41
Polynome used for X,Y and Theta trajectories.
Definition: PolynomeFoot.hh:126
double py
Definition: pgtypes.hh:122
t_cmache_ cmache_
Definition: qld.cpp:339
VOID C_f
Definition: qld.cpp:303
LinearizedInvertedPendulum2D()
Definition: LinearizedInvertedPendulum2D.cpp:40
Definition: pgtypes.hh:190
objective_variant_s objective_variant_t
Definition: intermediate-qp-matrices.hh:97
char * inseq
Definition: qld.cpp:221
boost_ublas::vector< double > X
Definition: rigid-body.hh:42
static integer nflag
Definition: qld.cpp:660
ComAndFootRealization * getComAndFootRealization()
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:457
double m_DSDuration
Numerical precision */ double m_eps;.
Definition: SupportFSM_backup.hh:52
const double & GetComHeight() const
Definition: LinearizedInvertedPendulum2D.cpp:63
unsigned int number
Definition: StepOverPolynome.hh:146
g_offset
Definition: qld.cpp:724
void SetStartingTimeIntervalsAndHeightVariation(std::vector< double > &lDeltaTj, std::vector< double > &lomegaj)
Set the starting point and the height variation.
Definition: AnalyticalZMPCOGTrajectory.cpp:293
static doublereal two
Definition: qld.cpp:680
#define FALSE_
Definition: qld.cpp:142
double z[3]
Definition: pgtypes.hh:55
trunk_t Trunk
TrunkState.
Definition: intermediate-qp-matrices.hh:62
virtual bool currentAcceleration(const vectorN &inAcceleration)=0
~LinearizedInvertedPendulum2D()
Definition: LinearizedInvertedPendulum2D.cpp:59
bool GetIntervalIndexFromTime(double t, unsigned int &j)
Get the index of the interval according to the time.
Definition: AnalyticalZMPCOGTrajectory.cpp:472
~Polynome()
Definition: Polynome.cpp:39
ftnint * inrecl
Definition: qld.cpp:231
~StepOverPolynomeFoot()
Definition: StepOverPolynome.cpp:95
Definition: ConvexHull.hh:39
void GetState(MAL_VECTOR_TYPE(double) &lxk)
Definition: LinearizedInvertedPendulum2D.cpp:108
#define MAL_MATRIX_RESIZE(name, nb_rows, nb_cols)
state_variant_t & State()
Definition: intermediate-qp-matrices.hh:114
state_variant_s state_variant_t
Definition: intermediate-qp-matrices.hh:81
char * cifmt
Definition: qld.cpp:167
doublereal * grad
Definition: qld.cpp:636
static integer iw
Definition: qld.cpp:670
double ComputeSecDerivative(double t)
Definition: Polynome.cpp:66
MAL_S4x4_MATRIX_TYPE(double) ZMPPreviewControlWithMultiBodyZMP
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:783
void DoComputeConvexHull(std::vector< CH_Point > aVecOfPoints, std::vector< CH_Point > &TheConvexHull)
Definition: ConvexHull.cpp:87
#define MAL_RET_VECTOR_DATABLOCK(name)
int BackwardSubstitution()
Compute v2 q (14b) in Dimitrov 2009. Second phase a Now solving LL^t v2 = v1 <-> L y = v1 with L^t v2...
Definition: PLDPHerdt.cpp:409
double doublereal
Definition: OptimalControllerSolver.cpp:41
double getHalfHeightDS() const
Definition: FootHalfSize.cpp:96
flag cerr
Definition: qld.cpp:196
flag cierr
Definition: qld.cpp:164
integer i
Definition: qld.cpp:241
#define MAL_RET_MATRIX_DATABLOCK(matrix)
static doublereal cvmax
Definition: qld.cpp:663
int dgges_(char *, char *, char *, L_fp, integer *, doublereal *, integer *, doublereal *, integer *, integer *, doublereal *, doublereal *, doublereal *, doublereal *, integer *, doublereal *, integer *, doublereal *, integer *, logical *, integer *)
virtual void CallMethod(std::string &Method, std::istringstream &Args)
Definition: FootConstraintsAsLinearSystem.cpp:541
char * address
Definition: qld.cpp:132
goto L550
Definition: qld.cpp:1454
int SupportFoot
Definition: pgtypes.hh:193
CH_Point HRP2CIO_GlobalP0
Definition: ConvexHull.cpp:37
static doublereal gb
Definition: qld.cpp:667
static integer kfinc
Definition: qld.cpp:660
ftnint icirnum
Definition: qld.cpp:178
double m_Q
Definition: OptimalControllerSolver.hh:198
void SamplingPeriodSim(double T)
Definition: rigid-body-system.hh:127
double StartingTime
Definition: pgtypes.hh:174
#define MAL_INVERSE(name, inv_matrix, type)
spline function calculation class to calculate cubic splines
Definition: StepOverPolynome.hh:128
void Trunk(const trunk_t &Trunk)
Definition: intermediate-qp-matrices.hh:130
integer * iprint
Definition: qld.cpp:389
Definition: PLDPSolver.hh:44
int ql0001_(m, me, mmax, n, nmax, mnn, c, d, a, b, xl, xu, x, u, iout, ifail, iprint, war, lwar, iwar, liwar, eps1) integer *m
void TransfertOneIntervalCoefficientsFromCOGTrajectoryToZMPOne(unsigned int IntervalIndex, double &lCOMZ, double &lZMPZ)
Transfert the coefficients from the COG trajectory to the ZMP.
Definition: AnalyticalZMPCOGTrajectory.cpp:386
bool multiBody() const
Definition: rigid-body-system.hh:150
#define MAL_S3_VECTOR_TYPE(type)
void SamplingPeriod(const double T)
Definition: SupportFSM.hh:93
double doublereal
Definition: qld.cpp:135
double EndingTime
Definition: pgtypes.hh:174
char * name
Definition: qld.cpp:253
double DeviationHipHeight
Definition: pgtypes.hh:107
int integer
Definition: qld.cpp:131
Definition: rigid-body.hh:63
com_t CoM
State of the Center of Mass.
Definition: intermediate-qp-matrices.hh:59
Structure to store the COM position computed by the preview control.
Definition: pgtypes.hh:52
double x[3]
Definition: pgtypes.hh:80
ftnint * innum
Definition: qld.cpp:215
void CoM(const com_t &CoM)
Definition: intermediate-qp-matrices.hh:125
double GetHeightOfCoM() const
Definition: PreviewControl.cpp:97
static integer ir
Definition: qld.cpp:670
support_state_t SupportState
Current support state.
Definition: intermediate-qp-matrices.hh:79
~StepOverPolynomeFootXtoTime()
Definition: StepOverPolynome.cpp:199
double StartTime
Definition: pgtypes.hh:192
int EvaluateMultiBodyZMP(int StartingIteration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:448
static integer inw2
Definition: qld.cpp:412
double m_Sensitivity
Definition: AnalyticalZMPCOGTrajectory.hh:264
double m_R
Definition: OptimalControllerSolver.hh:198
ftnint * inex
Definition: qld.cpp:213
VOID(* H_fp)()
Definition: qld.cpp:299
#define MAL_VECTOR_TYPE(type)
VOID(* C_fp)()
Definition: qld.cpp:295
double FluctuationMaximal()
Returns the maximal fluctuation for the first segment of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:458
struct rigid_body_state_s & operator=(const rigid_body_state_s &RB)
Definition: rigid-body.cpp:152
flag icierr
Definition: qld.cpp:173
Linear constraints with variable feet placement.
Definition: pgtypes.hh:180
doublereal E_f
Definition: qld.cpp:306
void set_inequalities(convex_hull_t &ConvexHull, const support_state_t &Support, ineq_e type)
Adapt inequalities to the support foot and its orientation.
Definition: relative-feet-inequalities.cpp:238
int StepNumber
Definition: pgtypes.hh:184
doublereal d__4
Definition: qld.cpp:646
static doublereal sumb
Definition: qld.cpp:653
int BackwardSubstitution()
Compute v2 q (14b) in Dimitrov 2009. Second phase a Now solving LL^t v2 = v1 <-> L y = v1 with L^t v2...
Definition: PLDPSolver.cpp:367
boost_ublas::vector< double > Vc_fY
Definition: intermediate-qp-matrices.hh:75
AnalyticalZMPCOGTrajectory(int lNbOfIntervals=0)
Definition: AnalyticalZMPCOGTrajectory.cpp:38
#define MAL_VECTOR_FILL(name, value)
integer(* I_fp)()
Definition: qld.cpp:292
void WriteCurrentZMPSolution(string filename, double *XkYk)
Definition: PLDPSolver.cpp:1039
#define MAL_VECTOR_DIM(name, type, nb_rows)
static integer iza
Definition: qld.cpp:675
goto L800
Definition: qld.cpp:1245
std::vector< double > m_RefTime
Definition: AnalyticalZMPCOGTrajectory.hh:246
bool GeneralizedSchur(MAL_MATRIX(&A, double), MAL_MATRIX(&B, double), MAL_VECTOR(&alphar, double), MAL_VECTOR(&alphai, double), MAL_VECTOR(&beta, double), MAL_MATRIX(&L, double), MAL_MATRIX(&R, double))
Definition: OptimalControllerSolver.cpp:133
~StepOverSpline()
Definition: StepOverPolynome.cpp:394
int ComputeInitialSolution(std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > &QueueOfLConstraintInequalitiesFreeFeet, std::deque< PatternGeneratorJRL::SupportFeet_t > &QueueOfSupportFeet, unsigned int NumberSteps, double *XkYk)
Definition: PLDPHerdt.cpp:295
double roll
Definition: pgtypes.hh:58
doublereal * xu
Definition: qld.cpp:388
goto L70
Definition: qld.cpp:918
void GetParametersWithInitPosInitSpeed(double &FT, double &FP, double &InitPos, double &InitSpeed)
Definition: PolynomeFoot.cpp:81
bool GetFromListOfZMPPolynomials(unsigned int j, Polynome *&aPoly) const
Definition: AnalyticalZMPCOGTrajectory.cpp:374
#define WALK_GEN_JRL_EXPORT
Definition: pgtypes.hh:41
double x
Definition: pgtypes.hh:192
a_offset
Definition: qld.cpp:428
void SetNumberOfIntervals(unsigned int lNbOfIntervals)
Set the number of Intervals for this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:71
static doublereal ratio
Definition: qld.cpp:663
ftnlen inacclen
Definition: qld.cpp:220
std::ostream & operator<<(std::ostream &o, const IntermedQPMat::objective_variant_s &r)
Definition: intermediate-qp-matrices.cpp:191
doublereal * xl
Definition: qld.cpp:388
int Setup(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:491
double theta
Definition: pgtypes.hh:143
boost_ublas::matrix< double, boost_ublas::row_major > S
State matrix.
Definition: rigid-body.hh:75
flag iciend
Definition: qld.cpp:175
virtual void getSoleSize(double &outLength, double &outWidth) const=0
void StoreCurrentZMPSolution(double *XkYk)
Definition: PLDPSolver.cpp:1010
double Mass() const
Definition: rigid-body-system.hh:140
~PLDPSolver()
Destructor.
Definition: PLDPSolver.cpp:159
static integer ifinc
Definition: qld.cpp:660
~AnalyticalZMPCOGTrajectory()
Definition: AnalyticalZMPCOGTrajectory.cpp:46
static integer j
Definition: qld.cpp:407
PLDPSolverHerdt(unsigned int CardU, double *iPu, double *Px, double *Pu, double *iLQ)
Constructor.
Definition: PLDPHerdt.cpp:56
Definition: LinearizedInvertedPendulum2D.hh:44
std::vector< double > m_W
Definition: AnalyticalZMPCOGTrajectory.hh:236
Class for computing trajectories.
Definition: Polynome.hh:43
static doublereal xmagr
Definition: qld.cpp:663
static integer ix
Definition: qld.cpp:670
RigidBodySystem(SimplePluginManager *SPM, CjrlHumanoidDynamicRobot *aHS, SupportFSM *FSM)
Definition: rigid-body-system.cpp:31
static integer idiag
Definition: qld.cpp:407
#define MAL_MATRIX_SET_IDENTITY(matrix)
std::deque< support_state_t > & SupportTrajectory()
Definition: rigid-body-system.hh:155
static doublereal onha
Definition: qld.cpp:653
std::vector< double > m_Coefficients
Vector of coefficients.
Definition: Polynome.hh:82
void GetCoefficients(std::vector< double > &lCoefficients) const
Definition: Polynome.cpp:77
void SetParameters(double FT, double MP)
Set the parameters.
Definition: PolynomeFoot.cpp:247
void GetParametersWithInitPosInitSpeed(double &FT, double &FP, double &InitPos, double &InitSpeed)
Definition: PolynomeFoot.cpp:215
int ForwardSubstitution()
Forward substitution. First Phase EE^t v2 = v1 <-> LL^t v2 = v1 Now solving L y = v1.
Definition: PLDPSolver.cpp:342
std::vector< double > m_V
Definition: AnalyticalZMPCOGTrajectory.hh:232
static integer ia
Definition: qld.cpp:668
const RigidBody & RightFoot() const
Definition: rigid-body-system.hh:118
std::vector< double > m_DeltaTj
Definition: AnalyticalZMPCOGTrajectory.hh:239
#define MAL_RET_A_by_B(A, B)
char * inform
Definition: qld.cpp:227
double dYaw
Definition: pgtypes.hh:209
static const unsigned int MODE_WITH_INITIALPOS
Definition: OptimalControllerSolver.hh:144
const rigid_body_state_t & State() const
Definition: rigid-body.hh:156
void reset()
Definition: rigid-body.cpp:171
void print() const
Definition: Polynome.cpp:88
static doublereal sumy
Definition: qld.cpp:655
char * inblank
Definition: qld.cpp:233
boost_ublas::vector< double > VcX
Selection matrix multiplied with the current foot position.
Definition: intermediate-qp-matrices.hh:71
Vardesc ** vars
Definition: qld.cpp:262
flag oerr
Definition: qld.cpp:183
const objective_variant_t & Objective(objective_e type) const
Definition: intermediate-qp-matrices.cpp:54
ftnint icirlen
Definition: qld.cpp:177
double pitch
Definition: pgtypes.hh:57
goto L170
Definition: qld.cpp:891
void SetParameters(MAL_VECTOR(Points, double), MAL_VECTOR(TimePoints, double), MAL_VECTOR(DerivativeEndPoints, double))
Definition: StepOverPolynome.cpp:408
double col
Definition: ConvexHull.hh:41
logical sb02ox(double *_alphar, double *_alphai, double *_beta)
Definition: OptimalControllerSolver.cpp:55
double z[3]
Definition: pgtypes.hh:80
doublereal(*)(* E_fp)()
Definition: qld.cpp:294
void GetPolynomialDegrees(std::vector< unsigned int > &lPolynomialDegree) const
Get the degree of each polynomials for the CoG.
Definition: AnalyticalZMPCOGTrajectory.cpp:338
void DSPeriod(const double DSPeriod)
Definition: SupportFSM.hh:78
int m_NbOfIntervals
Definition: AnalyticalZMPCOGTrajectory.hh:228
double m_DSSSDuration
Definition: SupportFSM_backup.hh:52
goto L860
Definition: qld.cpp:1482
const state_variant_t & State() const
Definition: intermediate-qp-matrices.hh:112
PatternGeneratorInterface(CjrlHumanoidDynamicRobot *)
Definition: patterngeneratorinterface.hh:63
char * inunf
Definition: qld.cpp:229
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:315
int ComputeInitialSolution(double *ZMPRef, double *XkYk, bool StartingSequence)
Definition: PLDPSolver.cpp:287
boost_ublas::matrix< double > V
Selection matrix for the previewed feet positions.
Definition: intermediate-qp-matrices.hh:65
static integer ira
Definition: qld.cpp:673
goto L880
Definition: qld.cpp:2025
linear_dynamics_s linear_dynamics_t
Definition: rigid-body.hh:86
static integer id
Definition: qld.cpp:668
VOID Z_f
Definition: qld.cpp:305
double x
Definition: pgtypes.hh:143
void SetComHeight(const double &)
Definition: LinearizedInvertedPendulum2D.cpp:68
void SetParameters(MAL_VECTOR(Points, double))
Definition: StepOverPolynome.cpp:273
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
doublereal * u
Definition: qld.cpp:388
static integer knext
Definition: qld.cpp:666
boost_ublas::matrix< double, boost_ublas::row_major > U
Control matrix.
Definition: rigid-body.hh:66
int m_Degree
Degree of the polynome.
Definition: Polynome.hh:79
static integer k1
Definition: qld.cpp:666
long int logical
Definition: OptimalControllerSolver.cpp:40
void CallToComAndFootRealization(COMState &acomp, FootAbsolutePosition &aLeftFAP, FootAbsolutePosition &aRightFAP, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration, int IterationNumber, int StageOfTheAlgorithm)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:110
int generate_trajectories(double time, const solution_t &Result, const std::deque< support_state_t > &SupportStates_deq, const std::deque< double > &PreviewedSupportAngles_deq, std::deque< FootAbsolutePosition > &LeftFootTraj_deq, std::deque< FootAbsolutePosition > &RightFootTraj_deq)
Generate final trajectories.
Definition: rigid-body-system.cpp:869
#define MAL_S3_VECTOR(name, type)
static integer ii
Definition: qld.cpp:670
void set_support_state(double time, unsigned int pi, support_state_t &Support, const reference_t &Ref) const
Initialize the previewed state.
Definition: SupportFSM.cpp:94
static doublereal parinc
Definition: qld.cpp:672
#define MAL_MATRIX_NB_ROWS(name)
double domega2
Definition: pgtypes.hh:145
void SetAbsoluteTimeReference(double anAbsoluteTimeReference)
Set Absolute time reference of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.hh:215
PreviewControl(SimplePluginManager *lSPM, unsigned int defaultMode=OptimalControllerSolver::MODE_WITH_INITIALPOS, bool computeWeightsAutomatically=false)
Definition: PreviewControl.cpp:38
#define MAL_MATRIX_RET_DETERMINANT(name, type)
goto L710
Definition: qld.cpp:1420
double SStime
Definition: pgtypes.hh:103
int GetStrategyForStageActivation()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:777
static doublereal ten
Definition: qld.cpp:410
WALK_GEN_JRL_EXPORT PatternGeneratorInterface * patternGeneratorInterfaceFactory(CjrlHumanoidDynamicRobot *aHDR)
reference_t & Reference()
Definition: intermediate-qp-matrices.hh:135
void UpdateTheZMPRefQueue(ZMPPosition NewZMPRefPos)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:753
#define MAL_VECTOR_SIZE(name)
const double & SamplingPeriodSim() const
Definition: rigid-body.hh:131
char * indir
Definition: qld.cpp:223
Structure to store the absolute foot position.
Definition: pgtypes.hh:140
void print()
Definition: StepOverPolynome.cpp:376
if m
Definition: qld.cpp:466
support_state_t & SupportState()
Definition: intermediate-qp-matrices.hh:142
int integer
Definition: OptimalControllerSolver.cpp:43
void NbStepsSSDS(const unsigned NbStepsSSDS)
Definition: SupportFSM.hh:88
ftnint * innrec
Definition: qld.cpp:232
virtual void rightFoot(CjrlFoot *inRightFoot)=0
int * meq
Definition: qld.cpp:634
virtual bool computeForwardKinematics()=0
doublereal dlapy2_(doublereal *, doublereal *)
static integer k
Definition: qld.cpp:656
int PrecomputeiPuPx(unsigned int NumberSteps)
Definition: PLDPHerdt.cpp:211
void SetCoGHyperbolicCoefficients(std::vector< double > &lV, std::vector< double > &lW)
Set the coefficients for the sinuse and cosinues function.
Definition: AnalyticalZMPCOGTrajectory.cpp:284
static integer iwd
Definition: qld.cpp:675
void FreePolynomes()
Definition: AnalyticalZMPCOGTrajectory.cpp:51
int nvars
Definition: qld.cpp:263
const com_t operator()() const
Accessor.
Definition: LinearizedInvertedPendulum2D.hh:145
int * n
Definition: qld.cpp:387
virtual const vector3d & zeroMomentumPoint() const=0
bool ComputeZMPSpeed(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:236
static doublereal diag
Definition: qld.cpp:394
int initialize()
Initialize.
Definition: rigid-body.cpp:48
StepOverClampedCubicSpline()
Definition: StepOverPolynome.cpp:403
double dx
Definition: pgtypes.hh:145
std::ostream & operator<<(std::ostream &os, const COMPosition_s &aCp)
Definition: pgtypes.hh:64
static integer iterc
Definition: qld.cpp:662
#define MAL_MATRIX_TYPE(type)
virtual bool getProperty(const std::string &, std::string &) const
printf("info on inconsistent constraint\n")
#define MAL_MATRIX_NB_COLS(name)
real r
Definition: qld.cpp:136
logical(* L_fp)(...)
Definition: OptimalControllerSolver.cpp:42
doublereal * g
Definition: qld.cpp:636
virtual void leftFoot(CjrlFoot *inLeftFoot)=0
ftnlen innamlen
Definition: qld.cpp:218
ftnlen ofnmlen
Definition: qld.cpp:186
void Mass(double Mass)
Definition: rigid-body.hh:148
double y
Definition: pgtypes.hh:209
boost_ublas::matrix< double > V_f
Selection matrix for relative feet positions.
Definition: intermediate-qp-matrices.hh:77
shortint(* J_fp)()
Definition: qld.cpp:291
ftnlen indirlen
Definition: qld.cpp:224
Finite state machine to determine the support parameters.
Definition: SupportFSM.hh:38
static integer iwy
Definition: qld.cpp:679
doublereal * war
Definition: qld.cpp:390
void GetStartingPointAndHeightVariation(std::vector< double > &lTj, std::vector< double > &lomegaj)
Get the starting point and the height variation.
Definition: AnalyticalZMPCOGTrajectory.cpp:355
Structure to store each foot position when the user is specifying a sequence of relative positions.
Definition: pgtypes.hh:100
boost_ublas::matrix< double > Um1
Inverse of control matrix.
Definition: rigid-body.hh:69
double theta
Definition: pgtypes.hh:123
void SetParametersWithInitPosInitSpeed(double FT, double FP, double InitPos, double InitSpeed)
Definition: PolynomeFoot.cpp:197
double omega
Definition: pgtypes.hh:143
int SecondStageOfControl(COMState &refandfinal)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:317
i__1
Definition: qld.cpp:470
static integer inw1
Definition: qld.cpp:412
State vectors.
Definition: rigid-body.hh:38
Polynome(int Degree)
Definition: Polynome.cpp:32
Polynome used for Z trajectory.
Definition: PolynomeFoot.hh:165
Definition: patterngeneratorinterface.hh:55
integer * ifail
Definition: qld.cpp:389
#define MAL_S4x4_MATRIX(name, type)
#define abs(x)
Definition: qld.cpp:267
integer * iout
Definition: qld.cpp:389
static integer iww
Definition: qld.cpp:679
void GetParametersWithInitPosInitSpeed(double &FT, double &MP, double &InitPos, double &InitSpeed)
Definition: PolynomeFoot.cpp:147
Definition: FootConstraintsAsLinearSystem.hh:55
double sx
Definition: pgtypes.hh:102
int compute_dyn_cjerk()
Initialize dynamics of the body center Suppose a piecewise constant jerk.
Definition: rigid-body-system.cpp:361
int inject_trajectory(unsigned int Axis, boost_ublas::vector< double > Trajectory)
Decouple degree of freedom by injected trajectory.
flag aerr
Definition: qld.cpp:203
bool setComAndFootRealization(ComAndFootRealization *aCFR)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:455
ftnint ounit
Definition: qld.cpp:184
ftnlen inseqlen
Definition: qld.cpp:222
static integer is
Definition: qld.cpp:670
void InitializeSolver()
Definition: PLDPSolver.cpp:136
doublereal * vsmall
Definition: qld.cpp:638
static integer mflag
Definition: qld.cpp:660
rigid_body_state_t increment_state(double Control)
Increment the state.
~SupportFSM()
Destructor.
Definition: SupportFSM.cpp:52
std::vector< Polynome * > m_ListOfCOGPolynomials
Definition: AnalyticalZMPCOGTrajectory.hh:252
~PLDPSolverHerdt()
Destructor.
Definition: PLDPHerdt.cpp:165
void SetSamplingPeriod(double lSamplingPeriod)
Setter for the sampling period.
Definition: PreviewControl.cpp:102
bool ComputeCOM(double t, double &r)
Definition: AnalyticalZMPCOGTrajectory.cpp:91
void SetPreviewControl(PreviewControl *aPC)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:102
VOID(* Z_fp)()
Definition: qld.cpp:296
void AllocateMemoryForSolver()
Definition: PLDPSolver.cpp:107
#define min(a, b)
Definition: qld.cpp:269
VOID H_f
Definition: qld.cpp:304
void SupportState(const support_state_t &SupportState)
Definition: intermediate-qp-matrices.hh:144
static integer maxit
Definition: qld.cpp:407
double GetAbsoluteTimeReference() const
Absolute Time reference of this trajectory.
Definition: AnalyticalZMPCOGTrajectory.hh:211
rigid_body_state_s()
Definition: rigid-body.cpp:143
boost_ublas::vector< double > Vc_fX
Selection matrix for the current foot position.
Definition: intermediate-qp-matrices.hh:75
~StepOverPolynomeFootZtoX()
Definition: StepOverPolynome.cpp:147
int * nmax
Definition: qld.cpp:387
double CoMHeight() const
Definition: rigid-body-system.hh:145
void clear()
Definition: rigid-body.hh:79
void SetParametersWithInitPosInitSpeed(double FT, double MP, double InitPos, double InitSpeed)
Definition: PolynomeFoot.cpp:122
int PrecomputeiPuPx()
Definition: PLDPSolver.cpp:205
~Polynome5()
Destructor.
Definition: PolynomeFoot.cpp:171
doublereal r
Definition: qld.cpp:137
virtual void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: PreviewControl.cpp:504
static integer iwwn
Definition: qld.cpp:654
char * oacc
Definition: qld.cpp:188
double GetValueSpline(MAL_VECTOR(TimePoints, double), double CurrentLocalTime)
Definition: StepOverPolynome.cpp:325
ftnint aunit
Definition: qld.cpp:204
double domega
Definition: pgtypes.hh:145
Class to implement the preview control.
Definition: PreviewControl.hh:53
~ComputeConvexHull()
Definition: ConvexHull.cpp:82
void print()
Definition: StepOverPolynome.cpp:526
static integer ip
Definition: qld.cpp:670
Polynome used for Z trajectory.
Definition: PolynomeFoot.hh:86
ZMPPreviewControlWithMultiBodyZMP(SimplePluginManager *lSPM)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:46
double time
Definition: pgtypes.hh:149
void setState(MAL_VECTOR_TYPE(double) lxk)
Definition: LinearizedInvertedPendulum2D.cpp:117
doublereal * x
Definition: qld.cpp:388
void SetParameters(double FT, double MP)
Set the parameters.
Definition: PolynomeFoot.cpp:100
double ComputeAlpha(vector< unsigned int > &NewActivatedConstraints, unsigned int NumberSteps)
Definition: PLDPHerdt.cpp:576
void Reference(const reference_t &Ref)
Definition: intermediate-qp-matrices.hh:137
ostream & operator<<(ostream &os, const AnalyticalZMPCOGTrajectory &obj)
Definition: AnalyticalZMPCOGTrajectory.cpp:509
void SetParameters(MAL_VECTOR(Xbound, double), std::vector< double > timedistr)
Definition: StepOverPolynome.cpp:158
Definition: PLDPSolver.hh:38
void setSupportState(const double &Time, const int &pi, SupportState_t &Support, const ReferenceAbsoluteVelocity &RefVel)
Initialize the previewed state.
char * name
Definition: qld.cpp:261
real(* R_fp)()
Definition: qld.cpp:293
int BuildLinearConstraintInequalities(std::deque< FootAbsolutePosition > &LeftFootAbsolutePositions, std::deque< FootAbsolutePosition > &RightFootAbsolutePositions, std::deque< LinearConstraintInequality_t * > &QueueOfLConstraintInequalities, double ConstraintOnX, double ConstraintOnY)
Definition: FootConstraintsAsLinearSystem.cpp:258
Definition: StepOverPolynome.hh:152
double ddtheta
Definition: pgtypes.hh:147
Definition: PLDPSolverHerdt.hh:45
StepOverPolynomeFootXtoTime()
Definition: StepOverPolynome.cpp:153
void GetK(MAL_MATRIX(&LK, double))
Definition: OptimalControllerSolver.cpp:365
CjrlHumanoidDynamicRobot * getHumanoidDynamicRobot() const
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:499
char * ofnm
Definition: qld.cpp:185
void SetParameters(MAL_VECTOR(boundCond, double), std::vector< double > timeDistr)
Definition: StepOverPolynome.cpp:45
RigidBody & LeftFoot()
Definition: rigid-body-system.hh:113
void dump_objective(objective_e type, std::ostream &aos)
Definition: intermediate-qp-matrices.cpp:123
logical sb02ow(double *_alphar, double *, double *_beta)
Definition: OptimalControllerSolver.cpp:86
RigidBody & RightFoot()
Definition: rigid-body-system.hh:120
#define MAL_S4x4_MATRIX_ACCESS_I_J(name, i, j)
void initialize()
Initialize.
Definition: rigid-body-system.cpp:55
double SamplingPeriod() const
Definition: SupportFSM.hh:91
Structure to store the absolute reference.
Definition: pgtypes.hh:206
void Building3rdOrderPolynomial(unsigned int anIntervalj, double pjTjm1, double pjTj)
Definition: AnalyticalZMPCOGTrajectory.cpp:425
void CreateExtraCOMBuffer(deque< COMState > &ExtraCOMBuffer, deque< ZMPPosition > &ExtraZMPBuffer, deque< ZMPPosition > &ExtraZMPRefBuffer)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:665
const reference_t & Reference() const
Definition: intermediate-qp-matrices.hh:133
int stepType
Definition: pgtypes.hh:154
static integer info
Definition: qld.cpp:405
void compute_linear_system(convex_hull_t &ConvexHull, const support_state_t &PrwSupport) const
Compute the linear inequalities associated with the convex hull specified by a vector of points.
Definition: relative-feet-inequalities.cpp:265
void DistanceCHRep(CH_Point &s1, CH_Point &s2, double &distance1, double &distance2)
Definition: ConvexHull.cpp:52
Definition: rigid-body.hh:90
shortint h
Definition: qld.cpp:240
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:78
float real
Definition: qld.cpp:134
void NbSamplingsPreviewed(unsigned N)
Definition: rigid-body.hh:143
~RelativeFeetInequalities()
Definition: relative-feet-inequalities.cpp:84
Polynome used for Z trajectory during stepover.
Definition: StepOverPolynome.hh:67
Custom (value based) container providing intermediate elements for the construction of a QP.
Definition: intermediate-qp-matrices.hh:42
double DSSSPeriod() const
Definition: SupportFSM.hh:81
void SamplingPeriodAct(double Ta)
Definition: rigid-body-system.hh:132
int OneIterationOfPreview1D(MAL_MATRIX(&x, double), double &sxzmp, deque< double > &ZMPPositions, unsigned int lindex, double &zmpx2, bool Simulation)
One iteration of the preview control along one axis (using queues)
int m_Nl
Definition: OptimalControllerSolver.hh:206
static integer iwz
Definition: qld.cpp:681
doublereal * d
Definition: qld.cpp:388
static doublereal tempa
Definition: qld.cpp:661
~RigidBody()
Definition: rigid-body.cpp:42
std::vector< Polynome * > m_ListOfZMPPolynomials
Definition: AnalyticalZMPCOGTrajectory.hh:255
static integer iwr
Definition: qld.cpp:677
void Mass(double Mass)
Definition: rigid-body-system.hh:142
static doublereal sumc
Definition: qld.cpp:653
double z
Definition: pgtypes.hh:209
static const int ZMPCOM_TRAJECTORY_FULL
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:194
double SamplingPeriod() const
Getter for the sampling period.
Definition: PreviewControl.cpp:86
char * inname
Definition: qld.cpp:217
short int shortlogical
Definition: qld.cpp:139
double ddy
Definition: pgtypes.hh:147
void setConstraints(double OnX, double OnY, double DSFeetDistance)
Definition: FootHalfSize.cpp:76
virtual const vector3d & positionCenterOfMass() const=0
#define MAL_C_eq_A_by_B(C, A, B)
char * csta
Definition: qld.cpp:198
double SamplingPeriodSim() const
Definition: rigid-body-system.hh:125
doublereal d__2
Definition: qld.cpp:646
int ComputeProjectedDescentDirection()
Compute Projected descent direction.
Definition: PLDPSolver.cpp:404
static logical lower
Definition: qld.cpp:665
~FootConstraintsAsLinearSystem()
Definition: FootConstraintsAsLinearSystem.cpp:51
virtual bool currentVelocity(const vectorN &inVelocity)=0
doublereal * w
Definition: qld.cpp:640
linear_dynamics_t & DynamicsCoPJerk()
Definition: rigid-body-system.hh:103
ftnint * innamed
Definition: qld.cpp:216
short int shortint
Definition: qld.cpp:133
void SamplingPeriodAct(double Ta)
Definition: rigid-body.hh:138
int EvaluateStartingCoM(MAL_MATRIX(&, double) BodyAngles, MAL_S3_VECTOR(&, double) aStartingCOMState, MAL_VECTOR(&, double) aWaistPose, FootAbsolutePosition &InitLeftFootPosition, FootAbsolutePosition &InitRightFootPosition)
flag ciend
Definition: qld.cpp:166
ftnlen inblanklen
Definition: qld.cpp:234
Polynome3(double FT, double FP)
Constructor: FT: Final time FP: Final position.
Definition: PolynomeFoot.cpp:36
Polynome6(double FT, double MP)
Constructor: FT: Final time MP: Middle position.
Definition: PolynomeFoot.cpp:242
complex c
Definition: qld.cpp:244
const support_state_t & SupportState() const
Definition: intermediate-qp-matrices.hh:140
double Compute(double t)
Definition: Polynome.cpp:44
double GetValueSpline(MAL_VECTOR(TimePoints, double), double CurrentLocalTime)
Definition: StepOverPolynome.cpp:475
ftnint orl
Definition: qld.cpp:190
int SetupIterativePhase(deque< ZMPPosition > &ZMPRefPositions, deque< COMState > &COMStates, deque< FootAbsolutePosition > &LeftFootPositions, deque< FootAbsolutePosition > &RightFootPositions, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration, int localindex)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:611
g_dim1
Definition: qld.cpp:723
void ReadPrecomputedFile(string aFileName)
Read the file of parameters aFileName and set the sampling period, the preview control time,...
Definition: PreviewControl.cpp:142
void SetParametersWithInitPosInitSpeed(double &FT, double &FP, double &InitPos, double &InitSpeed)
Definition: PolynomeFoot.cpp:59
void StepPeriod(const double StepPeriod)
Definition: SupportFSM.hh:73
char * oblnk
Definition: qld.cpp:191
double theta
Definition: pgtypes.hh:192
#define MAL_MATRIX(name, type)
const linear_dynamics_t & DynamicsCoPJerk() const
Definition: rigid-body-system.hh:101
double SamplingPeriodAct() const
Definition: rigid-body-system.hh:130
int SolveProblem(double *CstPartOfTheCostFunction, unsigned int NbOfConstraints, double *LinearPartOfConstraints, double *CstPartOfConstraints, double *ZMPRef, double *XkYk, double *X, std::vector< int > &SimilarConstraint, unsigned int NumberOfRemovedConstraints, bool StartingSequence)
Solve the optimization problem.
Definition: PLDPSolver.cpp:654
long ftnlen
Definition: qld.cpp:158
static integer jfinc
Definition: qld.cpp:660
void SetParameters(MAL_VECTOR(boundCond, double), std::vector< double > timeDistr)
Definition: StepOverPolynome.cpp:212
double px
Definition: pgtypes.hh:122
com_t OneIteration(double CX, double CY)
Simulate one iteration of the LIPM.
Definition: LinearizedInvertedPendulum2D.cpp:230
This class computes the gains for preview control for a given discrete system. The discrete system is...
Definition: OptimalControllerSolver.hh:139
void SetStrategyForStageActivation(int anAlgo)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:758
Polynome used for X,Y and Theta trajectories.
Definition: PolynomeFoot.hh:48
goto L775
Definition: qld.cpp:1599
bool invertMatrix(const boost_ublas::matrix< T > &input, boost_ublas::matrix< T > &inverse)
Definition: rigid-body-system.cpp:268
double x[3]
Definition: pgtypes.hh:54
static integer kdrop
Definition: qld.cpp:664
char * infile
Definition: qld.cpp:211
struct LinearConstraintInequality_s LinearConstraintInequality_t
Definition: pgtypes.hh:176
goto L740
Definition: qld.cpp:1210
c_dim1
Definition: qld.cpp:431
const RigidBody & CoM() const
Definition: rigid-body-system.hh:106
StepOverPolynomeHip4()
Definition: StepOverPolynome.cpp:207
Definition: ConvexHull.cpp:38
int ComputeLinearSystem(std::vector< CH_Point > aVecOfPoints, MAL_MATRIX(&A, double), MAL_MATRIX(&B, double), MAL_VECTOR(&C, double))
Definition: FootConstraintsAsLinearSystem.cpp:97
std::vector< double > m_omegaj
Definition: AnalyticalZMPCOGTrajectory.hh:243
MAL_MATRIX(Coefficients, double)
boost_ublas::vector< double > VcshiftX
Shifted selection matrix multiplied with the current feet position.
Definition: intermediate-qp-matrices.hh:73
void dump_state(std::ostream &aos)
Definition: intermediate-qp-matrices.cpp:145
static logical lql
Definition: qld.cpp:411
double roll[3]
Definition: pgtypes.hh:83
unsigned int number
Definition: StepOverPolynome.hh:171
#define MAL_RET_TRANSPOSE(matrix)
char * ofm
Definition: qld.cpp:189
reference_t Ref
Objective independent QP elements.
Definition: intermediate-qp-matrices.hh:56
int FirstStageOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:378
void RightFoot(const RigidBody &RightFoot)
Definition: rigid-body-system.hh:122
boost_ublas::matrix< double > UT
Transpose of control matrix.
Definition: rigid-body.hh:72
goto L640
Definition: qld.cpp:1532
const com_t & CoM() const
Definition: intermediate-qp-matrices.hh:123
boost_ublas::matrix< double > VT
Transpose of V.
Definition: intermediate-qp-matrices.hh:69
int update(const std::deque< support_state_t > &SupportStates_deq, const std::deque< FootAbsolutePosition > &LeftFootTraj_deq, const std::deque< FootAbsolutePosition > &RightFootTraj_deq)
Update feet matrices.
Definition: rigid-body-system.cpp:228
void SetParameters(double FT, double FP)
Definition: PolynomeFoot.cpp:41
void GetHyperbolicCoefficients(std::vector< double > &lV, std::vector< double > &lW) const
Get the coefficients for the sinuse and cosinues function.
Definition: AnalyticalZMPCOGTrajectory.cpp:348
double ddz
Definition: pgtypes.hh:147
double ddomega
Definition: pgtypes.hh:147
long ftnint
Definition: qld.cpp:159
flag inerr
Definition: qld.cpp:209
int InitializeSystem()
Initialize the system.
Definition: LinearizedInvertedPendulum2D.cpp:123
int BuildLinearConstraintInequalities2(std::deque< FootAbsolutePosition > &LeftFootAbsolutePositions, std::deque< FootAbsolutePosition > &RightFootAbsolutePositions, std::deque< LinearConstraintInequality_t * > &QueueOfLConstraintInequalities, double ConstraintOnX, double ConstraintOnY)
static integer iu
Definition: qld.cpp:670
unsigned NBStepsSSDS() const
Definition: SupportFSM.hh:86
boost_ublas::vector< double > VcshiftY
Definition: intermediate-qp-matrices.hh:73
void SetRobotControlPeriod(const double &)
Definition: LinearizedInvertedPendulum2D.cpp:94
long int logical
Definition: qld.cpp:138
const double & SamplingPeriodAct() const
Definition: rigid-body.hh:136
static doublereal qpeps
Definition: qld.cpp:408
goto L549
Definition: qld.cpp:1432
ftnint cunit
Definition: qld.cpp:197
const double & GetRobotControlPeriod()
Definition: LinearizedInvertedPendulum2D.cpp:89
static integer itref
Definition: qld.cpp:662
static doublereal suma
Definition: qld.cpp:653
double y[3]
Definition: pgtypes.hh:80
const linear_dynamics_t & Dynamics(dynamics_e) const
Definition: rigid-body.cpp:91
char * inacc
Definition: qld.cpp:219
char * addr
Definition: qld.cpp:254
RigidBody()
Definition: rigid-body.cpp:30
~Polynome3()
Destructor.
Definition: PolynomeFoot.cpp:92
#define MAL_VECTOR_RESIZE(name, nb_rows)
static doublereal fdiff
Definition: qld.cpp:657
int(* S_fp)()
Definition: qld.cpp:300
#define TRUE_
Definition: qld.cpp:141
long Long
Definition: qld.cpp:250
static integer iz
Definition: qld.cpp:670
static doublereal zero
Definition: qld.cpp:406
static integer i
Definition: qld.cpp:407
void CallMethod(std::string &Method, std::istringstream &astrm)
Overloading method of SimplePlugin.
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:882
bool setHumanoidDynamicRobot(CjrlHumanoidDynamicRobot *aHumanoidDynamicRobot)
Definition: ZMPPreviewControlWithMultiBodyZMP.hh:493
~PreviewControl()
Definition: PreviewControl.cpp:81
int interpolate(solution_t Result, std::deque< ZMPPosition > &FinalZMPTraj_deq, std::deque< COMState > &FinalCOMTraj_deq, std::deque< FootAbsolutePosition > &FinalLeftFootTraj_deq, std::deque< FootAbsolutePosition > &FinalRightFootTraj_deq)
Interpolate.
static doublereal parnew
Definition: qld.cpp:672
void NbSamplingsPreviewed(unsigned N)
Definition: rigid-body-system.hh:137
std::deque< rigid_body_state_t > & Trajectory()
Definition: rigid-body.hh:151
static integer ju
Definition: qld.cpp:670
virtual void CallMethod(std::string &Method, std::istringstream &Args)
Reimplement the interface of SimplePluginManager.
Definition: relative-feet-inequalities.cpp:323
double ComputeDerivative(double t)
Definition: Polynome.cpp:55
struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity
Definition: pgtypes.hh:216
~RigidBodySystem()
Definition: rigid-body-system.cpp:45
Definition: PLDPSolver.hh:40
int SolveProblem(std::deque< PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_t > &QueueOfLConstraintInequalitiesFreeFeet, std::deque< PatternGeneratorJRL::SupportFeet_t > &QueueOfSupportFeet, double *CstPartOfTheCostFunction, unsigned int NbOfConstraints, double *LinearPartOfConstraints, double *CstPartOfConstraints, double *XkYk, double *X, unsigned int NumberOfRemovedConstraints, unsigned int NbRemovedFootCstr, bool StartingSequence, unsigned int NumberSteps, bool CurrentStateChanged, double time)
Solve the optimization problem.
Definition: PLDPHerdt.cpp:693
int type
Definition: qld.cpp:256
char * osta
Definition: qld.cpp:187
int stepType
Definition: pgtypes.hh:125
double dz
Definition: pgtypes.hh:145
static integer iwx
Definition: qld.cpp:679
dynamics_e Type
Definition: rigid-body.hh:77
void set_vertices(convex_hull_t &ConvexHull, const support_state_t &SupportState, ineq_e type)
Adapt vertices to the support foot and its orientation.
Definition: relative-feet-inequalities.cpp:186
int OneGlobalStepOfControl(FootAbsolutePosition &LeftFootPosition, FootAbsolutePosition &RightFootPosition, ZMPPosition &NewZMPRefPos, COMState &finalCOMState, MAL_VECTOR_TYPE(double) &CurrentConfiguration, MAL_VECTOR_TYPE(double) &CurrentVelocity, MAL_VECTOR_TYPE(double) &CurrentAcceleration)
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:196
OptimalControllerSolver(MAL_MATRIX(&A, double), MAL_MATRIX(&b, double), MAL_MATRIX(&c, double), double Q, double R, unsigned int Nl)
Definition: OptimalControllerSolver.cpp:98
int OneIterationOfPreview(MAL_MATRIX(&x, double), MAL_MATRIX(&y, double), double &sxzmp, double &syzmp, deque< PatternGeneratorJRL::ZMPPosition > &ZMPPositions, unsigned int lindex, double &zmpx2, double &zmpy2, bool Simulation)
One iteration of the preview control.
Definition: PreviewControl.cpp:324
goto L770
Definition: qld.cpp:1828
unsigned NbSamplingsPreviewed() const
Definition: rigid-body-system.hh:135
void GetF(MAL_MATRIX(&LF, double))
Definition: OptimalControllerSolver.cpp:360
Definition: pgtypes.hh:168
void GetNumberOfIntervals(unsigned int &lNbOfIntervals) const
Set the number of Intervals for this trajectory.
Definition: AnalyticalZMPCOGTrajectory.cpp:343
void InitializeSolver(unsigned int NumberSteps)
Definition: PLDPHerdt.cpp:142
double yaw
Definition: pgtypes.hh:56
boost_ublas::vector< double > Roll
Definition: rigid-body.hh:49
static integer nm
Definition: qld.cpp:670
double ddx
Definition: pgtypes.hh:147
doublereal * eps1
Definition: qld.cpp:392
~Polynome4()
Destructor.
Definition: PolynomeFoot.cpp:157
ftnint * inopen
Definition: qld.cpp:214
double m_AbsoluteTimeReference
Definition: AnalyticalZMPCOGTrajectory.hh:261
const unsigned & NbSamplingsPreviewed() const
Definition: rigid-body.hh:141
boost_ublas::matrix< double > Vshift
Shifted selection matrix for the previewed feet positions.
Definition: intermediate-qp-matrices.hh:67
void ComputeWeights(unsigned int Mode)
Definition: OptimalControllerSolver.cpp:200
void dump(const char *filename) const
Definition: intermediate-qp-matrices.cpp:182
static integer nu
Definition: qld.cpp:670
doublereal d__1
Definition: qld.cpp:643
void ComputeOptimalWeights(unsigned int mode)
Compute optimal weights.
Definition: PreviewControl.cpp:198
goto L930
Definition: qld.cpp:1177
int FindSimilarConstraints(MAL_MATRIX(&A, double), std::vector< int > &SimilarConstraints)
Definition: FootConstraintsAsLinearSystem.cpp:55
static integer jflag
Definition: qld.cpp:658
int GetStrategyForPCStages()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:834
boost_ublas::vector< double > VcY
Definition: intermediate-qp-matrices.hh:71
double z
Definition: pgtypes.hh:143
MAL_VECTOR(RefVectorX, double)
double getHalfWidth() const
Definition: FootHalfSize.cpp:91
FootConstraintsAsLinearSystem(SimplePluginManager *aSPM, CjrlHumanoidDynamicRobot *aHS)
Definition: FootConstraintsAsLinearSystem.cpp:43
a_dim1
Definition: qld.cpp:427
boost_ublas::vector< double > Pitch
Definition: rigid-body.hh:48
integer * lwar
Definition: qld.cpp:391
void SetCoefficients(const std::vector< double > &lCoefficients)
Definition: Polynome.cpp:83
doublecomplex z
Definition: qld.cpp:245
char * icifmt
Definition: qld.cpp:176
double PreviewControlTime() const
Definition: PreviewControl.cpp:91
double omega2
Definition: pgtypes.hh:143
void updateHalfSize()
Definition: FootHalfSize.cpp:62
COMState GetLastCOMFromFirstStage()
Definition: ZMPPreviewControlWithMultiBodyZMP.cpp:310
double theta
Definition: pgtypes.hh:102