Java Twist.getMatrix Examples

Example #1

File: GeometricJacobian.java Project: ihmcrobotics/ihmc-open-robotics-software

 /** Computes the Jacobian. */
 public void compute() {
   int column = 0;
   for (int i = 0; i < unitTwistList.size(); i++) {
     Twist twist = unitTwistList.get(i);
     tempTwist.set(twist);
     tempTwist.changeFrame(jacobianFrame);
     tempTwist.getMatrix(tempMatrix, 0);
     CommonOps.extract(
         tempMatrix,
         0,
         tempMatrix.getNumRows(),
         0,
         tempMatrix.getNumCols(),
         jacobian,
         0,
         column++);
   }
 }