Robot.h

#ifndef MODELING_ROBOT_H
#define MODELING_ROBOT_H

#include <KrisLibrary/robotics/RobotWithGeometry.h>
#include <KrisLibrary/utils/PropertyMap.h>
#include "ManagedGeometry.h"

using namespace std;

struct RobotJoint
{
  enum Type { Weld, Normal, Spin, Floating, FloatingPlanar, BallAndSocket, Closed };

  Type type;
  int linkIndex;
  int baseIndex;
  Vector3 localPt,attachmentPt;
};

struct RobotJointDriver
{
  enum Type { Normal, Affine, Translation, Rotation, Custom };

  int NumControls() const;  //number of input controls
  int NumLinks() const;
  bool Affects(int link) const;

  Type type;
  vector<int> linkIndices;
  Real qmin,qmax;           //min/max values
  Real vmin,vmax;           //min/max velocities
  Real amin,amax;           //min/max accelerations
  Real tmin,tmax;           //min/max torques
  vector<Real> affScaling;  //for Affine joints
  vector<Real> affOffset;   //for Affine joints
  Real servoP,servoI,servoD;  //servo parameters
  Real dryFriction;           //constant friction coefficient
  Real viscousFriction;       //velocity-dependent friction coefficient
};

class Robot : public RobotWithGeometry
{
public:
  virtual std::string LinkName(int i) const;
  int LinkIndex(const char* name) const;
  bool Load(const char* fn);
  bool LoadRob(const char* fn);
  bool LoadURDF(const char* fn);
  bool Save(const char* fn);
  bool LoadGeometry(int i,const char* file);
  void SetGeomFiles(const char* geomPrefix="",const char* geomExt="off");  
  void SetGeomFiles(const vector<string>& geomFiles);
  bool SaveGeometry(const char* prefix="");  
  void InitStandardJoints();
  bool CheckValid() const;
  //adds a geometry to the geometry of the given link
  void Mount(int link,const Geometry::AnyGeometry3D& geom,const RigidTransform& T);
  //adds a subchain as descendents of a given link
  void Mount(int link,const Robot& subchain,const RigidTransform& T,const char* prefix=NULL);
  void Merge(const std::vector<Robot*>& robots);

  bool DoesJointAffect(int joint,int dof) const;
  void GetJointIndices(int joint,vector<int>& indices) const;
  void SetJointByTransform(int joint,int link,const RigidTransform& T);
  void SetJointByOrientation(int joint,int link,const Matrix3& R);
  void SetJointVelocityByMoment(int joint,int link,const Vector3& w,const Vector3& v);

  bool IsPassiveDOF(int dof) const;
  bool DoesDriverAffect(int driver,int dof) const;
  void GetDriverIndices(int driver,vector<int>& indices) const;
  Vector2 GetDriverLimits(int driver) const;
  Real GetDriverValue(int driver) const;
  Real GetDriverVelocity(int driver) const;
  void SetDriverValue(int driver,Real value);
  void SetDriverVelocity(int driver,Real value);
  void GetDriverJacobian(int driver,Vector& J);

  void ComputeLipschitzMatrix();

  string name;
  vector<string> geomFiles;   
  vector<ManagedGeometry> geomManagers; 
  Vector accMax;   
  vector<RobotJoint> joints;
  vector<RobotJointDriver> drivers;
  vector<string> linkNames;
  vector<string> driverNames;

  PropertyMap properties;

  Matrix lipschitzMatrix;

  static bool disableGeometryLoading;
};

#endif