StateEstimator.h

#ifndef STATE_ESTIMATOR_H
#define STATE_ESTIMATOR_H

#include "Sensor.h"
#include <Klampt/Control/Command.h>
#include <Klampt/Modeling/Robot.h>
#include <KrisLibrary/robotics/Wrench.h>

namespace Klampt {

class ODERobot;

struct RobotStateEstimator
{
  RobotStateEstimator(RobotModel& _robot) :robot(_robot) {}
  virtual ~RobotStateEstimator() {}
  virtual void ReadSensors(RobotSensors& sensors) {}
  virtual void UpdateModel() {}
  virtual void ReadCommand(const RobotMotorCommand& command) {}
  virtual void Advance(Real dt) {}
  virtual void Reset() {}

  RobotModel& robot;
};

struct OmniscientStateEstimator : public RobotStateEstimator
{
  OmniscientStateEstimator(RobotModel& _robot,ODERobot& _oderobot)
    :RobotStateEstimator(_robot),oderobot(_oderobot) {}
  virtual ~OmniscientStateEstimator();
  virtual void UpdateModel() override;

  ODERobot& oderobot;
};

struct IntegratedStateEstimator : public RobotStateEstimator
{
  IntegratedStateEstimator(RobotModel& _robot);
  virtual ~IntegratedStateEstimator() {}
  virtual void ReadSensors(RobotSensors& sensors) override;
  virtual void UpdateModel() override {
    robot.UpdateConfig(q_predicted);
    robot.dq = dq_predicted;
  }
  virtual void ReadCommand(const RobotMotorCommand& command) override;
  virtual void Advance(Real dt) override;
  virtual void Reset() override;

  //instead of ReadCommand, this estimator uses SetDDQ
  void SetDDQ(const Vector& ddq) { ddq_predicted = ddq; }

  Real last_dt;
  Vector q_predicted,dq_predicted,ddq_predicted;
  Vector q_sensed,dq_sensed;
  //these correspond to the world positions of the accelerometers in the sensors
  vector<RigidTransform> accelerometerFrames;
  vector<RigidBodyVelocity> accelerometerVels;
};

} //namespace Klampt

#endif