jspace Namespace Reference

Contains the joint-space dynamics model and related classes. More...


Classes

struct  controller_info_getter_s
 Interface for retrieving custom info (DOF names etc) from controllers. More...
struct  jspace_controller_info_getter_s
 Default info getter is based on the jspace::Model. More...
class  Controller
class  FloatController
class  GoalControllerBase
class  JointGoalController
class  Model
class  RobotAPI
 Abstract API for talking to a robot. More...
class  State
class  Status
struct  tao_node_info_s
 
Note:
TAO supports multiple joints per link, but all use cases so far seem to require that exactly one joint sits between two links, so we treat joint names just as link names until further notice.
More...
struct  tao_tree_info_s

Typedefs

typedef std::map< int, int > id_counter_t
typedef std::map< int, taoDNode * > idToNodeMap_t
typedef Eigen::Transform3d Transform
typedef Eigen::Translation3d Translation
typedef Eigen::Quaternion< double > Quaternion
typedef Eigen::VectorXd Vector
typedef Eigen::MatrixXd Matrix

Enumerations

enum  compensation_flags_t { COMP_NONE = 0x00, COMP_GRAVITY = 0x01, COMP_CORIOLIS = 0x02, COMP_MASS_INERTIA = 0x04 }

Functions

void inertia_parallel_axis_transform (double in_ixx, double in_ixy, double in_ixz, double in_iyy, double in_iyz, double in_izz, double trans_x, double trans_y, double trans_z, double mass, double &out_ixx, double &out_ixy, double &out_ixz, double &out_iyy, double &out_iyz, double &out_izz)
 Parallel axis transform with fundamental types.
void inertia_parallel_axis_transform (deMatrix3 const &in_inertia, deVector3 const &translation, double mass, deMatrix3 &out_inertia)
 Parallel axis transform with TAO types.
void inertia_similarity_transform (double in_ixx, double in_ixy, double in_ixz, double in_iyy, double in_iyz, double in_izz, deQuaternion const &rotation, deMatrix3 &out_inertia)
 Similarity transform with a mixture of fundamental and TAO types.
void inertia_similarity_transform (deMatrix3 const &in_inertia, deQuaternion const &rotation,deMatrix3 &out_inertia)
 Similarity transform with TAO types.
void inertia_similarity_transform (double in_ixx, double in_ixy, double in_ixz, double in_iyy, double in_iyz, double in_izz, double rot_qx, double rot_qy, double rot_qz, double rot_qw, double &out_ixx, double &out_ixy, double &out_ixz, double &out_iyy, double &out_iyz, double &out_izz)
 Similarity transform with fundamental types.
void fuse_mass_properties (double orig_mass, deMatrix3 const &orig_inertia, deFrame const &orig_com, double adtl_mass, deMatrix3 const &adtl_inertia, deFrame const &adtl_com, deFrame const &home_of_adtl_wrt_orig, double &fused_mass, deMatrix3 &fused_inertia, deFrame &fused_com)
 Fuse the mass properties (mass, center of mass, and tensor of inertia) of two bodies, one of them referred to as "original", the other as "additional".
void fuse_mass_properties (deMassProp &original, deMassProp &additional, deFrame const &home_of_additional_wrt_original,deMassProp &fused)
 Fuse mass properties (see above for more details).
void mass_inertia_explicit_form (Model const &model, Matrix &mass_inertia, std::ostream *dbgos) throw (std::runtime_error)
static void _pad (Vector &vv, size_t nn)
std::string inertia_matrix_to_string (deMatrix3 const &mx)
void dump_tao_tree (std::ostream &os, taoDNode *root, std::string prefix, bool detailed, std::vector< std::string > *id_to_link_name, std::vector< std::string > *id_to_joint_name)
 Write a textual description of the TAO tree rooted at the given node to the given std::ostream.
static void _dump_tao_tree_info (std::ostream &os, taoDNode *root, tao_tree_info_s::node_info_t const &info, std::string prefix, bool detailed)
void dump_tao_tree_info (std::ostream &os, tao_tree_info_s *tree, std::string prefix, bool detailed)
 Similar to dump_tao_tree() but uses the more recent tao_tree_info_s structure.
static void _dump_tao_tree_info_saixml (std::ostream &os, taoDNode *root, tao_tree_info_s::node_info_t const &info, std::string prefix) throw (std::runtime_error)
void dump_tao_tree_info_saixml (std::ostream &os, tao_tree_info_s *tree) throw (std::runtime_error)
 Similar to dump_tao_tree_info(), but attempts to spew it out in a format that can be read back into a SAI XML parser.
