SCL
1.0
Standard Control Library : Control, dynamics, physics, and simulation

Classes  
struct  SAncestryEntry 
Public Member Functions  
Model ()  
int  init (STaoTreeInfo *kgm_tree, STaoTreeInfo *cc_tree, double grav_x_, double grav_y_, double grav_z_, std::ostream *msg) 
void  update (State const &state) 
void  setState (State const &state) 
State const &  getState () const 
size_t  getNNodes () const 
size_t  getNJoints () const 
size_t  getNDOF () const 
std::string  getNodeName (size_t id) const 
std::string  getJointName (size_t id) const 
taoDNode *  getNode (size_t id) const 
taoDNode *  getNodeByName (std::string const &name) const 
taoDNode *  getNodeByJointName (std::string const &name) const 
void  getJointLimits (Eigen::VectorXd &joint_limits_lower, Eigen::VectorXd &joint_limits_upper) const 
void  updateKinematics () 
bool  getGlobalFrame (taoDNode const *node, Eigen::Affine3d &global_transform) const 
bool  computeGlobalFrame (taoDNode const *node, Eigen::Affine3d const &local_transform, Eigen::Affine3d &global_transform) const 
bool  computeGlobalFrame (taoDNode const *node, double local_x, double local_y, double local_z, Eigen::Affine3d &global_transform) const 
bool  computeGlobalFrame (taoDNode const *node, Eigen::VectorXd const &local_translation, Eigen::Affine3d &global_transform) const 
bool  computeGlobalCOMFrame (taoDNode const *node, Eigen::Affine3d &global_com_transform) const 
bool  computeJacobian (taoDNode const *node, Eigen::MatrixXd &jacobian) const 
bool  computeJacobian (taoDNode const *node, double gx, double gy, double gz, Eigen::MatrixXd &jacobian) const 
bool  computeJacobian (taoDNode const *node, Eigen::VectorXd const &global_point, Eigen::MatrixXd &jacobian) const 
void  updateDynamics () 
bool  computeCOM (Eigen::VectorXd &com, Eigen::MatrixXd *opt_jcom) const 
void  computeGravity () 
bool  disableGravityCompensation (size_t index, bool disable) 
bool  getGravity (Eigen::VectorXd &gravity) const 
void  computeCoriolisCentrifugal () 
bool  getCoriolisCentrifugal (Eigen::VectorXd &coriolis_centrifugal) const 
void  computeMassInertia () 
bool  getMassInertia (Eigen::MatrixXd &mass_inertia) const 
void  computeInverseMassInertia () 
bool  getInverseMassInertia (Eigen::MatrixXd &inverse_mass_inertia) const 
STaoTreeInfo *  _getKGMTree () 
STaoTreeInfo *  _getCCTree () 
Private Types  
typedef std::set< size_t >  dof_set_t 
Private Attributes  
std::size_t  ndof_ 
STaoTreeInfo *  kgm_tree_ 
STaoTreeInfo *  cc_tree_ 
dof_set_t  gravity_disabled_ 
deVector3 *  gravity_ 
State  state_ 
Eigen::VectorXd  g_torque_ 
Eigen::VectorXd  cc_torque_ 
std::vector< double >  a_upper_triangular_ 
std::vector< double >  ainv_upper_triangular_ 
std::map< taoDNode *, std::list< SAncestryEntry > >  ancestry_table_ 
jspace::Model::Model  (  ) 
Please use the init() method in order to initialize your jspace::Model. It does some sanity checking, and error handling from within a constructor is just not so great.

inline 
For debugging only, access to the optional Corioliscentrifugal tree. Can NULL if the user is not interested in Corioliscentrifugal effects.

inline 
For debugging only, access to the kinematicsgravitymassinertia tree.
bool jspace::Model::computeCOM  (  Eigen::VectorXd &  com, 
Eigen::MatrixXd *  opt_jcom  
)  const 
Computes the location of the center of gravity, and optionally also its Jacobian. Pass opt_jcom=0 if you are not interested in the Jacobian. Failures can only be due to calls of computeJacobian() that happens for each node's contribution to the Jacobian of the COM.
void jspace::Model::computeCoriolisCentrifugal  (  ) 
Compute the Coriolis and contrifugal jointtorque vector. If you set cc_tree=NULL in the constructor, then this is a noop.
bool jspace::Model::computeGlobalFrame  (  taoDNode const *  node, 
Eigen::Affine3d const &  local_transform,  
Eigen::Affine3d &  global_transform  
)  const 
Compute the global frame (translation and rotation) corresponding to a local frame expressed wrt the origin of a given node.
bool jspace::Model::computeGlobalFrame  (  taoDNode const *  node, 
double  local_x,  
double  local_y,  
double  local_z,  
Eigen::Affine3d &  global_transform  
)  const 
Convenience method in case you are only interested in the translational part and hold the local point in three doubles. The copmuted global_transform will have the same rotational component as the node's origin.
bool jspace::Model::computeGlobalFrame  (  taoDNode const *  node, 
Eigen::VectorXd const &  local_translation,  
Eigen::Affine3d &  global_transform  
)  const 
Convenience method in case you are only interested in the translational part and hold the local point in a threedimensional vector. The copmuted global_transform will have the same rotational component as the node's origin.
void jspace::Model::computeGravity  (  ) 
Compute the gravity jointtorque vector.
void jspace::Model::computeInverseMassInertia  (  ) 
Compute the inverse jointspace massinertia matrix.
bool jspace::Model::computeJacobian  (  taoDNode const *  node, 
Eigen::MatrixXd &  jacobian  
)  const 
Compute the Jacobian (J_v over J_omega) at the origin of a given node.
bool jspace::Model::computeJacobian  (  taoDNode const *  node, 
double  gx,  
double  gy,  
double  gz,  
Eigen::MatrixXd &  jacobian  
)  const 
Compute the Jacobian (J_v over J_omega) for a given node, at a point expressed wrt to the global frame.

