hmi.animation
Class AnalyticalIKSolver

java.lang.Object
  hmi.animation.AnalyticalIKSolver

public class AnalyticalIKSolver
extends Object

IK calculations for a 7 DOF Joint construction (e.g. arm/leg) Loosely based on the IKAN toolkit: Tolani, Deepak, Goswami, Ambarish and Badler, Norman I., Real-time inverse kinematics techniques for anthropomorphic limbs (2000), in: Graphical Models and Image Processing, 62:5(353--388) Given matrices G, S, T solves the equation G = S*Rx*T*R1 for R1,Rx where R1 represent general rotation matrices Ry represents a rotation about the y axis and G is the desired goal matrix S, T are constant matrices In the case of the arm: S : Wrist to Elbow transformation Rx : Elbow joint T : Elbow to Shoulder transformation R1 : Shoulder joints This method assumes a close to HAnim resting pose

Author:

welberge

Nested Class Summary

static class

AnalyticalIKSolver.LimbPosition

Field Summary

private float[]	a
private float[]	aRot
private float[]	axis
private float[]	backAxis
private float[]	C
private static double	DOT_MARGIN
private float[]	e
private float[]	elbowRotAxis
private float	elbowStartRotation
private float[]	eNorm
private float[]	forward
private float[]	goal
(package private) float	l1
(package private) float	l2
static int	LEFT
private AnalyticalIKSolver.LimbPosition	limbPosition
private float[]	n
private float[]	planeNormal
private boolean	project
private float	projectionLength
private float	R
private float[]	R1
static int	RIGHT
private static double	ROT_MARGIN
private float[]	Rx
private float[]	sewT
private float[]	Sv
private double	swivel
private float[]	tempv
private float[]	Tv
private float[]	u
private float[]	v
private float[]	w
private float[]	WH
private float[]	x
private float[]	y
private float[]	z

Constructor Summary

AnalyticalIKSolver(float[] Sv, float[] Tv, AnalyticalIKSolver.LimbPosition lp)
Constructor

AnalyticalIKSolver(float[] Sv, float[] Tv, AnalyticalIKSolver.LimbPosition limbPosition, float projectionLength)
Constructor, creates a IK system with projection enabled

Method Summary

float[]	getA()
float[]	getC()
double	getR()
float[]	getR1()
float[]	getRx()
double	getSwivel(float[] e, float[] g) Calculate the swivel angle, given an elbow/wrist position and a goal
void	setProject(boolean proj) Project onto local sphere?
void	setProjectionLength(float length) Set the length (=radius of projection sphere)
void	setSwivel(double swivel) set the swivel angle
void	solve(float[] goal, float[] qSho, float[] qElb) Solves the joint configurations for a goal and the current end effector
boolean	solveIt(float[] g) Solve the elbow and shoulder rotations for a certain rotation goal
private boolean	solveRy(double L1, double L2, double L3) Find the elbow rotation
static void	translateToLocalSystem(VJoint obj1, VJoint obj2, float[] src, float[] dst) Translates a vector src in obj1-coordinates to the local coordinate system for a visual object obj2, excluding obj2's rotation

Methods inherited from class java.lang.Object

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

Field Detail

Sv

private float[] Sv

Tv

private float[] Tv

WH

private float[] WH

Rx

private float[] Rx

R1

private float[] R1

LEFT

public static final int LEFT

See Also:

Constant Field Values

RIGHT

public static final int RIGHT

See Also:

Constant Field Values

swivel

private double swivel

projectionLength

private float projectionLength

project

private boolean project

sewT

private float[] sewT

R

private float R

elbowStartRotation

private float elbowStartRotation

C

private float[] C

goal

private float[] goal

a

private float[] a

elbowRotAxis

private float[] elbowRotAxis

limbPosition

private AnalyticalIKSolver.LimbPosition limbPosition

ROT_MARGIN

private static final double ROT_MARGIN

See Also:

Constant Field Values

DOT_MARGIN

private static final double DOT_MARGIN

See Also:

Constant Field Values

l1

float l1

l2

float l2

x

private float[] x

y

private float[] y

z

private float[] z

w

private float[] w

n

private float[] n

e

private float[] e

tempv

private float[] tempv

forward

private float[] forward

axis

private float[] axis

backAxis

private float[] backAxis

planeNormal

private float[] planeNormal

aRot

private float[] aRot

eNorm

private float[] eNorm

u

private float[] u

v

private float[] v

Constructor Detail

AnalyticalIKSolver

public AnalyticalIKSolver(float[] Sv,
                          float[] Tv,
                          AnalyticalIKSolver.LimbPosition lp)

Constructor

Parameters:

Sv - wrist to elbow vector in the shoulder coordinate system

Tv - elbow to shoulder vector in the shoulder coordinate system

lp - arm or leg

AnalyticalIKSolver

public AnalyticalIKSolver(float[] Sv,
                          float[] Tv,
                          AnalyticalIKSolver.LimbPosition limbPosition,
                          float projectionLength)

Constructor, creates a IK system with projection enabled

Parameters:

Sv - wrist to elbow vector

Tv - elbow to shoulder vector

limbPosition - arm or leg

projectionLength -

Method Detail

setSwivel

public void setSwivel(double swivel)

set the swivel angle

Parameters:

swivel - the new swivel angle

solve

public void solve(float[] goal,
                  float[] qSho,
                  float[] qElb)

Solves the joint configurations for a goal and the current end effector

Parameters:

goal - goal position

qSho - quaternion rotation around shoulder(-like) joint

qElb - quaternion rotation around elbow(-like) joint

getSwivel

public double getSwivel(float[] e,
                        float[] g)

Calculate the swivel angle, given an elbow/wrist position and a goal

Parameters:

e - elbow/wrist position in shoulder/hip coordinates

g - the goal position

Returns:

the swivel angle

solveIt

public boolean solveIt(float[] g)

Solve the elbow and shoulder rotations for a certain rotation goal

Parameters:

g - the goal wrist position

Returns:

true is solvable, false otherwise

solveRy

private boolean solveRy(double L1,
                        double L2,
                        double L3)

Find the elbow rotation

getR1

public float[] getR1()

Returns:

Returns the R1.

getRx

public float[] getRx()

Returns:

Returns the Rx.

getC

public float[] getC()

Returns:

Returns the center of the elbow rotation circle.

getR

public double getR()

Returns:

Returns the radius of the elbow rotation circle.

getA

public float[] getA()

setProject

public void setProject(boolean proj)

Project onto local sphere?

Parameters:

proj - new project value

setProjectionLength

public void setProjectionLength(float length)

Set the length (=radius of projection sphere)

Parameters:

length - the new projection length

translateToLocalSystem

public static void translateToLocalSystem(VJoint obj1,
                                          VJoint obj2,
                                          float[] src,
                                          float[] dst)

Translates a vector src in obj1-coordinates to the local coordinate system for a visual object obj2, excluding obj2's rotation

Parameters:

obj1 - the visual object in which the src coordinates are defined

obj2 - the visual object in which the dst coordinates should be defined

src - the vector in world coordinates