GeneralizedRobot.h

#ifndef GENERALIZED_ROBOT_H
#define GENERALIZED_ROBOT_H

#include "Robot.h"
#include "RigidObject.h"
#include "World.h"
#include <map>

class GeneralizedRobot
{
 public:
  GeneralizedRobot();
  GeneralizedRobot(RobotWorld& world);
  int Add(Robot* robot,const char* name=NULL);
  int Add(RigidObject* object,const char* name=NULL);
  void Remove(int id);
  void Remove(const char* name) { Remove(ID(name)); }
  void Remove(Robot* robot) { Remove(ID(robot)); }
  void Remove(RigidObject* object) { Remove(ID(object)); }
  int ID(const char* name) const;
  int ID(Robot* robot) const;
  int ID(RigidObject* object) const;
  int NumDof() const;
  string DofName(int index) const;
  int DofToID(int index) const;
  pair<int,int> Dofs(int id) const;
  pair<int,int> Dofs(const char* name) const { return Dofs(ID(name)); }
  pair<int,int> Dofs(Robot* robot) const { return Dofs(ID(robot)); }
  pair<int,int> Dofs(RigidObject* object) const { return Dofs(ID(object)); }
  int Dof(int id,int link) const { return Dofs(id).first+link; }
  int Dof(Robot* robot,int link) const { return Dof(ID(robot),link); }
  int Dof(const char* name,int link) const { return Dof(ID(name),link); }
  void SetConfig(const Config& q);
  void SetVelocity(const Vector& v);
  void GetConfig(Config& q) const;
  void GetVelocity(Vector& v) const;
  void UpdateGeometry();
  void Split(const Config& q,vector<Config>& qsplit) const;
  void SplitRefs(const Config& q,vector<Config>& qsplit) const;
  void Join(const vector<Config>& qsplit,Config& q) const;
  void Interpolate(const Config& a,const Config& b,Real u,Config& out) const;
  void InterpolateVelocity(const Config& a,const Config& b,Real u,Vector& dq) const;
  //Integrates a velocity vector dq from q to obtain the configuration out
  void Integrate(const Config& q,const Vector& dq,Config& out) const;
  Real Distance(const Config& a,const Config& b,Real floatingRotationWeight=1.0) const;
  void GetJointLimits(Config& qmin,Config& qmax) const;
  Vector3 GetCOM() const;
  void GetMegaRobot(Robot& voltron) const;

  struct Element
  {
    string name;
    Robot* robot;
    RigidObject* object;
    int indexStart,indexEnd;
  };
  map<int,Element> elements;
};


void ObjectToRobot(const RigidObject& object,Robot& robot);

void ConfigToTransform(const Vector& q,RigidTransform& T);

void TransformToConfig(const RigidTransform& T,Vector& q);



#endif