KinematicInertialPoseObserver.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_observers/Observer.h>
#include <mc_observers/api.h>
#include <mc_rbdyn/Robot.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_observers
{
 
struct MC_OBSERVER_DLLAPI KinematicInertialPoseObserver : public Observer
{
  KinematicInertialPoseObserver(const std::string & type, double dt) : Observer(type, dt) {}
 
  void configure(const mc_control::MCController & ctl, const mc_rtc::Configuration & config) override;
 
  void reset(const mc_control::MCController & ctl) override;
 
  bool run(const mc_control::MCController & ctl) override;
 
  void update(mc_control::MCController & ctl) override;
 
  const sva::PTransformd & posW() const { return pose_; }
 
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;
 
  void estimateOrientation(const mc_rbdyn::Robot & robot, const mc_rbdyn::Robot & realRobot);
 
  void estimatePosition(const mc_control::MCController & ctl);
 
protected:
  std::string robot_; 
  std::string realRobot_; 
  std::string imuSensor_; 
  std::string anchorFrameFunction_ = ""; 
  sva::PTransformd X_0_anchorFrame_ =
      sva::PTransformd::Identity(); 
  sva::PTransformd X_0_anchorFrameReal_ =
      sva::PTransformd::Identity(); 
 
  double maxAnchorFrameDiscontinuity_ =
      0.01; 
  bool anchorFrameJumped_ = false; 
  bool firstIter_ = true; 
private:
  sva::PTransformd pose_ = sva::PTransformd::Identity(); 
  bool showAnchorFrame_ = false; 
  bool showAnchorFrameReal_ = false; 
  bool showPose_ = false; 
  bool advancedGUI_ = false; 
 
  bool logPose_ = true; 
  bool logAnchorFrame_ = true; 
};
 
} // namespace mc_observers