javax.robotics.engine.clik
Class ClikJacobianInverse

java.lang.Object
  javax.robotics.engine.clik.ClikJacobianInverse

All Implemented Interfaces:

CLIK

Direct Known Subclasses:

ClikJacobianTranspose

public class ClikJacobianInverse
extends java.lang.Object
implements CLIK

Closed Loop Inverse Kinematics with Jacobian inverse. The class implements this scheme:

The inverse is computed using SVD decomposition:

J^-1 = J^T*(J*J^T + λ²*I)^-1
where λ² is the damped factor equals 1 - (σ_r/ε)*λ²_max if σ_r< ε, equals 0 if σ_r≥ ε; σ_r is the smallest not null singular value.

The orientation error is computed as quaternion error.
The integration method used is the forward Euler.

Since:

1.0.2

Version:

11/11/2005

Author:

Carmine Lia

Constructor Summary

ClikJacobianInverse(Robot robot, DMatrix Kp, DMatrix Ko)
Costructor with default sample time, dt = 1e-3 and eps = 0.02, lambdaMax=0.02.

ClikJacobianInverse(Robot robot, DMatrix Kp, DMatrix Ko, double dt)
Costructor with eps = 0.02, lambdaMax=0.02.

ClikJacobianInverse(Robot robot, DMatrix Kp, DMatrix Ko, double dt, double eps, double lambdaMax)
Constructor.

Method Summary

double	getEps() Gets the singular region area.
RVector	getJointsPosition() Gets the current joints position.
RVector	getJointsVelocity() Gets the current joints velocity.
DMatrix	getKo() Gets the matrix of error orientation gain.
DMatrix	getKp() Gets the matrix of error position gain.
double	getLambdaMax() Gets the maximum damped factor.
RVector3d	getOrientationError() Gets the current orientation error.
RVector3d	getPositionError() Gets the current position error.
double	getSampleTime() Gets the sample time.
void	setEps(double eps) Sets the singular region area.
void	setKo(DMatrix ko) Sets the matrix of error orientation gain.
void	setKp(DMatrix kp) Sets the matrix of error position gain.
void	setLambdaMax(double lambdaMax) Sets the maximum damped factor.
void	setSampleTime(double h) Sets the sample time.
void	step(RVector3d pd, RVector3d ppd, Quaternion quatd, RVector3d wd) Compute the joints position and velocity from a given desired end effector position and orientation.

Methods inherited from class java.lang.Object

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

Constructor Detail

ClikJacobianInverse

public ClikJacobianInverse(Robot robot,
                           DMatrix Kp,
                           DMatrix Ko)

Costructor with default sample time, dt = 1e-3 and eps = 0.02, lambdaMax=0.02.

Parameters:

robot - the robot.

Kp - the diagonal matrix of error position gain.

Ko - the diagonal matrix of error orientationo gain.

ClikJacobianInverse

public ClikJacobianInverse(Robot robot,
                           DMatrix Kp,
                           DMatrix Ko,
                           double dt)

Costructor with eps = 0.02, lambdaMax=0.02.

Parameters:

robot - the robot.

Kp - the diagonal matrix of error position gain.

Ko - the diagonal matrix of error orientationo gain.

dt - the sample time.

ClikJacobianInverse

public ClikJacobianInverse(Robot robot,
                           DMatrix Kp,
                           DMatrix Ko,
                           double dt,
                           double eps,
                           double lambdaMax)

Constructor.

Parameters:

robot - the robot.

Kp - the diagonal matrix of error position gain.

Ko - the diagonal matrix of error orientationo gain.

dt - the sample time.

eps - the area of singular region.

lambdaMax - the maximum of damped factor.

Method Detail

getEps

public final double getEps()

Gets the singular region area.

Specified by:

getEps in interface CLIK

Returns:

the eps.

setEps

public final void setEps(double eps)

Sets the singular region area.

Specified by:

setEps in interface CLIK

Parameters:

eps - The eps to set.

getSampleTime

public final double getSampleTime()

Gets the sample time.

Specified by:

getSampleTime in interface CLIK

Returns:

the sample time.

setSampleTime

public final void setSampleTime(double h)

Sets the sample time.

Specified by:

setSampleTime in interface CLIK

Parameters:

h - The sample time to set.

getKp

public final DMatrix getKp()

Gets the matrix of error position gain.

Specified by:

getKp in interface CLIK

Returns:

the diagonal matrix.

setKp

public final void setKp(DMatrix kp)

Sets the matrix of error position gain.

Specified by:

setKp in interface CLIK

Parameters:

kp - The diagonal matrix to set.

getKo

public final DMatrix getKo()

Gets the matrix of error orientation gain.

Specified by:

getKo in interface CLIK

Returns:

the diagonal matrix.

setKo

public final void setKo(DMatrix ko)

Sets the matrix of error orientation gain.

Specified by:

setKo in interface CLIK

Parameters:

ko - The diagonal matrix to set.

getLambdaMax

public final double getLambdaMax()

Gets the maximum damped factor.

Specified by:

getLambdaMax in interface CLIK

Returns:

the damped factor.

setLambdaMax

public final void setLambdaMax(double lambdaMax)

Sets the maximum damped factor.

Specified by:

setLambdaMax in interface CLIK

Parameters:

lambdaMax - The damped factor to set.

getPositionError

public final RVector3d getPositionError()

Gets the current position error.

Specified by:

getPositionError in interface CLIK

Returns:

the position error vector.

getOrientationError

public final RVector3d getOrientationError()

Gets the current orientation error. The error is computed form quaternion as:

η(q)&epsilon_d - &eta_d&epsilon(q) - S(&epsilon_d)&epsilon(q)

where η and ε are the vector and the scalar components of quaternion respectively.

Specified by:

getOrientationError in interface CLIK

Returns:

the orientation error vector.

getJointsPosition

public final RVector getJointsPosition()

Gets the current joints position.

Specified by:

getJointsPosition in interface CLIK

Returns:

the joints position vector.

getJointsVelocity

public final RVector getJointsVelocity()

Gets the current joints velocity.

Specified by:

getJointsVelocity in interface CLIK

Returns:

the joints velocity vector.

step

public void step(RVector3d pd,
                 RVector3d ppd,
                 Quaternion quatd,
                 RVector3d wd)

Compute the joints position and velocity from a given desired end effector position and orientation. The joints velocity is computed as:

the joints position is computed by Euler integration:
q(t+dt) = q(t) + q'(t)*dt;

Specified by:

step in interface CLIK

Parameters:

pd - the vector of desired end effector position.

ppd - the vector of desired end effector velocity.

quatd - the desired end effector quaternion.

wd - the desired end effector angular velocity.