Homepage Demos Overview Downloads Tutorials Reference
Credits

Kinematics Class Reference

#include <Kinematics.h>

Inheritance diagram for Kinematics:

Inheritance graph
[legend]
List of all members.

Detailed Description

Provides access to the mathematical functionality of the ROBOOP package using Tekkotsu data structures.

You should read the Kinematics tutorial to get a general understanding of the math involved and diagrams for usage with supported robots.

This class involves all aspects of the forward kinematics: calculations concerning locations and orientations in space given a known set of joint configurations. There is a global instantiation of Kinematics named kine, which can be used to perform these calculations regarding the joint positions currently in state.

To perform kinematics on a hypothetical set of joint values, use PostureEngine or one of its subclasses. PostureEngine also adds inverse kinematic functions, which will allow you to determine joint angles in order to reach a given point.

The underlying ROBOOP library does not currently handle branching kinematic chains - in other words, each limb of the robot is a separate ROBOOP::Robot as far as ROBOOP is concerned. This Kinematics class interfaces the Tekkotsu array index approach to referencing joints with ROBOOP's chain based hierarchy.

Thus, wherever a reference frame index is requested, you can simply supply one of the output indexes in the usual manner:

 kine->jointToBase(HeadOffset+TiltOffset); 

However, there are also a number of points on the body which are not joints, but should have their own reference frames, such as the base frame, or the camera. These frames have their own indices, listed in the robot info file for the model in question (such as ERS7Info.h), with names ending in FrameOffset.

 kine->jointToBase(CameraFrameOffset); 
Note that for these non-joint-associated reference frames, the link and joint frames are always identical, so you can use either version of the corresponding functions.

Since newmat matrix library is used by ROBOOP, you will need to pass and receive information in newmat matrices. Kinematics class provides static pack and unpack functions which can convert individual x,y,z variables into a NEWMAT::ColumnVector, and vice versa. However, for readability of your code and long-term ease of use, we recommend embracing the newmat data structures directly when appropriate.

  // Find the ray from the camera to whatever the near-field IR is hitting:
  NEWMAT::Matrix T = kine->jointToJoint(NearIRFrameOffset,CameraFrameOffset);
  NEWMAT::ColumnVector camera_ray = T*Kinematics::pack(0,0,state->sensors[NearIRDistOffset]);
  float x,y; // x and y will be in the range -1 to 1 for resolution layer independence
  config->vision.computePixel(x,y,camera_ray(1),camera_ray(2),camera_ray(3));

Finally, for each model we have created a database of "interest points" -- locations of notable interest on the body of the robot. These may be of use to people attempting to use the limbs to manipulate objects. To access these interest points, call either getLinkInterestPoint or getJointInterestPoint with the name of the interest point, obtained from the diagrams.

Note that you can pass a comma separated list of interest point names and the result will be the midpoint of those interest points:

 kine->getLinkInterestPoint(BaseFrameOffset,"LowerInnerFrontLFrShin,LowerOuterFrontLFrShin"); 

See also:
PostureEngine for inverse kinematics

Configuration File Format

The file is actually read by ROBOOP::Config, and thus the syntax of the file is defined by that class. However, Tekkotsu will look for some additional sections beyond what is expected by ROBOOP.

In any give link section, a tekkotsu_output field may appear, which specifies the index (aka offset) of the corresponding joint in Tekkotsu, as defined by the model's Info.h file (e.g. ERS7Info.h). Alternatively, tekkotsu_frame may be specified, which should specify the offset of an abstract reference frame, which does not correspond to any joint. Typically this is used for things such as the camera, or un-actuated joints, such as the spring-loaded ankles.

Additionally, Kinematics will look for an custom InterestPoints section, which should contain a Length field for specifying the number of interest points. Kinematics will then attempt to read InterestPointN for 1 through Length.

Each InterestPointN section should contain:

  • name - (string) name which will be passed to get*InterestPoint() to retrieve this IP
  • chain - (string) name of the chain the IP is in, must match one of the kinematic chains loaded from the file
  • link - (unsigned int) the index of the link the IP is connected to
  • x - (float) the x location of the point, in link-relative coordinates
  • y - (float) the y location of the point, in link-relative coordinates
  • z - (float) the z location of the point, in link-relative coordinates

