FeedforwardController.h

#ifndef FEEDFORWARD_CONTROLLER_H
#define FEEDFORWARD_CONTROLLER_H

#include "Controller.h"
#include <Klampt/Sensing/StateEstimator.h>
#include <KrisLibrary/robotics/Wrench.h>

class FeedforwardController : public RobotController
{
 public:
  FeedforwardController(Robot& robot,shared_ptr<RobotController> base);
  virtual ~FeedforwardController() {}
  virtual const char* Type() const { return "FeedforwardController"; }
  virtual void Update(Real dt);
  virtual void Reset();
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;

  //getters/setters
  virtual map<string,string> Settings() const;
  virtual bool GetSetting(const string& name,string& str) const;
  virtual bool SetSetting(const string& name,const string& str);

  //commands
  void ZeroForces();
  void AddForce(int link,const Vector3& f,const Vector3& worldpt);
  virtual vector<string> Commands() const;
  virtual bool SendCommand(const string& name,const string& str);

  //helpers
  void SolveTorques(Vector& torques,Real dt);

  shared_ptr<RobotController> base;
  shared_ptr<RobotStateEstimator> stateEstimator;
  bool enableGravityCompensation,enableFeedforwardAcceleration;
  Vector3 gravity;
  //external forces and moments about the link origin
  vector<Wrench> wrenches;
};


#endif