Kinematics Class Reference

Forward and inverse kinematics calculations modeled after the ROBOOP package, but using Tekkotsu data structures. More...

#include <Kinematics.h>

Inheritance diagram for Kinematics:

[legend]

List of all members.

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...
Public Member Functions
	Kinematics ()
	Constructor, pass the full path to the kinematics configuration file.
	Kinematics (const Kinematics &k)
	Copy constructor, everything is either update-before-use or static, copy is normal init.
Kinematics &	operator= (const Kinematics &k)
	Assignment operator, everything is either update-before-use or static, assignment is no-op.
virtual	~Kinematics ()
	Destructor.
const KinematicJoint *	getKinematicJoint (unsigned int idx) const
	returns the KinematicJoint structure for the specified Tekkotsu output or reference frame offset
fmat::Column< 3 >	getJointPosition (unsigned int idx) const
fmat::Transform	linkToBase (unsigned int link)
	Returns a matrix for transforming from link frame j to base frame.
fmat::Transform	jointToBase (unsigned int joint)
	Returns a matrix for transforming from joint frame j to base frame.
fmat::Transform	baseToLink (unsigned int link)
	Returns a matrix for transforming from the base frame to link j frame.
fmat::Transform	baseToJoint (unsigned int joint)
	Returns a matrix for transforming from the base frame to joint j frame.
fmat::Transform	linkToLink (unsigned int iL, unsigned int oL)
	Returns a matrix for transforming from link iL to link oL.
fmat::Transform	jointToJoint (unsigned int iJ, unsigned int oJ)
	Returns a matrix for transforming from joint iJ to joint oJ.
fmat::Transform	jointToLink (unsigned int iJ, unsigned int oL)
	Returns a matrix for transforming from joint iJ to link oL.
fmat::Transform	linkToJoint (unsigned int iL, unsigned int oJ)
	Returns a matrix for transforming from link iL to joint oJ.
void	getInterestPoint (const std::string &name, unsigned int &link, fmat::Column< 3 > &ip)
	Returns the location of a named point and the link it is attached to.
const fmat::Column< 3 > &	getLinkInterestPoint (unsigned int link, const std::string &name)
	Returns the location of a named point, relative to any desired reference frame.
const fmat::Column< 3 >	getJointInterestPoint (unsigned int joint, const std::string &name)
void	calcLegHeights (const fmat::Column< 3 > &down, float heights[NumLegs])
	Calculate the leg heights along a given "down" vector (0 is level with base frame).
LegOrder_t	findUnusedLeg (const fmat::Column< 3 > &down)
	Find the leg which is in least contact with ground.
PlaneEquation	calculateGroundPlane ()
	Find the ground plane by fitting a plane to the lowest 3 interest points.
PlaneEquation	calculateGroundPlane (const fmat::Column< 3 > &down)
	Find the ground plane by fitting a plane to the lowest 3 interest points.
fmat::Column< 4 >	projectToPlane (unsigned int j, const fmat::Column< 3 > &r_j, unsigned int b, const PlaneEquation &p_b, unsigned int f, float objCentroidHeight=0)
	Find the point of intersection between a ray and a plane.
fmat::Column< 4 >	projectToGround (unsigned int j, const fmat::Column< 3 > &r_j, float objCentroidHeight=0)
	Find the location of an object on the ground from an arbitrary ray r_j in reference frame j (probably CameraFrameOffset).
fmat::Column< 4 >	projectToGround (const VisionObjectEvent &visObj, float objCentroidHeight=0)
	Find the location of an object on the ground (the easy way from a vision object event (i.e. EventBase::visObjEGID)).
fmat::Column< 4 >	projectToGround (const VisionObjectEvent &visObj, const PlaneEquation &gndPlane, float objCentroidHeight=0)
	Find the location of an object on the ground with a custom ground plane specification.
Static Public Member Functions
static fmat::Column< 3 >	pack (float x, float y, float z)
	A simple utility function, converts x,y,z,h to a fmat::Column<3>.
static fmat::Column< 4 >	pack (float x, float y, float z, float h)
	A simple utility function, converts x,y,z,h to a fmat::Column<4>.
