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, please see PostureEngine or its subclasses. PostureEngine also includes 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 Robot as far as ROBOOP is concerned. This class interfaces the Tekkotsu array index approach to referencing joints with ROBOOP's chain based hierarchy.

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

 kine->frameToBase(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->frameToBase(CameraFrameOffset); 

Since newmat matrix library is used by ROBOOP, you will need to pass and receive information in newmat matrices. This class provides 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:
  // x and y will be in the range -1 to 1 for resolution layer independence
  // This ignores lens distortion - just proof of concept
  NEWMAT::Matrix T = kine->frameToFrame(NearIRFrameOffset,CameraFrameOffset);
  NEWMAT::ColumnVector camera_ray = T*Kinematics::pack(0,0,state->sensors[NearIRDistOffset]);
  float x=atan(camera_ray(1),camera_ray(3))/config->vision.horizFOV/2;
  float y=atan(camera_ray(2),camera_ray(3))/config->vision.vertFOV/2;

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, simply call either getLinkInterestPoint or getFrameInterestPoint 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

Definition at line 83 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 j)
 Returns a matrix for transforming from link j to base frame.
NEWMAT::ReturnMatrix frameToBase (unsigned int j)
 Returns a matrix for transforming from frame j to base frame.
NEWMAT::ReturnMatrix baseToLink (unsigned int j)
 Returns a matrix for transforming from the base frame to link j.
NEWMAT::ReturnMatrix baseToFrame (unsigned int j)
 Returns a matrix for transforming from the base frame to frame j.
NEWMAT::ReturnMatrix linkToLink (unsigned int ij, unsigned int oj)
 Returns a matrix for transforming from link ij to link oj frame.
NEWMAT::ReturnMatrix linkToFrame (unsigned int ij, unsigned int oj)
 Returns a matrix for transforming from link ij to frame oj.
NEWMAT::ReturnMatrix frameToLink (unsigned int ij, unsigned int oj)
 Returns a matrix for transforming from frame ij to link oj.
NEWMAT::ReturnMatrix frameToFrame (unsigned int ij, unsigned int oj)
 Returns a matrix for transforming from frame ij to frame oj frame.
void getInterestPoint (const std::string &name, unsigned int &j, NEWMAT::Matrix &ip)
 Returns the location of a named point and the link it is attached to.
NEWMAT::ReturnMatrix getFrameInterestPoint (unsigned int frame, const std::string &name)
 Returns the location of a named point, relative to any desired 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

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.
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.
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.
ROBOOP::ConfiggetConfig ()

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 they represent
JointMap jointMaps [NumReferenceFrames]
 holds mapping from tekkotsu output index to chain and link indicies

Static Protected Attributes

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


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

Referenced by init().


Constructor & Destructor Documentation

Kinematics::Kinematics  )  [inline]
 

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

Definition at line 86 of file Kinematics.h.

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

Destructor.

Definition at line 91 of file Kinematics.h.


Member Function Documentation

NEWMAT::ReturnMatrix Kinematics::baseToFrame unsigned int  j  ) 
 

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

Parameters:
[in] j the frame number, see class notes for values

Definition at line 124 of file Kinematics.cc.

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

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

Parameters:
[in] j the link number, see class notes for values

Definition at line 136 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 325 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

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

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

NEWMAT::ReturnMatrix Kinematics::frameToBase unsigned int  j  ) 
 

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

Parameters:
[in] j the frame number, see class notes for values

Definition at line 100 of file Kinematics.cc.

Referenced by calculateGroundPlane(), and findUnusedLeg().

NEWMAT::ReturnMatrix Kinematics::frameToFrame unsigned int  ij,
unsigned int  oj
 

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

Parameters:
[in] ij the frame number to convert from, see class notes for values
[in] oj the frame number to convert to, see class notes for values

Definition at line 148 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::frameToLink unsigned int  ij,
unsigned int  oj
 

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

Parameters:
[in] ij the frame number to convert from, see class notes for values
[in] oj the link number to convert to, see class notes for values

Definition at line 173 of file Kinematics.cc.

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

Definition at line 220 of file Kinematics.h.

NEWMAT::ReturnMatrix Kinematics::getFrameInterestPoint unsigned int  frame,
const std::string &  name
 

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

Parameters:
[in] frame the desired 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 270 of file Kinematics.cc.

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

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

void Kinematics::getInterestPoint const std::string &  name,
unsigned int &  j,
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] j on exit, joint index of the link, or -1U if not found
[out] ip on exit, a homogenous column vector of the requested point
If name is not found, j will be -1 and ip will be all 0's.

Definition at line 248 of file Kinematics.cc.

Referenced by getFrameInterestPoint().

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 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 153 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.

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

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

Parameters:
[in] j the link number, see class notes for values

Definition at line 112 of file Kinematics.cc.

Referenced by KinematicSampleBehavior::processEvent().

NEWMAT::ReturnMatrix Kinematics::linkToFrame unsigned int  ij,
unsigned int  oj
 

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

Parameters:
[in] ij the link number to convert from, see class notes for values
[in] oj the frame number to convert to, see class notes for values

Definition at line 198 of file Kinematics.cc.

NEWMAT::ReturnMatrix Kinematics::linkToLink unsigned int  ij,
unsigned int  oj
 

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

Parameters:
[in] ij the link number to convert from, see class notes for values
[in] oj the link number to convert to, see class notes for values

Definition at line 223 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 249 of file Kinematics.h.

Referenced by baseToFrame(), baseToLink(), frameToBase(), frameToFrame(), frameToLink(), getFrameInterestPoint(), linkToBase(), linkToFrame(), linkToLink(), and PostureEngine::solveLinkPosition().

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

Referenced by getInterestPoint(), 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 homogenous 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:
homogenous coordinate of intersection (may be infinity)

Definition at line 344 of file Kinematics.cc.

Referenced by GroundPlaneBehavior::processEvent().

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

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

Referenced by baseToFrame(), baseToLink(), frameToBase(), frameToFrame(), frameToLink(), getFrameInterestPoint(), linkToBase(), linkToFrame(), 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 they represent

Definition at line 267 of file Kinematics.h.

Referenced by getInterestPoint(), init(), 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 264 of file Kinematics.h.

Referenced by baseToFrame(), baseToLink(), frameToBase(), frameToFrame(), frameToLink(), getFrameInterestPoint(), init(), linkToBase(), linkToFrame(), linkToLink(), 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 289 of file Kinematics.h.

Referenced by init().

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 init().


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

Tekkotsu v2.2
Generated Tue Oct 19 14:20:18 2004 by Doxygen 1.3.9.1