IKThreeLink Class Reference

Performs analytical solution for position control of a variety of three or fewer link configurations. More...

#include <IKThreeLink.h>

Inheritance diagram for IKThreeLink:

[legend]

Detailed Description

Performs analytical solution for position control of a variety of three or fewer link configurations.

Link configurations must conform to one of these patterns (R=revolute joint, P=prismatic joint):

R
P
R₁R₂, where R₁ and R₂ are either parallel or orthogonal
RP, where R and P are orthogonal
R₁R₂R₃, where R₁ and R₂ are orthogonal, and R₂ and R₃ are either orthogonal (Aibo leg) or parallel (Chiara leg)
R₁R₂P, where R₁ and R₂ are orthogonal, P is orthogonal to R₂, and P intersects R₁ (Pan/Tilt camera)

Orientation solutions are not supported (yet?) Solutions will degrade gracefully when out of reach, either effector range or joint limit, and should return the closest possible solution.

Definition at line 23 of file IKThreeLink.h.

List of all members.

Public Member Functions
	IKThreeLink ()
	constructor
virtual bool	solve (const Point &pEff, const Rotation &oriEff, KinematicJoint &j, const Position &pTgt, float posPri, const Orientation &oriTgt, float oriPri) const
	Solve to get an 'effector' (pEff, oriEff, relative to link following j) to a solution of pTgt, oriTgt (or at least a local minimum).
virtual IKSolver::StepResult_t	step (const Point &pEff, const Rotation &oriEff, KinematicJoint &j, const Position &pTgt, float pDist, float posPri, const Orientation &oriTgt, float oriDist, float oriPri) const
	Move an 'effector' (pEff, oriEff, relative to link following j) towards a solution of pTgt, oriTgt (or at least a local minimum).
Protected Member Functions
void	computeFirstLink (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool &valid) const
	forwards to either computeFirstLinkRevolute() or computeFirstLinkPrismatic() based on curlink
void	computeFirstLinkRevolute (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool &valid) const
	sets the angle of curlink based directly on the projected angle of the objective minus the projected angle of the effector point (Plink)
void	computeFirstLinkPrismatic (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool &valid) const
	sets the angle of curlink based directly on the projected angle of the objective minus the projected angle of the effector point (Plink)