static void	unpack (const fmat::SubVector< 4, const float > &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.
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.
virtual void	update () const
	refresh the joint settings in root from WorldState::outputs
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
Protected Attributes
KinematicJoint	root
	the root of the kinematic tree
unsigned int	lastUpdateTime
	determine if the joints are up to date (compare to WorldState::lastSensorUpdateTime)
KinematicJoint *	jointMaps [NumReferenceFrames]
	holds mapping from tekkotsu output index to chain and link indicies
Static Protected Attributes
static bool	staticsInited = false
	initially false, set to true after first Kinematics is initialized
static InterestPointMap	ips
	these interest points are shared by all Kinematics classes (i.e. all PostureEngines)

Detailed Description

Forward and inverse kinematics calculations modeled after the ROBOOP package, but 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.

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.

Example code:

  // Find the ray from the camera to whatever the near-field IR is hitting:
  fmat::Transform T = kine->jointToJoint(NearIRFrameOffset,CameraFrameOffset);
  fmat::Column<3> camera_ray = T*fmat::pack(0,0,state->sensors[NearIRDistOffset]);
  float x,y; // x will be in the range ±1 for resolution layer independence, y ranges ±y_dim/x_dim (i.e. ±1/aspectRatio), see Config::vision_config::y_range
  config->vision.computePixel(camera_ray[0],camera_ray[1],camera_ray[2],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

Definition at line 71 of file Kinematics.h.

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

Constructor & Destructor Documentation

Kinematics::Kinematics ( )

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

Definition at line 74 of file Kinematics.h.

Kinematics::Kinematics ( const Kinematics & k )

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

Definition at line 79 of file Kinematics.h.

Kinematics::~Kinematics ( ) [virtual]

Destructor.

Definition at line 39 of file Kinematics.cc.

Member Function Documentation

fmat::Transform 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 131 of file Kinematics.h.

Referenced by jointToJoint(), linkToJoint(), and DualCoding::projectMarkerToCam().

fmat::Transform 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 127 of file Kinematics.h.

Referenced by jointToLink(), and PostureEngine::solveLinkVector().

void Kinematics::calcLegHeights	(	const fmat::Column< 3 > &	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 44 of file Kinematics.cc.

Referenced by findUnusedLeg().

PlaneEquation Kinematics::calculateGroundPlane ( const fmat::Column< 3 > & 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 102 of file Kinematics.cc.

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

Referenced by projectToGround().

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

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

Definition at line 280 of file Kinematics.h.

Referenced by init().

LegOrder_t Kinematics::findUnusedLeg ( const fmat::Column< 3 > & 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 76 of file Kinematics.cc.

void Kinematics::getInterestPoint	(	const std::string &	name,
		unsigned int &	link,
		fmat::Column< 3 > &	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, the requested point, relative to the link frame returned in link

If name is not found, link will be -1 and ip will be all 0's.

Definition at line 170 of file Kinematics.h.

const fmat::Column<3> Kinematics::getJointInterestPoint	(	unsigned int	joint,
		const std::string &	name
	)

Definition at line 178 of file Kinematics.h.

fmat::Column<3> Kinematics::getJointPosition ( unsigned int idx ) const

Definition at line 100 of file Kinematics.h.

const KinematicJoint* Kinematics::getKinematicJoint ( unsigned int idx ) const

returns the KinematicJoint structure for the specified Tekkotsu output or reference frame offset

Definition at line 98 of file Kinematics.h.

Referenced by ArmControllerBehavior::ArmControllerBehavior(), RRTPlanner::initialize(), HeadPointerMC::lookAtPoint(), HeadPointerMC::lookInDirection(), ArmMC::moveOffsetToPoint(), and PlanarThreeLinkArm::PlanarThreeLinkArm().

const fmat::Column<3>& Kinematics::getLinkInterestPoint	(	unsigned int	link,
		const std::string &	name
	)

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

Referenced by Kinematics().

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

initializes static variables -- only call if not staticsInited

Definition at line 33 of file Kinematics.cc.

Referenced by checkStatics().

fmat::Transform 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 118 of file Kinematics.h.

Referenced by baseToJoint(), jointToJoint(), and jointToLink().

fmat::Transform 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 146 of file Kinematics.h.

fmat::Transform Kinematics::jointToLink	(	unsigned int	iJ,
		unsigned int	oL
	)

Returns a matrix for transforming from joint iJ to link oL.

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

fmat::Transform 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 109 of file Kinematics.h.

Referenced by baseToLink(), and linkToJoint().

fmat::Transform Kinematics::linkToJoint	(	unsigned int	iL,
		unsigned int	oJ
	)

Returns a matrix for transforming from link iL to joint 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 160 of file Kinematics.h.

fmat::Transform Kinematics::linkToLink	(	unsigned int	iL,
		unsigned int	oL
	)

Returns a matrix for transforming from link iL to link oL.

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

Kinematics& Kinematics::operator= ( const Kinematics & k )

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

Definition at line 86 of file Kinematics.h.

static fmat::Column<4> Kinematics::pack	(	float	x,
		float	y,
		float	z,
		float	h
	)			`[static]`

A simple utility function, converts x,y,z,h to a fmat::Column<4>.

Parameters:

`[in]`	x	the value for the first element
`[in]`	y	the value for the second element
`[in]`	z	the value for the third element
`[in]`	h	the value for the fourth element

Returns:: $\left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right]$

Definition at line 261 of file Kinematics.h.

static fmat::Column<3> Kinematics::pack	(	float	x,
		float	y,
		float	z
	)			`[static]`

A simple utility function, converts x,y,z,h to a fmat::Column<3>.

Parameters:

`[in]`	x	the value for the first element
`[in]`	y	the value for the second element
`[in]`	z	the value for the third element

Returns:: $\left[\begin{array}{c} x\\y\\z\\ \end{array}\right]$

Definition at line 253 of file Kinematics.h.

Referenced by pack(), projectToPlane(), and PostureEngine::solveLinkVector().

fmat::Column< 4 > Kinematics::projectToGround	(	const VisionObjectEvent &	visObj,
		const PlaneEquation &	gndPlane,
		float	objCentroidHeight = `0`
	)

Find the location of an object on the ground with a custom ground plane specification.

gndPlane must be specified relative to the base frame, in the form $p_1x + p_2y + p_3z = p_4$ ,

See also:: projectToPlane() for more control over projection and results

Definition at line 330 of file Kinematics.cc.

fmat::Column<4> Kinematics::projectToGround	(	const VisionObjectEvent &	visObj,
		float	objCentroidHeight = `0`
	)

Find the location of an object on the ground (the easy way from a vision object event (i.e. EventBase::visObjEGID)).

Parameters:

	visObj	the vision object to project
	objCentroidHeight	the height of the object's centroid above the ground Uses the default calculateGroundPlane(), otherwise call the other projectToGround() to specify a custom plane.

Definition at line 238 of file Kinematics.h.

fmat::Column<4> Kinematics::projectToGround	(	unsigned int	j,
		const fmat::Column< 3 > &	r_j,
		float	objCentroidHeight = `0`
	)

Find the location of an object on the ground from an arbitrary ray r_j in reference frame j (probably CameraFrameOffset).

Parameters:

	visObj	the vision object to project
	objCentroidHeight	the height of the object's centroid above the ground Uses the default calculateGroundPlane(), otherwise call projectToPlane() to specify a custom plane.

Definition at line 230 of file Kinematics.h.

Referenced by projectToGround().

fmat::Column< 4 > Kinematics::projectToPlane	(	unsigned int	j,
		const fmat::Column< 3 > &	r_j,
		unsigned int	b,
		const PlaneEquation &	p_b,
		unsigned int	f,
		float	objCentroidHeight = `0`
	)

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 ray through the origin of link j
	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
	objCentroidHeight	the height of the object's centroid above the ground

p_b should be of the form $p_1x + p_2y + p_3z = p_4$

Returns:: homogeneous coordinate of intersection (may be infinity)

For projecting to the ground plane, one of the specialized projectToGround() functions may be more convenient.

Mathematical implementation:

We'll convert the ray to the plane's reference frame, solve there. We find a point on the ray (ro_b) and the direction of the ray (rv_b). rv_b does not need to be normalized because we're going to find a scaling factor for it, and that factor accounts for current magnitude.

Proof, p=plane normal vector, d=plane displacement, r = ray direction, o = ray offset, x = [x y z] coordinates, t = scaling factor

$\begin{eqnarray*} \vec p \cdot\vec x &=& d \qquad{\rm(Definition\ of\ plane)}\\ \vec x &=& t \vec r + \vec o \qquad{\rm(Definition\ of\ ray\ through\ point)}\\ \vec p \cdot ( t \vec r + \vec o ) &=& d \\ \vec p \cdot t \vec r + \vec p \cdot \vec o &=& d \\ t ( \vec p \cdot \vec r ) &=& d - \vec p \cdot \vec o \\ t &=& \frac{d - \vec p \cdot \vec o}{\vec p \cdot \vec r} = \frac{dist}{align} \\ && {\rm (substitute\ back\ to\ find\ } \vec x = t \vec r + \vec o{\rm )} \end{eqnarray*}$

Find distance from the ray offset (ro_b) and the closest point on the plane.

Object height is applied along the plane normal toward the ray origin (we assume the ray source is "above" ground)

Find scaling factor by projecting ray vector (rv_b) onto plane normal.

Intersection point will be rv_b*dist/align + ro_b, but need to watch out for case where align==0 (rv_b and plane are parallel, no intersection)

Definition at line 256 of file Kinematics.cc.

Referenced by projectToGround().

static void Kinematics::unpack	(	const fmat::SubVector< 4, const float > &	m,
		float &	ox,
		float &	oy,
		float &	oz
	)			`[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 column to unpack from, applying scaling factor
`[out]`	ox	set to the first element of m, divided by fourth element
`[out]`	oy	set to the second element of m, divided by fourth element
`[out]`	oz	set to the third row element of m, divided by fourth element