private void readAndUpdateRootJointPositionAndOrientation() { packRootTransform(robot, temporaryRootToWorldTransform); temporaryRootToWorldTransform.normalizeRotationPart(); rootJoint.setJointConfiguration(temporaryRootToWorldTransform); }
private void readAndUpdateRootJointPositionAndOrientation() { packRootTransform(robot, temporaryRootToWorldTransform); temporaryRootToWorldTransform.normalizeRotationPart(); sensorOutputMap.setRootJointTransform(temporaryRootToWorldTransform); rootJointReferenceFrame.setTransformAndUpdate(temporaryRootToWorldTransform); }
public void updateJointPositions_SCS_to_ID() { if (scsFloatingJoint != null) { scsFloatingJoint.getTransformToWorld(transformToWorld); transformToWorld.normalizeRotationPart(); idFloatingJoint.setJointConfiguration(transformToWorld); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue()); } }
this.transformToParent.normalizeRotationPart();
referenceFrame.transformToRoot.normalizeRotationPart();
@Test public void testDeterminantRotationPart() throws Exception { Random random = new Random(42353L); RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform transform = new RigidBodyTransform(original); double corruptionFactor = 0.1; double m00 = original.getM00() * corruptionFactor; double m01 = original.getM01() * corruptionFactor; double m02 = original.getM02() * corruptionFactor; double m10 = original.getM10() * corruptionFactor; double m11 = original.getM11() * corruptionFactor; double m12 = original.getM12() * corruptionFactor; double m20 = original.getM20() * corruptionFactor; double m21 = original.getM21() * corruptionFactor; double m22 = original.getM22() * corruptionFactor; transform.setRotationUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); assertTrue(transform.determinantRotationPart() < corruptionFactor); transform.normalizeRotationPart(); assertEquals(1.0, transform.determinantRotationPart(), EPS); }
double m22 = original.getM22() + corruptionFactor * random.nextDouble(); transform.setRotationUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); transform.normalizeRotationPart(); transform.normalizeRotationPart();
@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(); }