OperationalSpaceController.h

#ifndef OPERATIONAL_SPACE_CONTROLLER_H
#define OPERATIONAL_SPACE_CONTROLLER_H

#include "Controller.h"
#include <Klampt/Sensing/StateEstimator.h>
#include <KrisLibrary/robotics/IK.h>
#include <KrisLibrary/robotics/Contact.h>
#include <KrisLibrary/utils/SmartPointer.h>

namespace Klampt {

//task is q''[indices] = ddqdes
struct JointAccelTask
{
  vector<int> indices;
  Vector ddqdes;
  Real weight;
};

//task is x''[indices] = ddxdes
//position terms go first like in IKFunctions
struct WorkspaceAccelTask
{
  IKGoal workspace;
  Vector ddxdes;
  Real weight;
};

//task is (R*cm'')[1:numAxes] = ddxdes
struct COMAccelTask
{
  Matrix3 R;
  int numAxes;
  Vector ddxdes;
  Real weight;
};

//task is T[indices] = Tdes
struct TorqueTask
{
  vector<int> indices;
  Vector Tdes;
  Real weight;
};

//Task is Af = fdes
//contacts point into the robot and are given the LOCAL frame
struct ContactForceTask
{
  vector<int> links;
  vector<ContactPoint> contacts;
  Matrix A;
  Vector fdes;
  Real weight,penetrationWeight;
};

struct OperationalSpaceController : public RobotController
{
  OperationalSpaceController(RobotModel& robot);

  virtual const char* Type() const { return "OperationalSpaceController"; }
  virtual void Update(Real dt);
  virtual void Reset();
  /*
  virtual bool ReadState(File& f) {
    if(!ReadFile(f,time)) return false;
    return true;
  }
  virtual bool WriteState(File& f) const {
    if(!WriteFile(f,time)) return false;
    return true;
  }
  */

  //the bulk of the work is done here
  void TasksToTorques(Vector& t);
  bool IsValid() const;

  SmartPointer<IntegratedStateEstimator> stateEstimator;
  Vector3 gravity;
  vector<JointAccelTask> jointTasks;
  vector<WorkspaceAccelTask> workspaceTasks;
  vector<COMAccelTask> comTasks;
  vector<TorqueTask> torqueTasks;
  vector<ContactForceTask> contactForceTasks;
};

} //namespace Klampt

#endif