javax.robotics.engine.clik
Class ClikJacobianPosTranspose

java.lang.Object
  extended by javax.robotics.engine.clik.ClikJacobianPosInverse
      extended by javax.robotics.engine.clik.ClikJacobianPosTranspose
All Implemented Interfaces:
CLIKR

public class ClikJacobianPosTranspose
extends ClikJacobianPosInverse

Closed Loop Inverse Kinematics with positional Jacobian transpose. The class implements the same scheme depicted in ClikJacobianTranspose:

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

Since:
1.0.2
Version:
11/11/2005
Author:
Carmine Lia

Constructor Summary
ClikJacobianPosTranspose(Robot robot, DMatrix Kp, int spaceDim)
          Costructor with default sample time, dt = 1e-3.
ClikJacobianPosTranspose(Robot robot, DMatrix Kp, int spaceDim, double dt)
          Costructor.
 
Method Summary
 void step(RVector pd, RVector ppd)
          Compute the joints position and velocity from a given desired end effector position and velocity.
 
Methods inherited from class javax.robotics.engine.clik.ClikJacobianPosInverse
getCartesianSpaceDim, getEps, getJointsPosition, getJointsVelocity, getKp, getLambdaMax, getPositionError, getSampleTime, setEps, setKp, setLambdaMax, setSampleTime, step
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Constructor Detail

ClikJacobianPosTranspose

public ClikJacobianPosTranspose(Robot robot,
                                DMatrix Kp,
                                int spaceDim)
Costructor with default sample time, dt = 1e-3.

Parameters:
robot - the robot.
Kp - the diagonal matrix of error position gain.
spaceDim - the dimension of robot operational space.

ClikJacobianPosTranspose

public ClikJacobianPosTranspose(Robot robot,
                                DMatrix Kp,
                                int spaceDim,
                                double dt)
Costructor.

Parameters:
robot - the robot.
Kp - the diagonal matrix of error position gain.
spaceDim - the dimension of robot operational space.
dt - the sample time.
Method Detail

step

public void step(RVector pd,
                 RVector ppd)
Compute the joints position and velocity from a given desired end effector position and velocity. The joints velocity is computed as:
q'=trans(J)*Ke
the joints position is computed by Euler integration:
q(t+dt) = q(t) + q'(t)*dt;

Specified by:
step in interface CLIKR
Overrides:
step in class ClikJacobianPosInverse
Parameters:
pd - the vector of desired end effector position.
ppd - the parameter is not considered.