stable-imu-fixed-contact-dynamical-system.hpp

 
#ifndef StableFIXED_CONTACTS_IMU_DYNAMICS_FUNCTOR_HPP
#define StableFIXED_CONTACTS_IMU_DYNAMICS_FUNCTOR_HPP
 
#include <vector>
 
#include <state-observation/api.h>
#include <state-observation/dynamical-system/dynamical-system-functor-base.hpp>
#include <state-observation/noise/noise-base.hpp>
#include <state-observation/sensors-simulation/accelerometer-gyrometer.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
 
namespace stateObservation
{
namespace flexibilityEstimation
{
class STATE_OBSERVATION_DLLAPI StableIMUFixedContactDynamicalSystem
: public stateObservation::DynamicalSystemFunctorBase
{
public:
  explicit StableIMUFixedContactDynamicalSystem(double dt);

  virtual ~StableIMUFixedContactDynamicalSystem();
 
  // stabilization of the acceleration linear
  virtual Vector3 stabilizeAccelerationLinear(Vector3, Vector3);
 
  // stabilization of the acceleration angular
  virtual Vector3 stabilizeAccelerationAngular(Vector3, Vector3);

  virtual stateObservation::Vector stateDynamics(const stateObservation::Vector & x,
                                                 const stateObservation::Vector & u,
                                                 TimeIndex k);

  virtual stateObservation::Vector measureDynamics(const stateObservation::Vector & x,
                                                   const stateObservation::Vector & u,
                                                   TimeIndex k);

  virtual void setProcessNoise(stateObservation::NoiseBase *);

  virtual void resetProcessNoise();

  virtual stateObservation::NoiseBase * getProcessNoise() const;

  virtual void setMeasurementNoise(stateObservation::NoiseBase *);

  virtual void resetMeasurementNoise();

  virtual stateObservation::NoiseBase * getMeasurementNoise() const;

  virtual void setSamplingPeriod(double dt);

  virtual Index getStateSize() const;
  virtual Index getInputSize() const;
  virtual Index getMeasurementSize() const;

  virtual void setContactsNumber(unsigned);

  virtual void setContactPosition(unsigned i, const Vector3 & position);
 
protected:
  typedef kine::indexes<kine::rotationVector> indexes;
 
  stateObservation::AccelerometerGyrometer sensor_;
 
  stateObservation::NoiseBase * processNoise_;
 
  double dt_;
 
  Vector3Unaligned orientationVector_;
  QuaternionUnaligned quaternion_;
 
  Quaternion computeQuaternion_(const Vector3 & x);
 
  static const Index stateSize_ = 18;
  static const Index inputSize_ = 15;
  static const Index measurementSizeBase_ = 6;
 
  Index measurementSize_;
 
  std::vector<Vector3, Eigen::aligned_allocator<Vector3>> contactPositions_;
 
private:
public:
};
} // namespace flexibilityEstimation
} // namespace stateObservation
 
#endif // FIXED-CONTACTS-IMU-DYNAMICS-FUNCTOR_HPP