Definition at line 115 of file Kinematics.h.

Public Member Functions

 Kinematics ()
 Constructor, pass the full path to the kinematics configuration file.
virtual ~Kinematics ()
 Destructor.
NEWMAT::ReturnMatrix linkToBase (unsigned int link)
 Returns a matrix for transforming from link frame j to base frame.
NEWMAT::ReturnMatrix jointToBase (unsigned int joint)
 Returns a matrix for transforming from joint frame j to base frame.
NEWMAT::ReturnMatrix baseToLink (unsigned int link)
 Returns a matrix for transforming from the base frame to link j frame.
NEWMAT::ReturnMatrix baseToJoint (unsigned int joint)
 Returns a matrix for transforming from the base frame to joint j frame.
NEWMAT::ReturnMatrix linkToLink (unsigned int iL, unsigned int oL)
 Returns a matrix for transforming from link ij to link oj.
NEWMAT::ReturnMatrix linkToJoint (unsigned int iL, unsigned int oJ)
 Returns a matrix for transforming from link frame ij to joint frame oj.
NEWMAT::ReturnMatrix jointToLink (unsigned int iJ, unsigned int oL)
 Returns a matrix for transforming from joint frame ij to link frame oj.
NEWMAT::ReturnMatrix jointToJoint (unsigned int iJ, unsigned int oJ)
 Returns a matrix for transforming from joint ij to joint oj.
void getInterestPoint (const std::string &name, unsigned int &link, NEWMAT::Matrix &ip)
 Returns the location of a named point and the link it is attached to.
NEWMAT::ReturnMatrix getJointInterestPoint (unsigned int joint, const std::string &name)
 Returns the location of a named point, relative to any desired joint reference frame.
NEWMAT::ReturnMatrix getLinkInterestPoint (unsigned int link, const std::string &name)
 Returns the location of a named point, relative to any desired reference frame.
LegOrder_t findUnusedLeg (const NEWMAT::ColumnVector &down)
 Find the leg which is in least contact with ground (as best we can anyway).
NEWMAT::ReturnMatrix calculateGroundPlane ()
 Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
NEWMAT::ReturnMatrix calculateGroundPlane (const NEWMAT::ColumnVector &down)
 Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
NEWMAT::ReturnMatrix projectToPlane (unsigned int j, const NEWMAT::ColumnVector &r_j, unsigned int b, const NEWMAT::ColumnVector &p_b, unsigned int f)
 Find the point of intersection between a ray and a plane.

Static Public Member Functions

static NEWMAT::ReturnMatrix pack (float x, float y, float z, float h=1)
 A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector.
static void unpack (NEWMAT::Matrix m, float &ox, float &oy, float &oz)
 A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz.
static void unpack (NEWMAT::Matrix m, float &ox, float &oy, float &oz, float &oh)
 A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh.
static ROBOOP::ConfiggetConfig ()
 returns the global ROBOOP::Config object which Kinematics classes initialize themselves from (roconfig)

Protected Types

typedef __gnu_cxx::hash_map<
const string, InterestPoint,
hashstring
InterestPointMap
 we'll be using the hash_map to store named interest points

Protected Member Functions

void init ()
 Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig.
void getInterestPoint (const std::string &name, unsigned int &c, unsigned int &l, NEWMAT::Matrix &ip)
 Returns the location of a named point, relative to the link it is attached to.
virtual void update (unsigned int c, unsigned int l)
 Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures.
bool lookup (unsigned int tkout, unsigned int &c, unsigned int &l)
 converts a Tekkotsu output index to a chain and link

Protected Attributes

std::vector< ROBOOP::Robot * > chains
 A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well).
std::vector< std::vector<
unsigned int > > 
chainMaps
 holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent
JointMap jointMaps [NumReferenceFrames]
 holds mapping from tekkotsu output index to chain and link indicies

Static Protected Attributes

static ROBOOP::Configroconfig = NULL
 cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk)
