public void compute(PointJacobian pointJacobian, FrameVector pPointVelocity) { GeometricJacobian jacobian = pointJacobian.getGeometricJacobian(); bodyFixedPoint.setIncludingFrame(pointJacobian.getPoint()); bodyFixedPoint.changeFrame(jacobian.getBaseFrame()); twistCalculator.getRelativeTwist(twist, jacobian.getBase(), jacobian.getEndEffector()); convectiveTermCalculator.computeJacobianDerivativeTerm(jacobian, convectiveTerm); convectiveTerm.changeFrame(jacobian.getBaseFrame(), twist, twist); twist.changeFrame(jacobian.getBaseFrame()); convectiveTerm.getAccelerationOfPointFixedInBodyFrame(twist, bodyFixedPoint, pPointVelocity); // bodyFixedPointVelocity.setToZero(jacobian.getBaseFrame()); // twist.packVelocityOfPointFixedInBodyFrame(bodyFixedPointVelocity, bodyFixedPoint); // // twist.packAngularPart(tempVector); // tempVector.cross(tempVector, bodyFixedPointVelocity); // pPointVelocity.add(tempVector); } }
endEffectorAcceleration.getAccelerationOfPointFixedInBodyFrame(twistOfCurrentWithRespectToNew, bodyFixedPoint, linearAccelerationToPack);