tilt-estimator-humanoid.hpp

 
#ifndef TILTESTIMATORHUMANOIDHPP
#define TILTESTIMATORHUMANOIDHPP
 
#include <state-observation/api.h>
#include <state-observation/observer/tilt-estimator.hpp>
#include <state-observation/observer/zero-delay-observer.hpp>
 
namespace stateObservation
{

class STATE_OBSERVATION_DLLAPI TiltEstimatorHumanoid : public TiltEstimator
{
public:
  TiltEstimatorHumanoid(double alpha, double beta, double gamma, double dt);

  void setSensorPositionInC(const Vector3 & p)
  {
    p_S_C_ = p;
  }
 
  Vector3 getSensorPositionInC()
  {
    return p_S_C_;
  }

  void setSensorOrientationInC(const Matrix3 & R)
  {
    R_S_C_ = R;
  }
  Matrix3 getSensorOrientationInC()
  {
    return R_S_C_;
  }
 
  Vector3 getVirtualLocalVelocityMeasurement()
  {
    return x1_;
  }

  void setSensorLinearVelocityInC(const Vector3 & v)
  {
    v_S_C_ = v;
  }
 
  Vector3 getSensorLinearVelocityInC()
  {
    return v_S_C_;
  }

  void setSensorAngularVelocityInC(const Vector3 & w)
  {
    w_S_C_ = w;
  }
  Vector3 getSensorAngularVelocityInC()
  {
    return w_S_C_;
  }

  void setControlOriginVelocityInW(const Vector3 & v)
  {
    v_C_ = v;
  }
  Vector3 getControlOriginVelocityInW()
  {
    return v_C_;
  }

  void resetImuLocVelHat();

#if defined(__clang__)
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Woverloaded-virtual"
#else
#  if defined(__GNUC__)
#    pragma GCC diagnostic push
#    pragma GCC diagnostic ignored "-Woverloaded-virtual"
#  endif
#endif
 
  // we also want to use the function setMeasurement from the TiltEstimator class, that is hidden by the following
  using TiltEstimator::setMeasurement;
  void setMeasurement(const Vector3 & ya_k, const Vector3 & yg_k, TimeIndex k);
 
#if defined(__clang__)
#  pragma clang diagnostic pop
#else
#  if defined(__GNUC__)
#    pragma GCC diagnostic pop
#  endif
#endif
 
public:
protected:
  Vector3 p_S_C_;

  Matrix3 R_S_C_;

  Vector3 v_S_C_;

  Vector3 w_S_C_;

  Vector3 v_C_;

  bool resetX1hat_ = false;

  void checkResetX1hat();
};
 
} // namespace stateObservation
 
#endif // TILTESTIMATORHUMANOIDHPP