static InterestPointMapips = NULL
 these interest points are shared by all Kinematics classes (i.e. all PostureEngines)

Classes

struct  hashstring
 allows us to use the STL hash_map with strings More...
struct  InterestPoint
 holds the position and attached link of a given interest point More...
struct  JointMap
 Allows mapping from tekkotsu output index to chain and link indicies. More...


Member Typedef Documentation

typedef __gnu_cxx::hash_map<const string,InterestPoint,hashstring> Kinematics::InterestPointMap [protected]
 

we'll be using the hash_map to store named interest points

Definition at line 338 of file Kinematics.h.


Constructor & Destructor Documentation

Kinematics::Kinematics  )  [inline]
 

Constructor, pass the full path to the kinematics configuration file.

Definition at line 118 of file Kinematics.h.

virtual Kinematics::~Kinematics  )  [inline, virtual]
 

Destructor.

Definition at line 123 of file Kinematics.h.


Member Function Documentation

NEWMAT::ReturnMatrix Kinematics::baseToJoint unsigned int  joint  ) 
 

Returns a matrix for transforming from the base frame to joint j frame.

Parameters:
[in] joint the output offset, see class notes for values

Definition at line 126 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::baseToLink unsigned int  link  ) 
 

Returns a matrix for transforming from the base frame to link j frame.

Parameters:
[in] link the output offset, see class notes for values

Definition at line 138 of file Kinematics.cc.

Referenced by PostureEngine::solveLinkVector().

NEWMAT::ReturnMatrix Kinematics::calculateGroundPlane const NEWMAT::ColumnVector down  ) 
 

Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).

Returns:
vector of the form $p_1x + p_2y + p_3z = p_4$, relative to the base frame

Definition at line 332 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::calculateGroundPlane  ) 
 

Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).

This function merely calls the other version of calculateGroundPlane with the current gravity vector as the "down" vector.

Returns:
vector of the form $p_1x + p_2y + p_3z = p_4$, relative to the base frame

Definition at line 327 of file Kinematics.cc.

Referenced by GroundPlaneBehavior::processEvent().

LegOrder_t Kinematics::findUnusedLeg const NEWMAT::ColumnVector down  ) 
 

Find the leg which is in least contact with ground (as best we can anyway).

This can be either based on gravity vector from accelerometer readings, or if that may be unreliable due to being in motion, we could do some basic balance modeling.

Returns:
index of leg which is highest in reference to gravity vector

Definition at line 310 of file Kinematics.cc.

Referenced by calculateGroundPlane(), and GroundPlaneBehavior::processEvent().

static ROBOOP::Config* Kinematics::getConfig  )  [inline, static]
 

returns the global ROBOOP::Config object which Kinematics classes initialize themselves from (roconfig)

Definition at line 253 of file Kinematics.h.

void Kinematics::getInterestPoint const std::string &  name,
unsigned int &  c,
unsigned int &  l,
NEWMAT::Matrix ip
[protected]
 

Returns the location of a named point, relative to the link it is attached to.

Parameters:
[in] name the name of the interest point; varies by model, see the diagrams for your model.
[out] c on exit, chain index the IP is on
[out] l on exit, link index the IP is on
[out] ip on exit, a homogeneous column vector of the requested point
If name is not found, c and l will be -1 and ip will be all 0's. This internal version of the function allows us to use c and l, ourselves, but users will probably want to use the getInterestPoint(name,j,ip) version

Definition at line 257 of file Kinematics.cc.

void Kinematics::getInterestPoint const std::string &  name,
unsigned int &  link,
NEWMAT::Matrix ip
 

Returns the location of a named point and the link it is attached to.

Parameters:
[in] name the name of the interest point; varies by model, see the diagrams for your model.
[out] link on exit, offset of the link, or -1U if not found
[out] ip on exit, a homogeneous column vector of the requested point, relative to the link frame returned in j
If name is not found, j will be -1 and ip will be all 0's.

Definition at line 250 of file Kinematics.cc.

Referenced by getJointInterestPoint().

NEWMAT::ReturnMatrix Kinematics::getJointInterestPoint unsigned int  joint,
const std::string &  name
 

