shady.common.multi.chain
Class ChainIK

java.lang.Object
  extended by 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