void	computeSecondLink (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	forwards to either computeSecondLinkRevolute() or computeSecondLinkPrismatic() based on curlink
void	computeSecondLinkRevolute (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	sets the angle of curlink based on the elevation of the objective point vs effector point, projecting about parent link's z axis (which will be set from computeFirstLink())
void	computeSecondLinkPrismatic (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	sets the length of curlink based on the distance of the objective point, projecting about parent link's z axis (which will be set from computeFirstLink())
void	computeThirdLink (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	forwards to either computeThirdLinkRevolute() or computeThirdLinkPrismatic() based on curlink
void	computeThirdLinkRevolute (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	sets the angle of curlink based on length of thigh (distance from curlink to parent origin), and distance from curlink to effector point, to achieve desired distance from parent to effector.
void	computeThirdLinkPrismatic (KinematicJoint &curlink, const fmat::Column< 3 > &Pobj, KinematicJoint &endlink, const fmat::Column< 3 > &Plink, bool invert, bool &valid) const
	sets the length of curlink based on length of thigh (distance from curlink to parent origin), the angle between the parent and effector point, and the distance from parent to objective.
Static Protected Member Functions
static unsigned int	setLinks (KinematicJoint &eff, KinematicJoint *links[], unsigned int remain)
	searches eff parents to assign remain mobile joints into links
static fmat::fmatReal	computePrismaticQ (fmat::fmatReal objD2, fmat::fmatReal neckD2, fmat::fmatReal inner)
	solves for a prismatic link, pass the objective distance squared, the "neck" distance squared, and the inner product of the neck angle, i.e. inner = neck·linkZ = neckD · 1 · cos(neckAng)
static float	normalize_angle (float x)
	ensures that t is in the range ±π (upper boundary may not be inclusive...?)
Protected Attributes
bool	invertThird
	there are two knee solutions, this will choose the non-default solution.
bool	hasInverseSolution
	set to true if there is another solution within range
float	inverseKnee
	alternative angle for knee
Static Protected Attributes
static const float	EPSILON = 1e-3f
	roundoff for numerical error (probably should split this for angular vs. linear values)
Static Private Attributes
static const std::string	autoRegisterIKThreeLink = IKSolver::getRegistry().registerType<IKThreeLink>("IKThreeLink")
	holds the class name, set via registration with the DeviceDriver registry

Constructor & Destructor Documentation

IKThreeLink::IKThreeLink ( )

constructor

Definition at line 26 of file IKThreeLink.h.

Member Function Documentation

void IKThreeLink::computeFirstLink	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool &	valid
	)			const `[protected]`

forwards to either computeFirstLinkRevolute() or computeFirstLinkPrismatic() based on curlink

Definition at line 41 of file IKThreeLink.h.

Referenced by solve().

void IKThreeLink::computeFirstLinkPrismatic	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool &	valid
	)			const `[protected]`

sets the angle of curlink based directly on the projected angle of the objective minus the projected angle of the effector point (Plink)

Definition at line 175 of file IKThreeLink.cc.

Referenced by computeFirstLink().

void IKThreeLink::computeFirstLinkRevolute	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool &	valid
	)			const `[protected]`

sets the angle of curlink based directly on the projected angle of the objective minus the projected angle of the effector point (Plink)

Definition at line 152 of file IKThreeLink.cc.

Referenced by computeFirstLink().

fmat::fmatReal IKThreeLink::computePrismaticQ	(	fmat::fmatReal	objD2,
		fmat::fmatReal	neckD2,
		fmat::fmatReal	inner
	)			`[static, protected]`

solves for a prismatic link, pass the objective distance squared, the "neck" distance squared, and the inner product of the neck angle, i.e. inner = neck·linkZ = neckD · 1 · cos(neckAng)

Definition at line 506 of file IKThreeLink.cc.

Referenced by computeSecondLinkPrismatic(), and computeThirdLinkPrismatic().

void IKThreeLink::computeSecondLink	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

forwards to either computeSecondLinkRevolute() or computeSecondLinkPrismatic() based on curlink

Definition at line 54 of file IKThreeLink.h.

Referenced by solve().

void IKThreeLink::computeSecondLinkPrismatic	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

sets the length of curlink based on the distance of the objective point, projecting about parent link's z axis (which will be set from computeFirstLink())

First link will perform some rotation, this function needs to set the distance to match the projection of the objective into the plane of rotation. Very similar to computeThirdLinkPrismatic() except how this projection is done (no ring of motion)

Definition at line 305 of file IKThreeLink.cc.

Referenced by computeSecondLink().

void IKThreeLink::computeSecondLinkRevolute	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

sets the angle of curlink based on the elevation of the objective point vs effector point, projecting about parent link's z axis (which will be set from computeFirstLink())

assumes that z axis of parent frame and current frame are either parallel or orthogonal

todo: would be nice if this could handle intermediary angles... currently anything not parallel is assumed to be orthogonal

Definition at line 186 of file IKThreeLink.cc.

Referenced by computeSecondLink().

void IKThreeLink::computeThirdLink	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

forwards to either computeThirdLinkRevolute() or computeThirdLinkPrismatic() based on curlink

Definition at line 67 of file IKThreeLink.h.

Referenced by solve().

void IKThreeLink::computeThirdLinkPrismatic	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

sets the length of curlink based on length of thigh (distance from curlink to parent origin), the angle between the parent and effector point, and the distance from parent to objective.

This version is intended mainly for solving to have a pan-tilt camera look at a point in space. The camera is the third link, and should be marked as prismatic. There can be immobile frames between this and the tilt joint, as we often want a specific configuration of x and y axes to match image coordinates. Z must point out of the camera, toward the scene.

Plink is ignored. This only solves for the z axis, and that must intersect the pan axis. (The math gets really hairy if the camera is not aligned to a radial from the pan joint.)

The approach is similar to computeThirdLinkRevolute(), using

length of the neck, i.e. distance from tilt to camera
angle at the camera (curlink) between neck link and z axis
distance from tilt's ring of motion to Pobj (ringObjD)

Definition at line 476 of file IKThreeLink.cc.

Referenced by computeThirdLink().

void IKThreeLink::computeThirdLinkRevolute	(	KinematicJoint &	curlink,
		const fmat::Column< 3 > &	Pobj,
		KinematicJoint &	endlink,
		const fmat::Column< 3 > &	Plink,
		bool	invert,
		bool &	valid
	)			const `[protected]`

sets the angle of curlink based on length of thigh (distance from curlink to parent origin), and distance from curlink to effector point, to achieve desired distance from parent to effector.

We'll compute the knee angle first, using:

length of the thigh ('thigh')
distance from knee (curlink) to Plink, projected to plane of curlink's rotation (cEffD)
distance from shoulder (previous link) to Pobj (ringObjD)

Two knee (curlink) configurations are supported: parallel to parLink (where parLink.d can be bundled as a z offset of the effector) or parallel to grandparent, with parLink.r==0. In the former case, the parent link's origin forms a ring about the grandparent's link of radius parLink.r. It is the distance of the objective to the ring (or just the center point when parLink.r==0) that curlink must match the effector's distance.

Definition at line 341 of file IKThreeLink.cc.

Referenced by computeThirdLink().

static float IKThreeLink::normalize_angle ( float x ) [static, protected]

ensures that t is in the range ±π (upper boundary may not be inclusive...?)

Definition at line 83 of file IKThreeLink.h.

Referenced by computeFirstLinkRevolute(), computeSecondLinkRevolute(), and computeThirdLinkRevolute().

unsigned int IKThreeLink::setLinks	(	KinematicJoint &	eff,
		KinematicJoint *	links[],
		unsigned int	remain
	)			`[static, protected]`

searches eff parents to assign remain mobile joints into links

Definition at line 137 of file IKThreeLink.cc.

Referenced by solve().

bool IKThreeLink::solve	(	const Point &	pEff,
		const Rotation &	oriEff,
		KinematicJoint &	j,
		const Position &	pTgt,
		float	posPri,
		const Orientation &	oriTgt,
		float	oriPri
	)			const `[virtual]`

Solve to get an 'effector' (pEff, oriEff, relative to link following j) to a solution of pTgt, oriTgt (or at least a local minimum).

Parameters:

	pEff	The point on the effector that should be brought to the target. Typically [0,0,0] if the effector is GripperFrame.
	oriEff	Rotation of the effector's reference frame; used to align some effector axis with the target's Orientation axes. For example, if oriTgt is a Parallel constraint, the effector will be free to rotate about the target's z-axis. If we want the effector to rotate about its y-axis, we use oriEff to rotate the effector y-axis to the target's z-axis.
	j	The kinematic joint at the end of the chain (e.g., GripperFrame)
	pTgt	The Position constraint that pEff should satisfy, i.e., the target location
	oriTgt	The Orientation constraint that pEff should satisfy at the target location
	posPri,oriPri	Relative priorities (weightings) of position and orientation solutions in case they are mutually exclusive

Implements IKSolver.

Definition at line 14 of file IKThreeLink.cc.

Referenced by step().

IKSolver::StepResult_t IKThreeLink::step	(	const Point &	pEff,
		const Rotation &	oriEff,
		KinematicJoint &	j,
		const Position &	pTgt,
		float	pDist,
		float	posPri,
		const Orientation &	oriTgt,
		float	oriDist,
		float	oriPri
	)			const `[virtual]`

Move an 'effector' (pEff, oriEff, relative to link following j) towards a solution of pTgt, oriTgt (or at least a local minimum).

pDist and oriDist specifies the maximum distance to move towards each solution; posPri and oriPri specify relative weighting of each solution in case they are mutually exclusive

Implements IKSolver.

Definition at line 117 of file IKThreeLink.cc.