Returns the location of a named point, relative to any desired joint reference frame.

Parameters:
[in] joint the desired joint reference frame to give results in
[in] name the name of the interest point; varies by model, see the diagrams for your model.
You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs

Definition at line 272 of file Kinematics.cc.

Referenced by getLinkInterestPoint(), WallTestBehavior::processEvent(), StareAtPawBehavior2::processEvent(), and KinematicSampleBehavior2::processEvent().

NEWMAT::ReturnMatrix Kinematics::getLinkInterestPoint unsigned int  link,
const std::string &  name
[inline]
 

Returns the location of a named point, relative to any desired reference frame.

Parameters:
[in] link the desired link reference frame to give results in
[in] name the name of the interest point; varies by model, see the diagrams for your model.
You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs

Definition at line 185 of file Kinematics.h.

Referenced by KinematicSampleBehavior2::processEvent().

void Kinematics::init  )  [protected]
 

Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig.

Definition at line 15 of file Kinematics.cc.

Referenced by Kinematics().

NEWMAT::ReturnMatrix Kinematics::jointToBase unsigned int  joint  ) 
 

Returns a matrix for transforming from joint frame j to base frame.

Parameters:
[in] joint the output offset, see class notes for values

Definition at line 102 of file Kinematics.cc.

Referenced by calculateGroundPlane(), findUnusedLeg(), and WallTestBehavior::processEvent().

NEWMAT::ReturnMatrix Kinematics::jointToJoint unsigned int  iJ,
unsigned int  oJ
 

Returns a matrix for transforming from joint ij to joint oj.

Parameters:
[in] iJ the output offset to convert from, see class notes for values
[in] oJ the output offset to convert to, see class notes for values

Definition at line 150 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::jointToLink unsigned int  iJ,
unsigned int  oL
 

Returns a matrix for transforming from joint frame ij to link frame oj.

Parameters:
[in] iJ the output offset to convert from, see class notes for values
[in] oL the output offset to convert to, see class notes for values

Definition at line 175 of file Kinematics.cc.

Referenced by getLinkInterestPoint().

NEWMAT::ReturnMatrix Kinematics::linkToBase unsigned int  link  ) 
 

Returns a matrix for transforming from link frame j to base frame.

Parameters:
[in] link the output offset, see class notes for values

Definition at line 114 of file Kinematics.cc.

Referenced by KinematicSampleBehavior::processEvent().

NEWMAT::ReturnMatrix Kinematics::linkToJoint unsigned int  iL,
unsigned int  oJ
 

Returns a matrix for transforming from link frame ij to joint frame oj.

Parameters:
[in] iL the output offset to convert from, see class notes for values
[in] oJ the output offset to convert to, see class notes for values

Definition at line 200 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::linkToLink unsigned int  iL,
unsigned int  oL
 

Returns a matrix for transforming from link ij to link oj.

Parameters:
[in] iL the output offset to convert from, see class notes for values
[in] oL the output offset to convert to, see class notes for values

Definition at line 225 of file Kinematics.cc.

Referenced by projectToPlane().

bool Kinematics::lookup unsigned int  tkout,
unsigned int &  c,
unsigned int &  l
[inline, protected]
 

converts a Tekkotsu output index to a chain and link

Parameters:
[in] tkout the tekkotsu index to lookup
[out] c set to the chain index that tkout is in
[out] l set to the link in c corresponding to tkout

Definition at line 282 of file Kinematics.h.

Referenced by baseToJoint(), baseToLink(), getJointInterestPoint(), jointToBase(), jointToJoint(), jointToLink(), linkToBase(), linkToJoint(), linkToLink(), and PostureEngine::solveLinkPosition().

static NEWMAT::ReturnMatrix Kinematics::pack float  x,
float  y,
float  z,
float  h = 1
[inline, static]
 

A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector.

Parameters:
[in] x the value for the first row
[in] y the value for the second row
[in] z the value for the third row
[in] h the value for the fourth row (defaults to 1 if not specified)
Returns:
$ \left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right] $

Definition at line 229 of file Kinematics.h.