void dump_tao_tree_info_lotusxml (std::ostream &os, std::string const &robot_name, std::string const &root_link_name, tao_tree_info_s *tree) throw (std::runtime_error)
 Similar to dump_tao_tree_info(), but attempts to spew it out in a format that can be read back into a Lotus XML parser.
void mapNodesToIDs (idToNodeMap_t &idToNodeMap, taoDNode *node) throw (std::runtime_error)
 Create a map between tao nodes and IDs.
int countNumberOfLinks (taoDNode *root)
 Count the total number of links connected to the given node, following all children in to the leaf nodes.
int countNumberOfJoints (taoDNode *node)
 Count the total number of joints attached to the given node and all its descendants.
int countDegreesOfFreedom (taoDNode *node)
 Count the total number of degrees of freedom of all the joints attached to the given node and all its descendants.
double computeTotalMass (taoDNode *node)
 Sum up the mass of the given node plus all its descendants.
static void _recurse_create_bare_tao_tree_info (tao_tree_info_s *tree_info, taoDNode *node)
tao_tree_info_s * create_bare_tao_tree_info (taoNodeRoot *root)
 Create a minimal tao_tree_info_s from a TAO tree.
static void tao_collect_ids (taoDNode *node, id_counter_t &id_counter)
int tao_consistency_check (taoNodeRoot *root,std::ostream *msg)
 Run a consistency check on a TAO tree.
template<typename container_t>
bool compare (container_t const &lhs, container_t const &rhs, typename container_t::value_type precision)
template<typename container_t>
void zero (container_t &vv)
bool compare (jspace::Matrix const &lhs, jspace::Matrix const &rhs, double precision)
bool compare (jspace::Quaternion const &lhs, jspace::Quaternion const &rhs, double precision)
std::string pretty_string (jspace::Vector const &vv)
std::string pretty_string (jspace::Quaternion const &qq)
std::string pretty_string (jspace::Matrix const &mm, std::string const &prefix)
void pretty_print (jspace::Vector const &vv, std::ostream &os, std::string const &title, std::string const &prefix, bool nonl)
void pretty_print (jspace::Quaternion const &qq, std::ostream &os, std::string const &title, std::string const &prefix, bool nonl)
std::string pretty_string (double vv)
void pretty_print (jspace::Matrix const &mm, std::ostream &os, std::string const &title, std::string const &prefix, bool vecmode, bool nonl)
void convert (jspace::Vector const &from, std::vector< double > &to)
void convert (std::vector< double > const &from, jspace::Vector &to)
void convert (double const *from, size_t length, jspace::Vector &to)
void pretty_print (Eigen::Vector3d const &vv, std::ostream &os, std::string const &title, std::string const &prefix, bool nonl=false)

Detailed Description

Contains the joint-space dynamics model and related classes.

Typedef Documentation

typedef std::map<int, int> jspace::id_counter_t

Definition at line 220 of file tao_util.cpp.

typedef std::map<int, taoDNode *> jspace::idToNodeMap_t

Definition at line 112 of file tao_util.hpp.

typedef Eigen::MatrixXd jspace::Matrix

Definition at line 38 of file wrap_eigen.hpp.

typedef Eigen::Quaternion<double> jspace::Quaternion

Definition at line 36 of file wrap_eigen.hpp.

typedef Eigen::Transform3d jspace::Transform

Definition at line 34 of file wrap_eigen.hpp.

typedef Eigen::Translation3d jspace::Translation

Definition at line 35 of file wrap_eigen.hpp.

typedef Eigen::VectorXd jspace::Vector

Definition at line 37 of file wrap_eigen.hpp.

Enumeration Type Documentation

enum jspace::compensation_flags_t

Enumerator:
COMP_NONE 
COMP_GRAVITY 
COMP_CORIOLIS 
COMP_MASS_INERTIA 

Definition at line 36 of file controller_library.hpp.

Function Documentation

static void jspace::_dump_tao_tree_info ( std::ostream &  os,
taoDNode *  root,
tao_tree_info_s::node_info_t const &  info,
std::string  prefix,
bool  detailed 
) [static]

Definition at line 115 of file tao_dump.cpp.

References inertia_matrix_to_string().

Referenced by dump_tao_tree_info().

Here is the call graph for this function:

static void jspace::_dump_tao_tree_info_saixml ( std::ostream &  os,
taoDNode *  root,
tao_tree_info_s::node_info_t const &  info,
std::string  prefix 
) throw (std::runtime_error) [static]

Definition at line 160 of file tao_dump.cpp.

