us.ihmc.utilities.screwTheory
Class GeometricJacobian

java.lang.Object
  extended by us.ihmc.utilities.screwTheory.GeometricJacobian

public class GeometricJacobian
extends java.lang.Object


Constructor Summary
GeometricJacobian(InverseDynamicsJoint[] joints, ReferenceFrame jacobianFrame)
           
GeometricJacobian(InverseDynamicsJoint joint, java.util.List<Twist> unitTwists, ReferenceFrame jacobianFrame)
           
GeometricJacobian(InverseDynamicsJoint joint, ReferenceFrame jacobianFrame)
           
GeometricJacobian(RigidBody ancestor, RigidBody descendant, ReferenceFrame jacobianFrame)
           
 
Method Summary
 void changeFrame(ReferenceFrame jacobianFrame)
           
 void compute()
          Computes the Jacobian.
 org.ejml.data.DenseMatrix64F computeJointTorques(Wrench wrench)
          returns the joint torque vector that corresponds to the given wrench
 double det()
           
 RigidBody getBase()
           
 ReferenceFrame getBaseFrame()
           
 RigidBody getEndEffector()
           
 ReferenceFrame getEndEffectorFrame()
           
 double getJacobianEntry(int row, int column)
          Returns one entry from the Jacobian matrix.
 ReferenceFrame getJacobianFrame()
           
 org.ejml.data.DenseMatrix64F getJacobianMatrix()
          Returns a reference to the underlying Matrix object.
 InverseDynamicsJoint[] getJointsInOrder()
           
 int getNumberOfColumns()
           
 java.lang.String getShortInfo()
           
 void packTwist(Twist twistToPack, org.ejml.data.DenseMatrix64F jointVelocities)
          packs the twist that corresponds to the given joint velocity vector
 java.lang.String toString()
           
 
Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, wait, wait, wait
 

Constructor Detail

GeometricJacobian

public GeometricJacobian(InverseDynamicsJoint joint,
                         java.util.List<Twist> unitTwists,
                         ReferenceFrame jacobianFrame)

GeometricJacobian

public GeometricJacobian(RigidBody ancestor,
                         RigidBody descendant,
                         ReferenceFrame jacobianFrame)

GeometricJacobian

public GeometricJacobian(InverseDynamicsJoint[] joints,
                         ReferenceFrame jacobianFrame)

GeometricJacobian

public GeometricJacobian(InverseDynamicsJoint joint,
                         ReferenceFrame jacobianFrame)
Method Detail

compute

public void compute()
Computes the Jacobian.


changeFrame

public void changeFrame(ReferenceFrame jacobianFrame)

packTwist

public void packTwist(Twist twistToPack,
                      org.ejml.data.DenseMatrix64F jointVelocities)
packs the twist that corresponds to the given joint velocity vector

Parameters:
jointVelocities - the joint velocity vector; should have the same ordering as the unitRelativeTwistsInBodyFrame that were passed in during construction, should be a column vector

computeJointTorques

public org.ejml.data.DenseMatrix64F computeJointTorques(Wrench wrench)
returns the joint torque vector that corresponds to the given wrench

Parameters:
wrench - the resulting wrench at the end effector. The wrench should be expressed in this Jacobian's jacobianFrame and its 'onWhat' frame should be this Jacobian's endEffectorFrame
Returns:
joint torques that result in the given wrench as a column vector

getJacobianMatrix

public org.ejml.data.DenseMatrix64F getJacobianMatrix()
Returns a reference to the underlying Matrix object. Does not recompute the Jacobian.

Returns:
a reference to the underlying Matrix object

det

public double det()
Returns:
the determinant of the Jacobian matrix

getJacobianEntry

public double getJacobianEntry(int row,
                               int column)
Returns one entry from the Jacobian matrix. Does not recompute the Jacobian.

Parameters:
row - the desired row of the Jacobian
column - the desired column of the Jacobian
Returns:
the entry at (row, column)

getNumberOfColumns

public int getNumberOfColumns()
Returns:
the number of columns in the Jacobian matrix

getJacobianFrame

public ReferenceFrame getJacobianFrame()
Returns:
the frame in which this Jacobian is expressed

getJointsInOrder

public InverseDynamicsJoint[] getJointsInOrder()

getBase

public RigidBody getBase()

getEndEffector

public RigidBody getEndEffector()

getBaseFrame

public ReferenceFrame getBaseFrame()

getEndEffectorFrame

public ReferenceFrame getEndEffectorFrame()

toString

public java.lang.String toString()
Overrides:
toString in class java.lang.Object

getShortInfo

public java.lang.String getShortInfo()