Referenced by calculateGroundPlane(), getInterestPoint(), WallTestBehavior::processEvent(), KinematicSampleBehavior::processEvent(), GroundPlaneBehavior::processEvent(), PostureEngine::solveLinkPosition(), and PostureEngine::solveLinkVector().

NEWMAT::ReturnMatrix Kinematics::projectToPlane unsigned int  j,
const NEWMAT::ColumnVector r_j,
unsigned int  b,
const NEWMAT::ColumnVector p_b,
unsigned int  f
 

Find the point of intersection between a ray and a plane.

Parameters:
j is the link number the ray is relative to
r_j is the line through the origin, in homogeneous coordinates
b is the link number the plane is relative to (probably BaseFrameOffset)
p_b represents the plane to be intersected
f is the link number the results should be relative to
p_b should be of the form $p_1x + p_2y + p_3z = p_4$
Returns:
homogeneous coordinate of intersection (may be infinity)

Definition at line 351 of file Kinematics.cc.

Referenced by GroundPlaneBehavior::processEvent().

static void Kinematics::unpack NEWMAT::Matrix  m,
float &  ox,
float &  oy,
float &  oz,
float &  oh
[inline, static]
 

A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh.

Parameters:
[in] m the matrix to unpack (only uses first column)
[out] ox set to the first row of the first column of m
[out] oy set to the second row of the first column of m
[out] oz set to the third row of the first column of m
[out] oh set to the fourth row of the first column of m

Definition at line 248 of file Kinematics.h.

static void Kinematics::unpack NEWMAT::Matrix  m,
float &  ox,
float &  oy,
float &  oz
[inline, static]
 

A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz.

Parameters:
[in] m the matrix to unpack (only uses first column)
[out] ox set to the first row of the first column of m, divided by fourth row
[out] oy set to the second row of the first column of m, divided by fourth row
[out] oz set to the third row of the first column of m, divided by fourth row

Definition at line 239 of file Kinematics.h.

Referenced by KinematicSampleBehavior::processEvent().

void Kinematics::update unsigned int  c,
unsigned int  l
[protected, virtual]
 

Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures.

This class will pull current values from WorldState, but it is expected that subclasses (i.e. PostureEngine) will want to provide their own joint values. Updates from link 1 through link l.

Parameters:
[in] c the chain to update
[in] l the last link to update (later links in the chain are left untouched)

Reimplemented in PostureEngine.

Definition at line 402 of file Kinematics.cc.

Referenced by baseToJoint(), baseToLink(), getJointInterestPoint(), jointToBase(), jointToJoint(), jointToLink(), linkToBase(), linkToJoint(), and linkToLink().


Member Data Documentation

std::vector< std::vector<unsigned int> > Kinematics::chainMaps [protected]
 

holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent

Definition at line 300 of file Kinematics.h.

Referenced by getInterestPoint(), init(), Kinematics(), and update().

std::vector<ROBOOP::Robot*> Kinematics::chains [protected]
 

A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well).

Definition at line 297 of file Kinematics.h.

Referenced by baseToJoint(), baseToLink(), getJointInterestPoint(), init(), jointToBase(), jointToJoint(), jointToLink(), Kinematics(), linkToBase(), linkToJoint(), linkToLink(), lookup(), and update().

Kinematics::InterestPointMap * Kinematics::ips = NULL [static, protected]
 

these interest points are shared by all Kinematics classes (i.e. all PostureEngines)

this is to reduce initialization time, but does mean one robot can't do kinematic calculations regarding a different model robot...

Definition at line 12 of file Kinematics.cc.

Referenced by getInterestPoint(), and init().

JointMap Kinematics::jointMaps[NumReferenceFrames] [protected]
 

holds mapping from tekkotsu output index to chain and link indicies

Definition at line 322 of file Kinematics.h.

Referenced by init(), and lookup().

ROBOOP::Config * Kinematics::roconfig = NULL [static, protected]
 

cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk)

Definition at line 11 of file Kinematics.cc.

Referenced by getConfig(), and init().


The documentation for this class was generated from the following files:

Tekkotsu v2.2.2
Generated Tue Jan 4 15:45:26 2005 by Doxygen 1.4.0