public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
private static void checkMultiplyAgainstEJML(RigidBodyTransform t1, RigidBodyTransform t2) { RigidBodyTransform t3 = new RigidBodyTransform(t1); t3.multiply(t2); DenseMatrix64F m1 = new DenseMatrix64F(4, 4); DenseMatrix64F m2 = new DenseMatrix64F(4, 4); DenseMatrix64F m3 = new DenseMatrix64F(4, 4); t1.get(m1); t2.get(m2); CommonOps.mult(m1, m2, m3); for (int row = 0; row < 4; row++) for (int column = 0; column < 4; column++) assertEquals(m3.get(row, column), t3.getElement(row, column), EPS); }
public static void setGeoregressionTransformFromVecmath(RigidBodyTransform vecmathTransform, Se3_F64 georegressionTransform) { double[] m1 = new double[16]; vecmathTransform.get(m1); double[][] rot = new double[][] { {m1[0], m1[1], m1[2]}, {m1[4], m1[5], m1[6]}, {m1[8], m1[9], m1[10]} }; DenseMatrix64F denseMatrix64F = new DenseMatrix64F(rot); georegressionTransform.setRotation(denseMatrix64F); georegressionTransform.setTranslation(m1[3], m1[7], m1[11]); }
/** * 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); }
private RigidBodyTransform randomTransform(Random random) { RigidBodyTransform transform = new RigidBodyTransform(); RigidBodyTransform tempTransform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(2 * Math.PI * random.nextDouble()); tempTransform.setRotationPitchAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); tempTransform.setRotationYawAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); double[] matrix = new double[16]; transform.get(matrix); matrix[3] = random.nextDouble(); matrix[7] = random.nextDouble(); matrix[11] = random.nextDouble(); return transform; } }
public void transform(RigidBodyTransform transform) { RotationMatrix rotation = new RotationMatrix(); Vector3D translation = new Vector3D(); transform.get(rotation, translation); translate(translation); rotate(rotation); }
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { RigidBodyTransform transformToRoot = originalFrame.getTransformToRoot(); if (previousRotation.containsNaN() || previousTranslation.containsNaN()) { transformToRoot.get(previousRotation, previousTranslation); angularVelocity.setToZero(); linearVelocity.setToZero(); } else { transformToRoot.get(rotation, translation); rotation.multiplyConjugateOther(previousRotation); rotation.getRotationVector(angularVelocity); angularVelocity.scale(1.0 / updateDT); linearVelocity.sub(translation, previousTranslation); linearVelocity.scale(1.0 / updateDT); transformToRoot.get(previousRotation, previousTranslation); transformToRoot.inverseTransform(angularVelocity); transformToRoot.inverseTransform(linearVelocity); } twistRelativeToParentToPack.setIncludingFrame(this, getParent(), this, angularVelocity, linearVelocity); } }
private void setNodeDataMessage(FootstepNodeDataMessage nodeDataMessage, FootstepNode node, int parentNodeIndex) { nodeDataMessage.setParentNodeId(parentNodeIndex); byte rejectionReason = rejectionReasons.containsKey(node) ? rejectionReasons.get(node).toByte() : (byte) 255; nodeDataMessage.setBipedalFootstepPlannerNodeRejectionReason(rejectionReason); nodeDataMessage.setRobotSide(node.getRobotSide().toByte()); nodeDataMessage.setXIndex(node.getXIndex()); nodeDataMessage.setYIndex(node.getYIndex()); nodeDataMessage.setYawIndex(node.getYawIndex()); FootstepNodeSnapData snapData = snapper.getSnapData(node); Point3D snapTranslationToSet = nodeDataMessage.getSnapTranslation(); Quaternion snapRotationToSet = nodeDataMessage.getSnapRotation(); snapData.getSnapTransform().get(snapRotationToSet, snapTranslationToSet); }
private void setNodeDataMessage(FootstepNodeDataMessage nodeDataMessage, FootstepNode node, int parentNodeIndex) { nodeDataMessage.setParentNodeId(parentNodeIndex); byte rejectionReason = rejectionReasons.containsKey(node) ? rejectionReasons.get(node).toByte() : (byte) 255; nodeDataMessage.setBipedalFootstepPlannerNodeRejectionReason(rejectionReason); nodeDataMessage.setRobotSide(node.getRobotSide().toByte()); nodeDataMessage.setXIndex(node.getXIndex()); nodeDataMessage.setYIndex(node.getYIndex()); nodeDataMessage.setYawIndex(node.getYawIndex()); FootstepNodeSnapData snapData = snapper.getSnapData(node); Point3D snapTranslationToSet = nodeDataMessage.getSnapTranslation(); Quaternion snapRotationToSet = nodeDataMessage.getSnapRotation(); snapData.getSnapTransform().get(snapRotationToSet, snapTranslationToSet); }
public void updateCamera() { if (cameraController != null) { cameraController.computeTransform(cameraTransform); cameraTransform.get(cameraRotation, cameraPosition); setLocationInZUpCoordinates(cameraPosition); setRotationInZUpcoordinates(cameraRotation); setHorizontalFoVInRadians((float) cameraController.getHorizontalFieldOfViewInRadians()); setClipDistanceNear((float) cameraController.getClipNear()); setClipDistanceFar((float) cameraController.getClipFar()); } updateFrustrum(); }
private void positionRobotInWorld(HumanoidFloatingRootJointRobot robot) { robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, positionInWorld); positionInWorld.setZ(groundZ + getPelvisToFoot(robot)); positionInWorld.add(offset); robot.setPositionInWorld(positionInWorld); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }
@Test public void testPreMultiplyWithRigidBodyTransform() throws Exception { Random random = new Random(465416L); // Test against EJML for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform t1 = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform t2 = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform t3 = new AffineTransform(t2); t3.preMultiply(t1); DenseMatrix64F m1 = new DenseMatrix64F(4, 4); DenseMatrix64F m2 = new DenseMatrix64F(4, 4); DenseMatrix64F m3 = new DenseMatrix64F(4, 4); t1.get(m1); t2.get(m2); CommonOps.mult(m1, m2, m3); for (int row = 0; row < 4; row++) for (int column = 0; column < 4; column++) assertEquals(m3.get(row, column), t3.getElement(row, column), EPS); } }
private void positionRobotInWorld(HumanoidFloatingRootJointRobot robot) { robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.get(rotation, positionInWorld); positionInWorld.setZ(groundZ + getPelvisToFoot(robot)); positionInWorld.add(offset); robot.setPositionInWorld(positionInWorld); FrameQuaternion frameOrientation = new FrameQuaternion(ReferenceFrame.getWorldFrame(), rotation); double[] yawPitchRoll = new double[3]; frameOrientation.getYawPitchRoll(yawPitchRoll); yawPitchRoll[0] = initialYaw; frameOrientation.setYawPitchRoll(yawPitchRoll); robot.setOrientation(frameOrientation); robot.update(); }
public static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { Quaternion32 quat = new Quaternion32(); us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32()); Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
private static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { us.ihmc.euclid.tuple4D.Quaternion32 quat = new us.ihmc.euclid.tuple4D.Quaternion32(); us.ihmc.euclid.tuple3D.Vector3D32 vector = new us.ihmc.euclid.tuple3D.Vector3D32(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX32(), vector.getY32(), vector.getZ32()); Quaternion jmeQuat = new Quaternion(quat.getX32(), quat.getY32(), quat.getZ32(), quat.getS32()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
@Test public void testSetTransform3DAndGetters() { Random random = new Random(351235L); Box3D box = new Box3D(); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); box.setPose(transform); RigidBodyTransform transformBack = new RigidBodyTransform(); box.getPose(transformBack); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transformBack, EPSILON); RotationMatrix matrix = new RotationMatrix(); Vector3D vector = new Vector3D(); transform.get(matrix, vector); RotationMatrix matrixBack = new RotationMatrix(); Point3D pointBack = new Point3D(); box.getOrientation(matrixBack); assertTrue(matrix.epsilonEquals(matrixBack, EPSILON)); box.getCenter(pointBack); assertTrue(vector.epsilonEquals(pointBack, EPSILON)); assertTrue(vector.epsilonEquals(box.getPosition(), EPSILON)); }
Matrix3D inertialFrameRotation = new Matrix3D(); jointFrameToInertialFrame.get(inertialFrameRotation, CoMOffset);
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); us.ihmc.euclid.tuple4D.Quaternion quat4d = new us.ihmc.euclid.tuple4D.Quaternion(); Vector3D vector3d = new Vector3D(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getS()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); us.ihmc.euclid.tuple4D.Quaternion quat4d = new us.ihmc.euclid.tuple4D.Quaternion(); Vector3D vector3d = new Vector3D(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getS()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetTransform3DAndGetters() { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(351235L); FrameBox3d box = new FrameBox3d(worldFrame); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); box.setTransform(transform); RigidBodyTransform transformBack = new RigidBodyTransform(); box.getTransform(transformBack); assertTrue(transform.epsilonEquals(transformBack, Epsilons.ONE_TRILLIONTH)); assertTrue(transform.epsilonEquals(box.getTransformCopy(), Epsilons.ONE_TRILLIONTH)); RotationMatrix matrix = new RotationMatrix(); Vector3D vector = new Vector3D(); transform.get(matrix, vector); RotationMatrix matrixBack = new RotationMatrix(); FramePoint3D CenterBack = new FramePoint3D(worldFrame); box.getRotation(matrixBack); assertTrue(matrix.epsilonEquals(matrixBack, Epsilons.ONE_TRILLIONTH)); assertTrue(matrix.epsilonEquals(box.getRotationCopy(), Epsilons.ONE_TRILLIONTH)); box.getCenter(CenterBack); assertTrue(vector.epsilonEquals(new Vector3D(CenterBack), 0.0)); assertTrue(vector.epsilonEquals(new Vector3D(box.getCenterCopy()), 0.0)); assertTrue(vector.epsilonEquals(new Vector3D(box.getCenter()), 0.0)); assertTrue(vector.epsilonEquals(new Vector3D(box.getCenterCopy()), 0.0)); }