shady.common.multi.chain
Class ChainIK

java.lang.Object
  shady.common.multi.chain.ChainIK

Direct Known Subclasses:

DLSIK, PseudoinverseIK, TransposeIK

public abstract class ChainIK
extends java.lang.Object

Base class for Inverse Kinematics for a MultiShady chain.

At a minimum, concrete subclasses must implement computeDOFDelta(int). Other protected methods may be overridden for additional customization.

To perform IK, instantiate a concrete subclass for a particular chain, and then call gotoTarget(double, double, double, int, double) as much as desired.

Copyright (C) 2006 Marsette A. Vona, III

This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

Author:

Marsette (Marty) A. Vona, III

Field Summary

protected vona.math.RigidTransform2D	blockEndToChainEnd Transform from current block end frame to chain end frame (end frame of chainEnd.
protected vona.math.RigidTransform2D	blockStartToEnd Transform from current block start to end frame.
protected vona.math.RigidTransform2D	blockStartToEndWithDelta Transform from current block start to end frame, including delta.
protected Block	chainEnd Last block in chain.
protected vona.math.RigidTransform2D	chainEndToWorld Transform from chain end frame (end frame of chainEnd) to world frame.
protected vona.math.RigidTransform2D	chainEndToWorldWithDelta Transform from chain end frame (end frame of chainEnd) to world frame, including delta in current block.
protected Block	chainStart First block in chain.
private static java.lang.String	cvsid CVS id.
static boolean	DEBUG Whether to debug.
static int	DEBUG_ITERATION Iteration modulus at which to debug, if DEBUG.
static double	DEFAULT_DOF_DELTA_CLAMP Default DOF delta component magnitude clamp.
static double	DEFAULT_TARGET_CLAMP Default target vector magnitude clamp.
protected double[]	dofDelta n DOF deltas found by computeDOFDelta(int), in order from chain start to end.
double	dofDeltaClamp dofDelta vector per-component magnitude clamp.
protected double	dt (Canonicalized and clamped) ee vector to target rotation component.
protected double	dx (Clamped) ee vector to target x component.
protected double	dy (Clamped) ee vector to target y component.
protected Jama.Matrix	jacobian Jacobian matrix.
static double	JACOBIAN_DELTA Delta passed to Block.recompute(RigidTransform2D, double) to compute Jacobian columns.
protected int	n Number of non-locked Blocks.
protected Sentinel	sentinel Chain sentinel.
double	targetClamp EE to target vector magnitude clamp.
protected vona.math.RigidTransform2D	worldToBlockStart Transform from world frame to current block start frame.
protected vona.math.RigidTransform2D	worldToChainEnd Transform from world frame to chain end frame (end frame of chainEnd).
protected vona.math.RigidTransform2D	worldToChainEndWithDelta Transform from world frame to chain end frame (end frame of chainEnd), including delta in current block.

Constructor Summary

ChainIK(Block chainEnd)
Create a new ChainIK for a given chain.

Method Summary

protected void	clampDOFDelta() Clamp dofDelta vector.
protected void	clampDOFDeltaMagnitude(double max) Clamp magnitude of each component of dofDelta vector.
protected void	clampTarget() Clamp vector to target (dx, dy, dt).
protected void	clampTargetMagnitude(double max) Clamp magnitude of vector to target (dx, dy, dt).
protected boolean	closeEnough(int iteration, double targetSlop) Check whether the current (un-clamped) vector to target (dx, dy, dt) is close enough.
protected abstract void	computeDOFDelta(int iteration) Subclasses must implement this to compute dofDelta vector from current jacobian and vector to target (dx, dy, dt).
boolean	gotoTarget(double x, double y, double t, int maxIterations, double targetSlop) IK to an end-effector space target (x, y, t).
protected void	updateJacobian(int iteration) Update the jacobian matrix.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

DEBUG

public static final boolean DEBUG

Whether to debug.

See Also:

Constant Field Values

DEBUG_ITERATION

public static final int DEBUG_ITERATION

Iteration modulus at which to debug, if DEBUG.

See Also:

Constant Field Values

JACOBIAN_DELTA

public static final double JACOBIAN_DELTA

Delta passed to Block.recompute(RigidTransform2D, double) to compute Jacobian columns.

See Also:

Constant Field Values

DEFAULT_TARGET_CLAMP

public static final double DEFAULT_TARGET_CLAMP

Default target vector magnitude clamp.

See Also:

Constant Field Values

DEFAULT_DOF_DELTA_CLAMP

public static final double DEFAULT_DOF_DELTA_CLAMP

Default DOF delta component magnitude clamp.

See Also:

Constant Field Values

cvsid

private static final java.lang.String cvsid

CVS id.

See Also:

Constant Field Values

chainEnd

protected final Block chainEnd

Last block in chain.

chainStart

protected final Block chainStart

First block in chain.

sentinel

protected final Sentinel sentinel

Chain sentinel.

jacobian

protected Jama.Matrix jacobian

Jacobian matrix.

Three rows (0 = target x, 1 = target y, 2 = target rotation in radians), n columns in order from chainStart to chainEnd.

n

protected final int n

Number of non-locked Blocks.

dx

protected double dx

(Clamped) ee vector to target x component.

dy

protected double dy

(Clamped) ee vector to target y component.

dt

protected double dt

(Canonicalized and clamped) ee vector to target rotation component.

dofDelta

protected double[] dofDelta

n DOF deltas found by computeDOFDelta(int), in order from chain start to end.

targetClamp

public double targetClamp

EE to target vector magnitude clamp.

Used in default impl of clampTarget().

dofDeltaClamp

public double dofDeltaClamp

dofDelta vector per-component magnitude clamp.

Used in default impl of clampDOFDelta().

worldToBlockStart

protected final vona.math.RigidTransform2D worldToBlockStart

Transform from world frame to current block start frame.

Used in updateJacobian(int).

blockStartToEnd

protected final vona.math.RigidTransform2D blockStartToEnd

Transform from current block start to end frame.

Used in updateJacobian(int).

blockStartToEndWithDelta

protected final vona.math.RigidTransform2D blockStartToEndWithDelta

Transform from current block start to end frame, including delta.

Used in updateJacobian(int).

blockEndToChainEnd

protected final vona.math.RigidTransform2D blockEndToChainEnd

Transform from current block end frame to chain end frame (end frame of chainEnd.

Used in updateJacobian(int).

worldToChainEnd

protected final vona.math.RigidTransform2D worldToChainEnd

Transform from world frame to chain end frame (end frame of chainEnd).

chainEndToWorld

protected final vona.math.RigidTransform2D chainEndToWorld

Transform from chain end frame (end frame of chainEnd) to world frame.

worldToChainEndWithDelta

protected final vona.math.RigidTransform2D worldToChainEndWithDelta

Transform from world frame to chain end frame (end frame of chainEnd), including delta in current block.

Used in updateJacobian(int).

chainEndToWorldWithDelta

protected final vona.math.RigidTransform2D chainEndToWorldWithDelta

Transform from chain end frame (end frame of chainEnd) to world frame, including delta in current block.

Used in updateJacobian(int).

Constructor Detail

ChainIK

public ChainIK(Block chainEnd)

Create a new ChainIK for a given chain.

If the topology of the chain is subsequently mutated in any way (including changing any Block.lock() status), a new ChainIK must be consed.

Parameters:

chainEnd - the end of the chain to control, not null. Note that the actual chain may be longer, but IK control will only be applied to (non-locked) blocks at or before chainEnd.

Method Detail

gotoTarget

public boolean gotoTarget(double x,
                          double y,
                          double t,
                          int maxIterations,
                          double targetSlop)

IK to an end-effector space target (x, y, t).

Iterates until closeEnough(int, double) or until maxIterations.

For each iteration we

1. compute nominal worldToChainEnd
2. updateJacobian(int)
3. compute and clamp ee vector to target (dx, dy, dt)
4. return iff closeEnough(int, double)
5. clampTarget()
6. computeDOFDelta(int) (actual IK happens here)
7. clampDOFDelta()
8. echo dofDelta to the unlocked blocks

Parameters:

x - the target x coordinate, NaN for no x movement

y - the target y coordinate, NaN for no y movement

t - the target rotation (ccw radians from world frame x-axis to EE (block end) frame x-axis), NaN for no rotation

maxIterations - the maximum number of iterations

Returns:

whether we exited closeEnough(int, double)

updateJacobian

protected void updateJacobian(int iteration)

Update the jacobian matrix.

Computes each column according to the corresponding Block.recompute(vona.math.RigidTransform2D, double) with JACOBIAN_DELTA.

closeEnough

protected boolean closeEnough(int iteration,
                              double targetSlop)

Check whether the current (un-clamped) vector to target (dx, dy, dt) is close enough.

Default impl checks that the translational are within targetSlop and the rotational component is within Math.atan2(targetSlop, ShadyCommon.CC/2.0).

Parameters:

iteration - the current iteration number

targetSlop - the max allowed target slop

Returns:

true iff close enough

clampTarget

protected void clampTarget()

Clamp vector to target (dx, dy, dt).

Default impl does clampTargetMagnitude(double)(targetClamp).

clampTargetMagnitude

protected void clampTargetMagnitude(double max)

Clamp magnitude of vector to target (dx, dy, dt).

Parameters:

max - the maximum magnitude

clampDOFDelta

protected void clampDOFDelta()

Clamp dofDelta vector.

Default impl does clampDOFDeltaMagnitude(double)(dofDeltaClamp).

clampDOFDeltaMagnitude

protected void clampDOFDeltaMagnitude(double max)

Clamp magnitude of each component of dofDelta vector.

Parameters:

max - the maximum magnitude

computeDOFDelta

protected abstract void computeDOFDelta(int iteration)

Subclasses must implement this to compute dofDelta vector from current jacobian and vector to target (dx, dy, dt).

This is the main IK step.

Parameters:

iteration - iteration number