inline 
Convenience method in case you are holding the global position in a threedimensional vector.
void jspace::Model::computeMassInertia  (  ) 
Compute the jointspace massinertia matrix, a.k.a. the kinetic energy matrix.
bool jspace::Model::disableGravityCompensation  (  size_t  index, 
bool  disable  
) 
Disable (or enable) gravity compensation for a given DOF (specified using its index). If you set disable
to true
, then getGravity() will knock out (set to zero) the corresponding entry of the gravity jointtorque vector.
true
is returned. Valid indices are 0<=index<getNDOF()
.disable
for this joint. bool jspace::Model::getCoriolisCentrifugal  (  Eigen::VectorXd &  coriolis_centrifugal  )  const 
Retrieve the Coriolis and contrifugal jointtorque vector.
bool jspace::Model::getGlobalFrame  (  taoDNode const *  node, 
Eigen::Affine3d &  global_transform  
)  const 
Retrieve the frame (translation and rotation) of a node origin.
bool jspace::Model::getGravity  (  Eigen::VectorXd &  gravity  )  const 
Retrieve the gravity jointtorque vector.
bool jspace::Model::getInverseMassInertia  (  Eigen::MatrixXd &  inverse_mass_inertia  )  const 
Retrieve the inverse jointspace massinertia matrix.
void jspace::Model::getJointLimits  (  Eigen::VectorXd &  joint_limits_lower, 
Eigen::VectorXd &  joint_limits_upper  
)  const 
Retrieve joint limit information. This method fills the provided vectors with the lower and upper joint limits. In case no joint limit information is available, it sets the lower limit to std::numeric_limits<double>::min()
and the upper limit to std::numeric_limits<double>::max()
.
std::string jspace::Model::getJointName  (  size_t  id  )  const 
Retrieve the name of a joint. Returns an empty string in case the id is invalid. Use getNJoints() to find out how many joints there are.
bool jspace::Model::getMassInertia  (  Eigen::MatrixXd &  mass_inertia  )  const 
Retrieve the jointspace massinertia matrix, a.k.a. the kinetic energy matrix.
size_t jspace::Model::getNDOF  (  )  const 
Compute or retrieve the cached number of degrees of freedom of the robot. Note that each joint can have any number of degrees of freedom, which is why this method might return something else than getNJoints().
size_t jspace::Model::getNJoints  (  )  const 
Compute or retrieve the cached number of joints in the robot. Note that each joint can have any number of degrees of freedom, which is why getNDOF() might come in handy, too.
size_t jspace::Model::getNNodes  (  )  const 
Compute or retrieve the cached number of nodes in the robot.
taoDNode * jspace::Model::getNode  (  size_t  id  )  const 
Retrieve a node by ID.
taoDNode * jspace::Model::getNodeByJointName  (  std::string const &  name  )  const 
Retrieve a node by joint name. This will find and retrieve the node to which the joint is attached (see also getNodeByName()), which allows you to retrieve the taoJoint instance itself.
taoDNode * jspace::Model::getNodeByName  (  std::string const &  name  )  const 
Retrieve a node by its name.
std::string jspace::Model::getNodeName  (  size_t  id  )  const 
Retrieve the name of a node. Returns an empty string in case the id is invalid. Use getNNodes() to find out how many nodes there are.

inline 
Retrieve the state passed to setState() (or update(), for that matter).
int jspace::Model::init  (  STaoTreeInfo *  kgm_tree, 
STaoTreeInfo *  cc_tree,  
double  grav_x_,  
double  grav_y_,  
double  grav_z_,  
std::ostream *  msg  
) 
Initialize the model with a TAO tree. Actually, it needs two copies of the same tree if you want to estimate centrifugal and Coriolis effects, but that is optional.
This method also does some sanity checks and will return a nonzero error code if something is amiss. In order to get humanreadable error strings, just pass a nonNULL msg pointer in as well. For instance, &std::cout will do nicely in most cases.
kgm_tree  TAO tree info used for computing kinematics, the gravity torque vector, the massinertia matrix, and its inverse. This tree will be deleted in the jspace::Model destructor. 
cc_tree  Optional TAO tree info for computing Coriolis and centrifugal torques. If you set this to NULL, then the Coriolis and centrifugal forces won't be computed. This tree will be deleted in the jspace::Model destructor. 
grav_x_  Gravity x * 
grav_y_  Gravity y * 
grav_z_  Gravity z * 
msg  Optional stream that will receive error messages from the consistency checks. 
void jspace::Model::setState  (  State const &  state  ) 
Inform the model about the joint state. We have to separate the state update from the computation of the various quantities in order to efficiently use the TAO tree representation, which forces us to distribute the state over its nodes before computing the model.
void jspace::Model::update  (  State const &  state  ) 
Calls setState(), updateKinematics(), and updateDynamics(). After calling the update() method, you can use any of the other methods without worrying whether you have already called the corresponding computeFoo() method.
void jspace::Model::updateDynamics  (  ) 
void jspace::Model::updateKinematics  (  ) 
Computes the node origins wrt the global frame.