Referenced by dump_tao_tree_info_saixml().

static void jspace::_pad ( Vector &  vv,
size_t  nn 
) [static]

Definition at line 82 of file State.cpp.

Referenced by jspace::State::resizeAndPadWithZeros().

static void jspace::_recurse_create_bare_tao_tree_info ( tao_tree_info_s *  tree_info,
taoDNode *  node 
) [static]

Definition at line 191 of file tao_util.cpp.

References jspace::tao_tree_info_s::info, and sfl::to_string().

Referenced by create_bare_tao_tree_info().

Here is the call graph for this function:

bool jspace::compare ( jspace::Quaternion const &  lhs,
jspace::Quaternion const &  rhs,
double  precision 
)

Definition at line 55 of file wrap_eigen.cpp.

References compare().

Here is the call graph for this function:

bool jspace::compare ( jspace::Matrix const &  lhs,
jspace::Matrix const &  rhs,
double  precision 
)

Definition at line 33 of file wrap_eigen.cpp.

template<typename container_t>
bool jspace::compare ( container_t const &  lhs,
container_t const &  rhs,
typename container_t::value_type  precision 
) [inline]

Definition at line 38 of file vector_util.hpp.

Referenced by compare(), and jspace::State::equal().

double jspace::computeTotalMass ( taoDNode *  node  ) 

Sum up the mass of the given node plus all its descendants.

Definition at line 88 of file tao_util.cpp.

void jspace::convert ( double const *  from,
size_t  length,
jspace::Vector &  to 
)

Definition at line 184 of file wrap_eigen.cpp.

void jspace::convert ( std::vector< double > const &  from,
jspace::Vector &  to 
)

Definition at line 178 of file wrap_eigen.cpp.

void jspace::convert ( jspace::Vector const &  from,
std::vector< double > &  to 
)

Definition at line 171 of file wrap_eigen.cpp.

int jspace::countDegreesOfFreedom ( taoDNode *  node  ) 

Count the total number of degrees of freedom of all the joints attached to the given node and all its descendants.

Definition at line 75 of file tao_util.cpp.

int jspace::countNumberOfJoints ( taoDNode *  node  ) 

Count the total number of joints attached to the given node and all its descendants.

Definition at line 62 of file tao_util.cpp.

int jspace::countNumberOfLinks ( taoDNode *  root  ) 

Count the total number of links connected to the given node, following all children in to the leaf nodes.

This number does NOT include the given link (because usually you will call this on the TAO root node in order to figure out how many degrees of freedom the robot has, in which case you do not count the root itself).

Definition at line 51 of file tao_util.cpp.

tao_tree_info_s * jspace::create_bare_tao_tree_info ( taoNodeRoot *  root  ) 

Create a minimal tao_tree_info_s from a TAO tree.

The created structure will use dummy names like "joint12" and humongous limits.

Definition at line 209 of file tao_util.cpp.

References _recurse_create_bare_tao_tree_info(), jspace::tao_tree_info_s::root, and jspace::tao_tree_info_s::tao_tree_info_s().

Here is the call graph for this function:

void jspace::dump_tao_tree ( std::ostream &  os,
taoDNode *  root,
std::string  prefix,
bool  detailed,
std::vector< std::string > *  id_to_link_name,
std::vector< std::string > *  id_to_joint_name 
)

Write a textual description of the TAO tree rooted at the given node to the given std::ostream.

Each line of output is prefixed by the given prefix, and each level of the tree is indented two spaces with respect to the previous level. If the detailed flag is specified, then a lot more information is given for each node. Optionally, you can pass pointers to vectors containing the node and joint names, which will then be output along with the ID (if there is an entry for that ID in the given id_to_link_name). Use NULL for id_to_link_name and/or id_to_link_name if you do not have that information, or don't care to have it printed.

Definition at line 77 of file tao_dump.cpp.

References inertia_matrix_to_string().

Here is the call graph for this function:

void jspace::dump_tao_tree_info ( std::ostream &  os,
tao_tree_info_s *  tree,
std::string  prefix,
bool  detailed 
)

Similar to dump_tao_tree() but uses the more recent tao_tree_info_s structure.

Definition at line 154 of file tao_dump.cpp.

References _dump_tao_tree_info(), jspace::tao_tree_info_s::info, and jspace::tao_tree_info_s::root.

Here is the call graph for this function:

void jspace::dump_tao_tree_info_lotusxml ( std::ostream &  os,
std::string const &  robot_name,
std::string const &  root_link_name,
tao_tree_info_s *  tree 
) throw (std::runtime_error)

