hmi.physics.featherstone
Class RNEASolver

java.lang.Object
  hmi.physics.featherstone.RNEASolver

public class RNEASolver
extends Object

Recursive Newton-Euler inverse dynamics solver, based on Rigid Body Dynamics Algorithms Roy Featherstone 2007 Solves for a branch of ball joints, does not handle external forces (yet) TODO (contributions are welcome): - support for hinge/universal joints - support for external forces

Author:

Herwin van Welbergen

Field Summary

private float[]	ai
private float[]	cwi
private float[]	F
private float[]	I
private float[]	iX0
private int[]	lambda
private float[]	r
private int	size
private float[]	tempq
private float[]	tempv
private float[]	tempv2
private float[]	vi
private float[]	X

Constructor Summary

RNEASolver(int joints, float[] translations, float[] spatialI)
Constructor

RNEASolver(int joints, float[] translations, float[] spatialI, int[] parent)
Constructor Convention: joint 0 is always the root joint

Method Summary

void	solve(float[] f, float[] v0, float[] a0, float[] qi, float[] wi, float[] diffwi) Solves for the forces on each joint
void	solveChain(float[] f, float[] v0, float[] a0, float[] qi, float[] wi, float[] diffwi) Solves for the forces on each joint

Methods inherited from class java.lang.Object

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

Field Detail

vi

private float[] vi

ai

private float[] ai

X

private float[] X

r

private float[] r

iX0

private float[] iX0

size

private int size

tempv

private float[] tempv

tempv2

private float[] tempv2

cwi

private float[] cwi

I

private float[] I

F

private float[] F

tempq

private float[] tempq

lambda

private int[] lambda

Constructor Detail

RNEASolver

public RNEASolver(int joints,
                  float[] translations,
                  float[] spatialI,
                  int[] parent)

Constructor Convention: joint 0 is always the root joint

Parameters:

joints - number of joints

translations - joint-to-joint translation vectors (an array of at least joint*3 length)

spatialI - spatial inertia tensors (an array of at least joint*13 length)

parent - the parent array, providing the parent of each joint, -1 for root

RNEASolver

public RNEASolver(int joints,
                  float[] translations,
                  float[] spatialI)

Constructor

Parameters:

joints - number of joints

translations - joint-to-joint translation vectors (an array of at least joint*3 length)

spatialI - spatial inertia tensors (an array of at least joint*13 length)

Method Detail

solve

public void solve(float[] f,
                  float[] v0,
                  float[] a0,
                  float[] qi,
                  float[] wi,
                  float[] diffwi)

Solves for the forces on each joint

Parameters:

f - output: spatial force on each joint

v0 - spatial velocity of the base frame

a0 - spatial acceleration of the base frame

qi - quaternion joint rotations

wi - local joint angular velocities

diffwi - local joint angular accelerations

solveChain

public void solveChain(float[] f,
                       float[] v0,
                       float[] a0,
                       float[] qi,
                       float[] wi,
                       float[] diffwi)

Solves for the forces on each joint

Parameters:

f - output: spatial force on each joint

v0 - spatial velocity of the base frame

a0 - spatial acceleration of the base frame

qi - quaternion joint rotations

wi - local joint angular velocities

diffwi - local joint angular accelerations