private void generateAppliedOrientation() { randomBodyAxisAngle.set(2.0 * (Math.random() - 0.5), 2.0 * (Math.random() - 0.5), 2.0 * (Math.random() - 0.5), Math.random() * 2.0 * Math.PI); //randomBodyAxisAngle.set(0.0, 0.0, 0.0, 0.0); // for debugging randomTransformBodyToWorld.setRotationAndZeroTranslation(randomBodyAxisAngle); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationAndZeroTranslation(randomBodyAxisAngle); }
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointTransform) { jointTransform.setRotationAndZeroTranslation(getJointOrientation()); }
public void rotate(RotationMatrixReadOnly rotationMatrix) { tempTransform.setRotationAndZeroTranslation(rotationMatrix); transform.set(transform); transform.multiply(tempTransform); }
public void rotate(QuaternionReadOnly rotationQuaternion) { tempTransform.setRotationAndZeroTranslation(rotationQuaternion); transform.set(transform); transform.multiply(tempTransform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionError.changeFrame(bodyFrame); EuclidGeometryTools.axisAngleFromZUpToVector3D(positionError, rotationToControlFrame); transformToParent.setRotationAndZeroTranslation(rotationToControlFrame); } };
/** * Computes the interpolation between the two transforms using the alpha parameter to control the blend. * Note that the transforms must have a proper rotation matrix, meaning it satisfies: R'R = I and det(R) = 1 * @param transform1 * @param transform2 * @param alpha Ranges from [0, 1], where return = (1- alpha) * tansform1 + (alpha) * transform2 * @return return = (1- alpha) * tansform1 + alpha * transform2 */ public void computeInterpolation(RigidBodyTransform transform1, RigidBodyTransform transform2, RigidBodyTransform result, double alpha) { alpha = MathTools.clamp(alpha, 0.0, 1.0); transform1.get(transform1Quaternion, transform1Translation); transform2.get(transform2Quaternion, transform2Translation); interpolatedTranslation.interpolate(transform1Translation, transform2Translation, alpha); interpolatedQuaternion.interpolate(transform1Quaternion, transform2Quaternion, alpha); result.setRotationAndZeroTranslation(interpolatedQuaternion); result.setTranslation(interpolatedTranslation); }
/** * Initialize the estimation to the provided IMU orientation. This will set the IMU velocity to zero and reset all * sensor biases. * * @param orientation of the IMU in world */ public void initialize(QuaternionReadOnly orientation) { imuTransform.setRotationAndZeroTranslation(orientation); imuTwist.setToZero(imuJoint.getFrameAfterJoint(), imuJoint.getFrameBeforeJoint(), imuJoint.getFrameAfterJoint()); poseState.initialize(imuTransform, imuTwist); linearAccelerationSensor.resetBias(); angularVelocitySensor.resetBias(); }
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0); t3d.setRotationAndZeroTranslation(rotAxisAngle4d); t3d.transform(v3d);
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { QuaternionPose pose = viconClient.getQuaternionPose(bodyName); if (pose == null) { pose = new QuaternionPose(); } setDataValid(pose.dataValid); bodyToWorldQuaternion.set(pose.qx, pose.qy, pose.qz, pose.qw); bodyToWorldTransform.setRotationAndZeroTranslation(bodyToWorldQuaternion); bodyToWorldTranslation.set(pose.xPosition, pose.yPosition, pose.zPosition); bodyToWorldTransform.setTranslation(bodyToWorldTranslation); transformToParent.set(bodyToWorldTransform); setLastUpdateTimeStamp(viconClient.getModelReadingTimeStamp(bodyName)); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2, p1); eX.normalize(); eY.sub(p3, p1); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumns(eX, eY, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
actualTransform.setRotationAndZeroTranslation(axisAngle); assertTrue(actualTransform.hasRotation()); assertFalse(actualTransform.hasTranslation()); actualTransform.setRotationAndZeroTranslation(new AxisAngle(EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0), 0.0)); assertFalse(actualTransform.hasRotation()); DenseMatrix64F denseMatrix = new DenseMatrix64F(3, 3); expectedRotation.get(denseMatrix); actualTransform.setRotationAndZeroTranslation(denseMatrix); assertTrue(actualTransform.hasRotation()); assertFalse(actualTransform.hasTranslation()); actualTransform.setRotationAndZeroTranslation(CommonOps.identity(3)); assertFalse(actualTransform.hasRotation()); actualTransform.setRotationAndZeroTranslation(quaternion); assertTrue(actualTransform.hasRotation()); assertFalse(actualTransform.hasTranslation()); actualTransform.setRotationAndZeroTranslation(new Quaternion()); assertFalse(actualTransform.hasRotation()); actualTransform.setRotationAndZeroTranslation(expectedRotation); assertTrue(actualTransform.hasRotation()); assertFalse(actualTransform.hasTranslation()); actualTransform.setRotationAndZeroTranslation(new RotationMatrix()); assertFalse(actualTransform.hasRotation());
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { originVector.set(origin); xAxis.set(positionToPointAt); xAxis.sub(originVector); double norm = xAxis.length(); if (norm > 1e-12) { xAxis.scale(1.0 / norm); } else { xAxis.set(1.0, 0.0, 0.0); } zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); yAxis.normalize(); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); transformToParent.setRotationAndZeroTranslation(rotationMatrix); transformToParent.setTranslation(originVector); } }
rootJointTransform.setRotationAndZeroTranslation(rootOrientation); rootJointTransform.inverseTransform(comAngMom); comAngularMomentum.set(comAngMom);
jointTransformUpdater = transform -> { axisAngle.set(joint.getJointAxis(), joint.getQ()); transform.setRotationAndZeroTranslation(axisAngle); };
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0); t3d.setRotationAndZeroTranslation(rotAxisAngle4d); t3d.transform(v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_camera_factor / 4.0); t3d.setRotationAndZeroTranslation(rotAxisAngle4d); t3d.transform(v3d);
@Test public void testApplyTransform2() { Box3D box3d = new Box3D(); RigidBodyTransform transform = new RigidBodyTransform(); Point3D point = new Point3D(); point.set(1.0, 1.0, 1.0); transform.setTranslation(point); box3d.applyTransform(transform); box3d.getCenter(point); Point3D expectedPosition = new Point3D(1.0, 1.0, 1.0); EuclidCoreTestTools.assertTuple3DEquals(expectedPosition, point, EPSILON); Quaternion quat = new Quaternion(); quat.setYawPitchRoll(1.0, 1.0, 1.0); Quaternion quat2 = new Quaternion(quat); transform.setRotationAndZeroTranslation(quat); box3d.applyTransform(transform); box3d.getPosition(point); box3d.getOrientation(quat); expectedPosition.applyTransform(transform); EuclidCoreTestTools.assertTuple3DEquals(expectedPosition, point, EPSILON); EuclidCoreTestTools.assertQuaternionEquals(quat2, quat, EPSILON); }
inertialFrameWithRespectToLinkFrame.setRotationAndZeroTranslation(MatrixTools.IDENTITY);
@Override public void computeTransform(RigidBodyTransform currXform) { update(); CameraMountInterface cameraMount = getCameraMount(); if (isMounted() && (cameraMount != null)) { cameraMount.getTransformToCamera(currXform); return; } positionOffset.set(getCamX(), getCamY(), getCamZ()); xAxis.set(getFixX(), getFixY(), getFixZ()); fixPointNode.translateTo(getFixX(), getFixY(), getFixZ()); xAxis.sub(positionOffset); xAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); yAxis.cross(zAxis, xAxis); yAxis.normalize(); zAxis.cross(xAxis, yAxis); rotationMatrix.setColumns(xAxis, yAxis, zAxis); currXform.setRotationAndZeroTranslation(rotationMatrix); currXform.setTranslation(positionOffset); currXform.normalizeRotationPart(); }