public void get(Matrix3d matrixToPack) { putYoValuesIntoFrameOrientation(); frameOrientation.getMatrix3d(matrixToPack); }
public void update(FrameOrientation currentOrientation) { checkReferenceFrameMatch(currentOrientation); currentOrientation.getMatrix3d(currentOrientationMatrix); update(currentOrientationMatrix); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { initialOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { currentOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { initialOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
private void applyCorrection(FramePoint desiredPosition, FrameOrientation desiredOrientation) { ReferenceFrame originalFrame = desiredPosition.getReferenceFrame(); desiredPosition.changeFrame(controlFrame); desiredOrientation.changeFrame(controlFrame); desiredPosition.sub(totalLinearCorrection); totalAngularCorrection.negate(); RotationTools.convertRotationVectorToAxisAngle(totalAngularCorrection.getVector(), angularDisplacementAsAxisAngle); angularDisplacementAsMatrix.set(angularDisplacementAsAxisAngle); desiredOrientation.getMatrix3d(correctedRotationMatrix); correctedRotationMatrix.mul(angularDisplacementAsMatrix, correctedRotationMatrix); desiredOrientation.set(correctedRotationMatrix); desiredPosition.changeFrame(originalFrame); desiredOrientation.changeFrame(originalFrame); }
modifiableVoxelLocation.changeFrame(jacobian.getBaseFrame()); orientation.changeFrame(jacobian.getBaseFrame()); orientation.getMatrix3d(rotationMatrix);