Similar to dump_tao_tree_info(), but attempts to spew it out in a format that can be read back into a Lotus XML parser.

Definition at line 255 of file tao_dump.cpp.

void jspace::dump_tao_tree_info_saixml ( std::ostream &  os,
tao_tree_info_s *  tree 
) throw (std::runtime_error)

Similar to dump_tao_tree_info(), but attempts to spew it out in a format that can be read back into a SAI XML parser.

Definition at line 245 of file tao_dump.cpp.

References _dump_tao_tree_info_saixml().

Here is the call graph for this function:

void jspace::fuse_mass_properties ( deMassProp &  original,
deMassProp &  additional,
deFrame const &  home_of_additional_wrt_original,
deMassProp &  fused 
)

Fuse mass properties (see above for more details).

Parameters:
fused  OK to pass same ref as original

Definition at line 220 of file inertia_util.cpp.

References fuse_mass_properties().

Here is the call graph for this function:

void jspace::fuse_mass_properties ( double  orig_mass,
deMatrix3 const &  orig_inertia,
deFrame const &  orig_com,
double  adtl_mass,
deMatrix3 const &  adtl_inertia,
deFrame const &  adtl_com,
deFrame const &  home_of_adtl_wrt_orig,
double &  fused_mass,
deMatrix3 &  fused_inertia,
deFrame &  fused_com 
)

Fuse the mass properties (mass, center of mass, and tensor of inertia) of two bodies, one of them referred to as "original", the other as "additional".

The result is computed with respect to the origin of the original body. Inertia tensors are assumed to be expressed wrt to the center of mass. The axes of the COM frames are assumed to be aligned with the axes of their home frames (in other words, the rotational parts of orig_com and adtl_com are ignored).

Definition at line 127 of file inertia_util.cpp.

References inertia_parallel_axis_transform(), and inertia_similarity_transform().

Referenced by fuse_mass_properties().

Here is the call graph for this function:

std::string jspace::inertia_matrix_to_string ( deMatrix3 const &  mx  ) 

Definition at line 55 of file tao_dump.cpp.

Referenced by _dump_tao_tree_info(), dump_tao_tree(), and std::operator<<().

void jspace::inertia_parallel_axis_transform ( deMatrix3 const &  in_inertia,
deVector3 const &  translation,
double  mass,
deMatrix3 &  out_inertia 
)

Parallel axis transform with TAO types.

Definition at line 56 of file inertia_util.cpp.

References inertia_parallel_axis_transform().

Here is the call graph for this function:

void jspace::inertia_parallel_axis_transform ( double  in_ixx,
double  in_ixy,
double  in_ixz,
double  in_iyy,
double  in_iyz,
double  in_izz,
double  trans_x,
double  trans_y,
double  trans_z,
double  mass,
double &  out_ixx,
double &  out_ixy,
double &  out_ixz,
double &  out_iyy,
double &  out_iyz,
double &  out_izz 
)

Parallel axis transform with fundamental types.

The parallel axis transform allows you to find the inertia tensor with respect to a frame that has been translated (but not rotated) with respect to the frame that was used to express the inertia tensor.

Definition at line 36 of file inertia_util.cpp.

Referenced by fuse_mass_properties(), and inertia_parallel_axis_transform().

void jspace::inertia_similarity_transform ( double  in_ixx,
double  in_ixy,
double  in_ixz,
double  in_iyy,
double  in_iyz,
double  in_izz,
double  rot_qx,
double  rot_qy,
double  rot_qz,
double  rot_qw,
double &  out_ixx,
double &  out_ixy,
double &  out_ixz,
double &  out_iyy,
double &  out_iyz,
double &  out_izz 
)

Similarity transform with fundamental types.

The similarity transform allows you to find the inertial tensor with respect to a frame that has been rotated (but not translated) with respect to the frame that was used to express the inertia tensor.

Definition at line 108 of file inertia_util.cpp.

References inertia_similarity_transform().

Here is the call graph for this function:

void jspace::inertia_similarity_transform ( deMatrix3 const &  in_inertia,
deQuaternion const &  rotation,
deMatrix3 &  out_inertia 
)

Similarity transform with TAO types.

Parameters:
out_inertia  OK to have &in_inertia == &out_inertia, we use copies.

Definition at line 97 of file inertia_util.cpp.

References inertia_similarity_transform().

Here is the call graph for this function:

void jspace::inertia_similarity_transform ( double  in_ixx,
double  in_ixy,
double  in_ixz,
double  in_iyy,
double  in_iyz,
double  in_izz,
deQuaternion const &  rotation,
deMatrix3 &  out_inertia 
)

