Tekkotsu: Kinematics Class Reference

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.

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:

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.

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.
Kinematics &	operator= (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.
void	calcLegHeights (const NEWMAT::ColumnVector &down, float heights[NumLegs])
	Calculate the leg heights along a given "down" vector (0 is level with base frame).
LegOrder_t	findUnusedLeg (const NEWMAT::ColumnVector &down)
	Find the leg which is in least contact with ground.
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::Config *	getConfig ()
	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
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::Robot *	newChain (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::Config *	roconfig = NULL
	cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk)
static InterestPointMap *	ips = 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 378 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 141 of file Kinematics.cc.

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

Referenced by PostureEngine::solveLinkVector().

void Kinematics::calcLegHeights	(	const NEWMAT::ColumnVector &	down,
		float	heights[NumLegs]
	)

Calculate the leg heights along a given "down" vector (0 is level with base frame).

This can be based on either the gravity vector from accelerometer readings, or if that may be unreliable due to being in motion, you could do some basic balance modeling and pass a predicted vector. This uses the interest point database to find the lowest interest point for each leg

Note:: on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer!

Definition at line 394 of file Kinematics.cc.

Referenced by findUnusedLeg().

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 , relative to the base frame

Definition at line 456 of file Kinematics.cc.

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 , relative to the base frame

Definition at line 446 of file Kinematics.cc.

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

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

Definition at line 285 of file Kinematics.h.

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

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

Find the leg which is in least contact with ground.

See also:: calcLegHeights()

Note:: on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer!

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

Definition at line 430 of file Kinematics.cc.

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

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

Definition at line 275 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 335 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 325 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 353 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.

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

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

Referenced by calcLegHeights(), and calculateGroundPlane().

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 213 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 241 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 168 of file Kinematics.cc.

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 269 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 297 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 313 of file Kinematics.h.

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

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

called by init to allocate/initialize each of chains

Definition at line 92 of file Kinematics.cc.

Referenced by init(), and initStatics().

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

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

Referenced by calcLegHeights(), calculateGroundPlane(), getInterestPoint(), 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)

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

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

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

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

Member Data Documentation

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

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

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

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