RobotLink3D.h

#ifndef ROBOT_LINK_3D
#define ROBOT_LINK_3D

#include "Frame.h"

class RobotLink3D
{
public:
  enum Type { Revolute, Prismatic };

  void SetRotationJoint(const Vector3& w);
  void SetTranslationJoint(const Vector3& v);

  void GetLocalTransform(Real qi,Frame3D& T) const;

  void GetVelocity(Real qi,Real dqi,const Vector3& p,Vector3& vel) const;
  void GetAngularVelocity(Real dqi,Vector3& omega) const;

  void GetJacobian(Real qi,const Vector3& p,Vector3& Jo,Vector3& Jp) const;
  void GetOrientationJacobian(Vector3& Jo) const;
  void GetPositionJacobian(Real qi,const Vector3& p,Vector3& Jp) const;

  //Position jacobian of points in frame 0 w.r.t. i 
  void GetJacobian(Real qi,Frame3D& J) const; 
  //Position jacobian of points in frame j w.r.t. i
  void GetJacobian(Real qi,const Frame3D& Tj_World,Frame3D& J) const; 

  void GetWorldInertia(Matrix3& inertiaWorld) const;
  void GetWorldCOM(Vector3& comWorld) const { T_World.mulPoint(com,comWorld); }

  Type type;
  Vector3 w;
  Real mass;
  Vector3 com;
  Matrix3 inertia;
  Frame3D T0_Parent;

  Frame3D T_World;
};

#endif