Similarity transform with a mixture of fundamental and TAO types.

Definition at line 79 of file inertia_util.cpp.

Referenced by fuse_mass_properties(), and inertia_similarity_transform().

void jspace::mapNodesToIDs ( idToNodeMap_t &  idToNodeMap,
taoDNode *  node 
) throw (std::runtime_error)

Create a map between tao nodes and IDs.

The idToNodeMap is not cleared for you: use this function to append to an existing map, or clear the map yourself prior to use.

Note:
Throws a runtime_error in case there is a duplicate ID

Definition at line 36 of file tao_util.cpp.

References sfl::to_string().

Here is the call graph for this function:

void jspace::mass_inertia_explicit_form ( Model const &  model,
Matrix &  mass_inertia,
std::ostream *  dbgos 
) throw (std::runtime_error)

Definition at line 240 of file inertia_util.cpp.

References model, ndof, and pretty_string().

Here is the call graph for this function:

void jspace::pretty_print ( Eigen::Vector3d const &  vv,
std::ostream &  os,
std::string const &  title,
std::string const &  prefix,
bool  nonl = false 
) [inline]

Definition at line 61 of file wrap_eigen.hpp.

References pretty_print().

Referenced by opspace::OrientationTask::dbg(), opspace::JointLimitTask::dbg(), opspace::TrajectoryTask::dbg(), opspace::DraftPIDTask::dbg(), opspace::ClassicTaskPostureController::dbg(), and opspace::Task::dump().

Here is the call graph for this function:

void jspace::pretty_print ( jspace::Matrix const &  mm,
std::ostream &  os,
std::string const &  title,
std::string const &  prefix,
bool  vecmode,
bool  nonl 
)

Definition at line 127 of file wrap_eigen.cpp.

References pretty_string().

Here is the call graph for this function:

void jspace::pretty_print ( jspace::Quaternion const &  qq,
std::ostream &  os,
std::string const &  title,
std::string const &  prefix,
bool  nonl 
)

Definition at line 93 of file wrap_eigen.cpp.

References pretty_print().

Here is the call graph for this function:

void jspace::pretty_print ( jspace::Vector const &  vv,
std::ostream &  os,
std::string const &  title,
std::string const &  prefix,
bool  nonl 
)

Definition at line 85 of file wrap_eigen.cpp.

Referenced by pretty_print(), pretty_string(), servo_cb(), and opspace::ParameterLog::writeFiles().

std::string jspace::pretty_string ( double  vv  ) 

Definition at line 101 of file wrap_eigen.cpp.

Referenced by opspace::MatrixParameter::dump(), and opspace::VectorParameter::dump().

std::string jspace::pretty_string ( jspace::Matrix const &  mm,
std::string const &  prefix 
)

Definition at line 77 of file wrap_eigen.cpp.

References pretty_print().

Here is the call graph for this function:

std::string jspace::pretty_string ( jspace::Quaternion const &  qq  ) 

Definition at line 69 of file wrap_eigen.cpp.

References pretty_print().

Here is the call graph for this function:

std::string jspace::pretty_string ( jspace::Vector const &  vv  ) 

Definition at line 61 of file wrap_eigen.cpp.

References pretty_print().

Referenced by mass_inertia_explicit_form(), pretty_print(), and opspace::ParameterLog::writeFiles().

Here is the call graph for this function:

static void jspace::tao_collect_ids ( taoDNode *  node,
id_counter_t &  id_counter 
) [static]

Definition at line 222 of file tao_util.cpp.

Referenced by tao_consistency_check().

int jspace::tao_consistency_check ( taoNodeRoot *  root,
std::ostream *  msg 
)

Run a consistency check on a TAO tree.

For the time being, this simply checks that each node has a unique ID, that the IDs range from 0 to N-1, and that the root has an ID of -1.

Returns:
0 on success, 1 if the root node's ID is wrong, 2 if there's a gap in the ID sequence, and 3 if there's a duplicate ID.
Parameters:
root  The TAO tree which should be checked.
msg  An optional pointer to a stream, where error messages get printed in case you specify a non-NULL pointer here.

Definition at line 238 of file tao_util.cpp.

References tao_collect_ids().

Referenced by jspace::Model::init().

Here is the call graph for this function:

template<typename container_t>
void jspace::zero ( container_t &  vv  )  [inline]

Definition at line 59 of file vector_util.hpp.

Generated on Fri Aug 26 01:32:29 2011 for Stanford Whole-Body Control Framework by  doxygen 1.5.4