ControlledSimulator.h

#ifndef ODE_CONTROLLED_SIMULATOR_H
#define ODE_CONTROLLED_SIMULATOR_H

#include <Klampt/Control/Controller.h>
#include "ODERobot.h"

class WorldSimulation;

class ControlledRobotSimulator
{
 public:
  ControlledRobotSimulator();
  void Init(Robot* robot,ODERobot* oderobot,RobotController* controller=NULL);
  void Step(Real dt,WorldSimulation* sim);
  void UpdateRobot();

  void GetCommandedConfig(Config& q);
  void GetCommandedVelocity(Config& dq);
  void GetSensedConfig(Config& q);
  void GetSensedVelocity(Config& dq);
  void GetSimulatedConfig(Config& q);
  void GetSimulatedVelocity(Config& dq);
  void GetActuatorTorques(Vector& t) const;
  void GetLinkTorques(Vector& t) const;
  RobotMotorCommand* GetCommands() { return &command; }

  bool ReadState(File& f);
  bool WriteState(File& f) const;

  //settings
  Robot* robot;
  ODERobot* oderobot;
  RobotController* controller;
  Real controlTimeStep;

  //state
  Real curTime;
  Real nextControlTime;
  RobotMotorCommand command;
  RobotSensors sensors;
  vector<Real> nextSenseTime;
};

#endif