Go to the documentation of this file.
8 #ifndef DYNAMICAL_SYSTEM_HPP_
9 #define DYNAMICAL_SYSTEM_HPP_
13 #include <state-observation/api.h>
20 #include <Eigen/Cholesky>
25 namespace flexibilityEstimation
40 static constexpr
unsigned posCom = 0;
41 static constexpr
unsigned velCom = 3;
42 static constexpr
unsigned accCom = 6;
43 static constexpr
unsigned inertia = 9;
44 static constexpr
unsigned angMoment = 15;
45 static constexpr
unsigned dotInertia = 18;
46 static constexpr
unsigned dotAngMoment = 24;
47 static constexpr
unsigned posIMU = 27;
48 static constexpr
unsigned oriIMU = 30;
49 static constexpr
unsigned linVelIMU = 33;
50 static constexpr
unsigned angVelIMU = 36;
51 static constexpr
unsigned linAccIMU = 39;
52 static constexpr
unsigned additionalForces = 42;
53 static constexpr
unsigned contacts = 48;
55 static constexpr
unsigned sizeBase = 48;
60 static constexpr
unsigned pos = 0;
61 static constexpr
unsigned ori = 3;
62 static constexpr
unsigned linVel = 6;
63 static constexpr
unsigned angVel = 9;
64 static constexpr
unsigned fc = 12;
65 static constexpr
unsigned unmodeledForces = 24;
66 static constexpr
unsigned comBias = 30;
67 static constexpr
unsigned drift = 32;
69 static constexpr
unsigned size = 35;
75 static constexpr
unsigned elasticContact = 1;
76 static constexpr
unsigned pendulum = 2;
77 static constexpr
unsigned none = 0;
91 void computeContactWrench(
const Matrix3 & orientation,
105 virtual void computeAccelerations(
const Vector3 & positionCom,
107 const Vector3 & accelerationCom,
109 const Vector3 & dotAngMomentum,
161 virtual void resetProcessNoise();
170 virtual void resetMeasurementNoise();
176 virtual void setSamplingPeriod(
double dt);
179 virtual Index getStateSize()
const;
182 virtual Index getInputSize()
const;
185 virtual void setInputSize(
Index i);
193 virtual Index getMeasurementSize()
const;
196 virtual void setContactsNumber(
unsigned);
209 virtual void setContactModel(
unsigned nb);
216 virtual void computeElastContactForcesAndMoments(
const IndexedVectorArray & contactPosArray,
249 virtual void computeForcesAndMoments(
const Vector & x,
const Vector & u);
251 virtual Vector getForcesAndMoments();
258 virtual void iterateDynamicsEuler(
const Vector3 & positionCom,
260 const Vector3 & accelerationCom,
262 const Vector3 & dotAngMomentum,
279 virtual void iterateDynamicsRK4(
const Vector3 & positionCom,
281 const Vector3 & accelerationCom,
283 const Vector3 & dotAngMomentum,
300 virtual void setWithForceMeasurements(
bool b);
301 virtual bool getWithForceMeasurements()
const;
302 virtual void setWithComBias(
bool b);
303 virtual bool getWithComBias()
const;
304 virtual void setWithAbsolutePosition(
bool b);
305 virtual bool getWithAbsolutePosition()
const;
306 void setWithUnmodeledForces(
bool b);
308 virtual void setKfe(
const Matrix3 &
m);
309 virtual void setKfv(
const Matrix3 &
m);
310 virtual void setKte(
const Matrix3 &
m);
311 virtual void setKtv(
const Matrix3 &
m);
313 virtual void setKfeRopes(
const Matrix3 &
m);
314 virtual void setKfvRopes(
const Matrix3 &
m);
315 virtual void setKteRopes(
const Matrix3 &
m);
316 virtual void setKtvRopes(
const Matrix3 &
m);
318 virtual Matrix getKfe()
const;
319 virtual Matrix getKfv()
const;
320 virtual Matrix getKte()
const;
321 virtual Matrix getKtv()
const;
323 virtual void setRobotMass(
double d);
325 virtual double getRobotMass()
const;
334 void updateMeasurementSize_();
343 static constexpr
Index stateSize_ = state::size;
345 static constexpr
Index measurementSizeBase_ = 6;
389 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
518 : curRotation0(
Matrix3::Identity()), orientationVector0(
Vector3::Zero()), curRotation1(
Matrix3::Identity()),
519 orientationVector1(
Vector3::Zero()), curRotation2(
Matrix3::Identity()), orientationVector2(
Vector3::Zero()),
520 curRotation3(
Matrix3::Identity()), orientationVector3(
Vector3::Zero())
526 if(i == 0)
return orientationVector0;
527 if(i == 1)
return orientationVector1;
528 if(i == 2)
return orientationVector2;
530 return orientationVector3;
535 if(i == 0)
return curRotation0;
536 if(i == 1)
return curRotation1;
537 if(i == 2)
return curRotation2;
545 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::VectorXd Vector
Dynamic sized scalar vector.
Definition: definitions.hpp:76
Matrix3 Rcit
Definition: imu-elastic-local-frame-dynamical-system.hpp:481
Implements the accelerometer-gyrometer inertial measuremen.
double sy
Definition: imu-elastic-local-frame-dynamical-system.hpp:406
Matrix3 crosstempM
Definition: imu-elastic-local-frame-dynamical-system.hpp:477
Vector3 imuOmega
Definition: imu-elastic-local-frame-dynamical-system.hpp:436
Matrix3 orinertia
Definition: imu-elastic-local-frame-dynamical-system.hpp:422
stateObservation::AccelerometerGyrometer sensor_
Definition: imu-elastic-local-frame-dynamical-system.hpp:330
IndexedVectorArray efforts
Definition: imu-elastic-local-frame-dynamical-system.hpp:461
Vector ykdy
Definition: imu-elastic-local-frame-dynamical-system.hpp:418
Vector3 & orientationVector(int i)
Definition: imu-elastic-local-frame-dynamical-system.hpp:524
Vector3 orientationControlV
Definition: imu-elastic-local-frame-dynamical-system.hpp:448
AngleAxis orientationAA
Definition: imu-elastic-local-frame-dynamical-system.hpp:408
Vector3 forcei
Definition: imu-elastic-local-frame-dynamical-system.hpp:488
Vector3 linearAcceleration
Definition: imu-elastic-local-frame-dynamical-system.hpp:470
IndexedVectorArray contactOriV
Definition: imu-elastic-local-frame-dynamical-system.hpp:454
Matrix3 inertia
Definition: imu-elastic-local-frame-dynamical-system.hpp:458
Vector3 pdrift
Definition: imu-elastic-local-frame-dynamical-system.hpp:403
Definition: imu-elastic-local-frame-dynamical-system.hpp:58
Matrix3 rimu
Definition: imu-elastic-local-frame-dynamical-system.hpp:434
Vector uk_
Definition: imu-elastic-local-frame-dynamical-system.hpp:356
Vector3 angularVelocityFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:395
Vector3 addMoment
Definition: imu-elastic-local-frame-dynamical-system.hpp:493
bool withComBias_
Definition: imu-elastic-local-frame-dynamical-system.hpp:372
AngleAxis aatotal
Definition: imu-elastic-local-frame-dynamical-system.hpp:428
Matrix3 dotInertia
Definition: imu-elastic-local-frame-dynamical-system.hpp:459
This is the base class of any functor that describes the dynamics of the state and the measurement....
Definition: dynamical-system-functor-base.hpp:32
TimeIndex kcurrent_
Definition: imu-elastic-local-frame-dynamical-system.hpp:369
Index measurementSize_
Definition: imu-elastic-local-frame-dynamical-system.hpp:362
Matrix3 skewV
Definition: imu-elastic-local-frame-dynamical-system.hpp:495
unsigned index_
Definition: imu-elastic-local-frame-dynamical-system.hpp:382
Vector3 contactVel
Definition: imu-elastic-local-frame-dynamical-system.hpp:483
Vector3 globalContactPos
Definition: imu-elastic-local-frame-dynamical-system.hpp:485
stateObservation::NoiseBase * processNoise_
Definition: imu-elastic-local-frame-dynamical-system.hpp:332
Matrix Jx
Definition: imu-elastic-local-frame-dynamical-system.hpp:431
IndexedVectorArray contactPosV
Definition: imu-elastic-local-frame-dynamical-system.hpp:453
Vector xk
Definition: imu-elastic-local-frame-dynamical-system.hpp:411
Optimization()
Definition: imu-elastic-local-frame-dynamical-system.hpp:517
unsigned nbContacts_
Definition: imu-elastic-local-frame-dynamical-system.hpp:346
Vector3 positionControl
Definition: imu-elastic-local-frame-dynamical-system.hpp:445
bool withAbsolutePos_
Definition: imu-elastic-local-frame-dynamical-system.hpp:373
double dt_
Definition: imu-elastic-local-frame-dynamical-system.hpp:336
Vector3 imuAcc
Definition: imu-elastic-local-frame-dynamical-system.hpp:435
Vector3 velocityControl
Definition: imu-elastic-local-frame-dynamical-system.hpp:446
Vector3 fm
Definition: imu-elastic-local-frame-dynamical-system.hpp:467
Definition: noise-base.hpp:28
Vector3 accelerationFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:393
Matrix3 rFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:399
Vector3 wx2Rc
Definition: imu-elastic-local-frame-dynamical-system.hpp:500
TimeIndex k_fory
Definition: imu-elastic-local-frame-dynamical-system.hpp:420
Matrix3 curRotation0
Definition: imu-elastic-local-frame-dynamical-system.hpp:508
Vector3 fi
Definition: imu-elastic-local-frame-dynamical-system.hpp:463
This class describes the dynamics of a robot's flexibility this dynamics with elastic forces to bring...
Definition: imu-elastic-local-frame-dynamical-system.hpp:34
Matrix3 rdrift
Definition: imu-elastic-local-frame-dynamical-system.hpp:404
Eigen::LLT< Matrix3 > LLTMatrix3
Definition: imu-elastic-local-frame-dynamical-system.hpp:80
Implements integrators for the kinematics, in terms or rotations and translations.
double robotMass_
Definition: imu-elastic-local-frame-dynamical-system.hpp:338
Eigen::MatrixXd Matrix
Dynamic sized Matrix.
Definition: definitions.hpp:100
std::vector< Vector3, Eigen::aligned_allocator< Vector3 > > contactPositions_
Definition: imu-elastic-local-frame-dynamical-system.hpp:364
Vector3 accelerationControl
Definition: imu-elastic-local-frame-dynamical-system.hpp:447
LLTMatrix3 invinertia
Definition: imu-elastic-local-frame-dynamical-system.hpp:424
Matrix3 Rci
Definition: imu-elastic-local-frame-dynamical-system.hpp:480
bool withForceMeasurements_
Definition: imu-elastic-local-frame-dynamical-system.hpp:371
virtual void setPrinted(bool b)
Definition: imu-elastic-local-frame-dynamical-system.hpp:211
IndexedVectorArray contactAngVelArray
Definition: imu-elastic-local-frame-dynamical-system.hpp:456
long int TimeIndex
Definition: definitions.hpp:139
Matrix3 rtotal
Definition: imu-elastic-local-frame-dynamical-system.hpp:426
Vector yk_
Definition: imu-elastic-local-frame-dynamical-system.hpp:359
Eigen::Matrix3d Matrix3
3x3 Scalar Matrix
Definition: definitions.hpp:109
Vector3 Ra
Definition: imu-elastic-local-frame-dynamical-system.hpp:502
Matrix3 rControl
Definition: imu-elastic-local-frame-dynamical-system.hpp:451
Vector3 angularAcceleration
Definition: imu-elastic-local-frame-dynamical-system.hpp:471
Matrix3 curRotation1
Definition: imu-elastic-local-frame-dynamical-system.hpp:510
stateObservation::Vector3 pe
Definition: imu-elastic-local-frame-dynamical-system.hpp:376
Vector tc_
Definition: imu-elastic-local-frame-dynamical-system.hpp:350
constexpr double m
mass of the robot
Definition: hrp2.hpp:36
Vector xk_fory_
Definition: imu-elastic-local-frame-dynamical-system.hpp:358
Vector3 velocityFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:392
Matrix3 rFlexT
Definition: imu-elastic-local-frame-dynamical-system.hpp:400
Vector yk
Definition: imu-elastic-local-frame-dynamical-system.hpp:417
Vector3 orientationVector0
Definition: imu-elastic-local-frame-dynamical-system.hpp:509
Vector3 contactPos
Definition: imu-elastic-local-frame-dynamical-system.hpp:482
Vector3 AngMomentum
Definition: imu-elastic-local-frame-dynamical-system.hpp:442
Vector3 velocityCom
Definition: imu-elastic-local-frame-dynamical-system.hpp:440
Vector3 drift
Definition: imu-elastic-local-frame-dynamical-system.hpp:402
Vector3 Rcp
Definition: imu-elastic-local-frame-dynamical-system.hpp:504
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vector3 positionFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:391
Vector xk1dx
Definition: imu-elastic-local-frame-dynamical-system.hpp:412
Vector6 momentaDot
Definition: imu-elastic-local-frame-dynamical-system.hpp:387
Eigen::Matrix< double, 6, 1 > Vector6
6D vector
Definition: definitions.hpp:97
Eigen::Index Index
Definition: definitions.hpp:138
virtual void setPe(stateObservation::Vector3 Pe)
Definition: imu-elastic-local-frame-dynamical-system.hpp:198
Matrix3 curRotation3
Definition: imu-elastic-local-frame-dynamical-system.hpp:514
Vector3 _2wxRv
Definition: imu-elastic-local-frame-dynamical-system.hpp:501
Vector fc_
Definition: imu-elastic-local-frame-dynamical-system.hpp:349
Vector3 angularAccelerationFlex
Definition: imu-elastic-local-frame-dynamical-system.hpp:396
Matrix3 Ktv_
Definition: imu-elastic-local-frame-dynamical-system.hpp:366
Vector3 orientationVector1
Definition: imu-elastic-local-frame-dynamical-system.hpp:511
Eigen::AngleAxis< double > AngleAxis
Euler Axis/Angle representation of orientation.
Definition: definitions.hpp:133
unsigned getContactsNumber(void) const
Gets the nimber of contacts.
Definition: imu-elastic-local-frame-dynamical-system.hpp:204
Vector3 orientationFlexV
Definition: imu-elastic-local-frame-dynamical-system.hpp:394
Vector3 orientationVector2
Definition: imu-elastic-local-frame-dynamical-system.hpp:513
double marginalStabilityFactor_
Definition: imu-elastic-local-frame-dynamical-system.hpp:378
Index inputSize_
Definition: imu-elastic-local-frame-dynamical-system.hpp:344
Matrix3 skewV2
Definition: imu-elastic-local-frame-dynamical-system.hpp:496
Definition: imu-elastic-local-frame-dynamical-system.hpp:384
Matrix Jy
Definition: imu-elastic-local-frame-dynamical-system.hpp:432
unsigned contactModel_
Definition: imu-elastic-local-frame-dynamical-system.hpp:347
Vector3 addForce
Definition: imu-elastic-local-frame-dynamical-system.hpp:492
Matrix3 curRotation2
Definition: imu-elastic-local-frame-dynamical-system.hpp:512
Matrix3 skewVR
Definition: imu-elastic-local-frame-dynamical-system.hpp:497
Matrix3 skewV2R
Definition: imu-elastic-local-frame-dynamical-system.hpp:498
bool withUnmodeledForces_
Definition: imu-elastic-local-frame-dynamical-system.hpp:374
Vector xk1
Definition: imu-elastic-local-frame-dynamical-system.hpp:410
Vector3 Rc
Definition: imu-elastic-local-frame-dynamical-system.hpp:503
Vector3 crosstempV
Definition: imu-elastic-local-frame-dynamical-system.hpp:476
Vector xk1_
Definition: imu-elastic-local-frame-dynamical-system.hpp:354
Eigen::Vector3d Vector3
3D vector
Definition: definitions.hpp:85
Vector3 RciContactPos
Definition: imu-elastic-local-frame-dynamical-system.hpp:484
Vector xdx
Definition: imu-elastic-local-frame-dynamical-system.hpp:414
Vector3 oritotal
Definition: imu-elastic-local-frame-dynamical-system.hpp:429
Vector3 positionComBias
Definition: imu-elastic-local-frame-dynamical-system.hpp:397
Matrix3 RIRT
Definition: imu-elastic-local-frame-dynamical-system.hpp:499
double robotMassInv_
Definition: imu-elastic-local-frame-dynamical-system.hpp:339
Vector dx_
Definition: imu-elastic-local-frame-dynamical-system.hpp:352
Vector3 dotAngMomentum
Definition: imu-elastic-local-frame-dynamical-system.hpp:443
Vector3 vt
Definition: imu-elastic-local-frame-dynamical-system.hpp:474
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
Vector3 positionCom
Definition: imu-elastic-local-frame-dynamical-system.hpp:439
Matrix3 Rt
Definition: imu-elastic-local-frame-dynamical-system.hpp:486
Vector xk_
Definition: imu-elastic-local-frame-dynamical-system.hpp:355
Matrix3 KtvRopes_
Definition: imu-elastic-local-frame-dynamical-system.hpp:367
Vector3 angularVelocityControl
Definition: imu-elastic-local-frame-dynamical-system.hpp:449
Vector3 tm
Definition: imu-elastic-local-frame-dynamical-system.hpp:468
Vector sensorState
Definition: imu-elastic-local-frame-dynamical-system.hpp:437
IndexedVectorArray contactVelArray
Definition: imu-elastic-local-frame-dynamical-system.hpp:455
Vector uk_fory_
Definition: imu-elastic-local-frame-dynamical-system.hpp:360
bool printed_
Definition: imu-elastic-local-frame-dynamical-system.hpp:328
Definitions of Hrp2 constants.
Vector3 ptotal
Definition: imu-elastic-local-frame-dynamical-system.hpp:427
Vector xk_fory
Definition: imu-elastic-local-frame-dynamical-system.hpp:416
Matrix3 & curRotation(int i)
Definition: imu-elastic-local-frame-dynamical-system.hpp:533
Vector3 vf
Definition: imu-elastic-local-frame-dynamical-system.hpp:473
Vector3 accelerationCom
Definition: imu-elastic-local-frame-dynamical-system.hpp:441
Vector3 orientationVector3
Definition: imu-elastic-local-frame-dynamical-system.hpp:515
Vector3 t
Definition: imu-elastic-local-frame-dynamical-system.hpp:464
Implements the accelerometer-gyrometer measurements.
Definition: accelerometer-gyrometer.hpp:33
Vector3 momenti
Definition: imu-elastic-local-frame-dynamical-system.hpp:489