Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Kinematics Class Reference

#include <Kinematics.h>

Inheritance diagram for Kinematics:

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:

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.

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(camera_ray(1),camera_ray(2),camera_ray(3),x,y);

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.
 Kinematics (const Kinematics &)
 Copy constructor, everything is either update-before-use or static, copy is normal init.
Kinematicsoperator= (const Kinematics &)
 Assignment operator, everything is either update-before-use or static, assignment is no-op.
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.
NEWMAT::ReturnMatrix calculateGroundPlane ()
 Find the ground plane by fitting a plane to the lowest 3 interest points.
NEWMAT::ReturnMatrix calculateGroundPlane (const NEWMAT::ColumnVector &down)
 Find the ground plane by fitting a plane to the lowest 3 interest points.
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 std::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

Static Protected Member Functions

static void initStatics ()
 initializes static variables -- only call if not staticsInited
static void checkStatics ()
 checks that statics have been initialized, and calls initStatics if they are missing
static ROBOOP::RobotnewChain (unsigned int chainIdx)
 called by init to allocate/initialize each of chains

Static Protected Attributes

static bool staticsInited = false
 initially false, set to true after first Kinematics is initialized
static 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).
static std::vector
< std::vector< unsigned int > > 
chainMaps
 holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent
static JointMap jointMaps [NumReferenceFrames]
 holds mapping from tekkotsu output index to chain and link indicies
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 std::string,InterestPoint,hashstring> Kinematics::InterestPointMap [protected]

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

Definition at line 380 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.

Kinematics::Kinematics ( const Kinematics  )  [inline]

Copy constructor, everything is either update-before-use or static, copy is normal init.

Definition at line 126 of file Kinematics.h.

Kinematics::~Kinematics (  )  [virtual]

Destructor.

Definition at line 142 of file Kinematics.cc.


Member Function Documentation

Kinematics& Kinematics::operator= ( const Kinematics  )  [inline]

Assignment operator, everything is either update-before-use or static, assignment is no-op.

Definition at line 134 of file Kinematics.h.

Referenced by PostureEngine::operator=().

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 169 of file Kinematics.cc.

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 154 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 199 of file Kinematics.cc.

Referenced by PostureEngine::solveLinkVector().

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 184 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 298 of file Kinematics.cc.

Referenced by projectToPlane().

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 270 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 242 of file Kinematics.cc.

Referenced by getLinkInterestPoint().

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 214 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 326 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 354 of file Kinematics.cc.

Referenced by getLinkInterestPoint().

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 199 of file Kinematics.h.

NEWMAT::ReturnMatrix Kinematics::calculateGroundPlane (  ) 

Find the ground plane by fitting a plane to the lowest 3 interest points.

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 605 of file Kinematics.cc.

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

Find the ground plane by fitting a plane to the lowest 3 interest points.

Note:
on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer!
Returns:
vector of the form $p_1x + p_2y + p_3z = p_4$, relative to the base frame

Definition at line 610 of file Kinematics.cc.

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)

Mathematical implementation:

Need to convert p_b to p_j

Once we have the transformation Tb_j from b to j, we need:
T2=inv(Tb_j)'; T2(3,1:end-1)=-T2(3,1:end-1);
but since we know a few things about the structure of T, we don't have to explicitly calculate that inverse.

After we obtain p_j, we can find the point of intersection of r_j and p_j using:

\[ \frac{p_d}{p_{xyz} \cdot r}r \]

Where $p_{xyz}$ is the first three elemnts of p_j, and $p_d$ is the fourth (hopefully last) element of p_j.

Of course, if $p_{xyz} \cdot r$ is 0, then r and p are parallel (since p_j is the normal of the plane, so a line perpendicular to the normal is parallel to the plane), so we set the resulting homogeneous coordinates accordingly to represent an interesection at infinity.

Definition at line 617 of file Kinematics.cc.

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 253 of file Kinematics.h.

Referenced by calculateGroundPlane(), getInterestPoint(), PostureEngine::solveLinkPosition(), and PostureEngine::solveLinkVector().

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 263 of file Kinematics.h.

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 272 of file Kinematics.h.

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

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

Definition at line 277 of file Kinematics.h.

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.

Reimplemented in PostureMC.

Definition at line 23 of file Kinematics.cc.

Referenced by Kinematics().

void Kinematics::initStatics (  )  [static, protected]

initializes static variables -- only call if not staticsInited

Definition at line 35 of file Kinematics.cc.

Referenced by checkStatics().

static void Kinematics::checkStatics (  )  [inline, static, protected]

checks that statics have been initialized, and calls initStatics if they are missing

Definition at line 287 of file Kinematics.h.

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

ROBOOP::Robot * Kinematics::newChain ( unsigned int  chainIdx  )  [static, protected]

called by init to allocate/initialize each of chains

Definition at line 93 of file Kinematics.cc.

Referenced by init(), and initStatics().

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 336 of file Kinematics.cc.

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 668 of file Kinematics.cc.

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

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 315 of file Kinematics.h.

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


Member Data Documentation

bool Kinematics::staticsInited = false [static, protected]

initially false, set to true after first Kinematics is initialized

Definition at line 330 of file Kinematics.h.

Referenced by checkStatics(), and initStatics().

std::vector< ROBOOP::Robot * > Kinematics::chains [static, 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).

static allocation solves problems with shared memory regions, but becomes thread-UNsafe...

Definition at line 338 of file Kinematics.h.

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

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

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

Definition at line 342 of file Kinematics.h.

Referenced by getInterestPoint(), newChain(), PostureEngine::solveLinkPosition(), PostureEngine::update(), and update().

holds mapping from tekkotsu output index to chain and link indicies

Definition at line 364 of file Kinematics.h.

Referenced by initStatics(), lookup(), and newChain().

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 367 of file Kinematics.h.

Referenced by getConfig(), initStatics(), and newChain().

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 384 of file Kinematics.h.

Referenced by getInterestPoint(), and initStatics().


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

Tekkotsu v4.0
Generated Thu Nov 22 00:58:27 2007 by Doxygen 1.5.4