KinematicInertialObserver.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_filter/LowPass.h>
#include <mc_observers/KinematicInertialPoseObserver.h>
#include <mc_observers/api.h>
 
namespace mc_observers
{
struct MC_OBSERVER_DLLAPI KinematicInertialObserver : public KinematicInertialPoseObserver
{
  KinematicInertialObserver(const std::string & type, double dt)
  : KinematicInertialPoseObserver(type, dt), velFilter_(dt, 2 * dt)
  {
  }
 
  void configure(const mc_control::MCController & ctl, const mc_rtc::Configuration & config) override;
 
  void reset(const mc_control::MCController & ctl) override;
 
  void reset(const mc_control::MCController & ctl, const sva::MotionVecd & velW);
  bool run(const mc_control::MCController & ctl) override;
  void update(mc_control::MCController & ctl) override;
 
  const sva::MotionVecd & velW() const;
 
protected:
  void addToLogger(const mc_control::MCController & ctl, mc_rtc::Logger &, const std::string & category) override;
  void addToGUI(const mc_control::MCController &,
                mc_rtc::gui::StateBuilder &,
                const std::vector<std::string> & category) override;
 
private:
  bool showVelocity_ = true;
  mc_rtc::gui::ArrowConfig velocityArrowConfig_;
 
  sva::PTransformd posWPrev_ = sva::PTransformd::Identity();
 
  mc_filter::LowPass<sva::MotionVecd> velFilter_;
  sva::MotionVecd velW_ = sva::MotionVecd::Zero();
 
  bool logVelocity_ = true; 
 
private:
  using KinematicInertialPoseObserver::reset;
};
} // namespace mc_observers