ControlledRobot.h

#ifndef CONTROLLED_ROBOT_H
#define CONTROLLED_ROBOT_H

#include "Controller.h"

namespace Klampt {

class ControlledRobot
{
 public:
  ControlledRobot();
  virtual ~ControlledRobot() {}
  virtual bool Init(RobotModel* robot,RobotController* controller=NULL);
  virtual void Step(Real dt);
  virtual void ReadSensorData(RobotSensors& sensors)=0;
  virtual void WriteCommandData(const RobotMotorCommand& command)=0;

  //convenience functions for inspection
  void GetCommandedConfig(Config& q);
  void GetCommandedVelocity(Config& dq);
  void GetSensedConfig(Config& q);
  void GetSensedVelocity(Config& dq);

  //settings
  RobotModel* klamptRobotModel;
  RobotController* klamptController;

  //state
  RobotMotorCommand command;
  RobotSensors sensors;
};

} //namespace Klampt

#endif