public static RigidBodyTransform createTransformFromTranslationAndEulerAngles(Vector3D translation, Vector3D eulerAngles) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationEuler(eulerAngles); transform.setTranslation(translation); return transform; }
Vector3D eulerAngles = EuclidCoreRandomTools.nextRotationVector(random); expectedRotation.setEuler(eulerAngles); actualTransform.setRotationEuler(eulerAngles); assertTrue(actualTransform.hasRotation()); assertTrue(actualTransform.hasTranslation()); double rotZ = Math.PI * random.nextDouble(); expectedRotation.setEuler(rotX, rotY, rotZ); actualTransform.setRotationEuler(rotX, rotY, rotZ); assertTrue(actualTransform.hasRotation()); assertTrue(actualTransform.hasTranslation());
translateThenRotate.setRotationEuler(0.0, -0.0, Math.PI / 2.0); translateThenRotate.setTranslation(2.0, 0.0, 0.0); generator.translateThenRotate(translateThenRotate);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomFullFootholds() { int numTests = 100; RigidBodyTransform nodeToWorldTransform = new RigidBodyTransform(); for (int i = 0; i < numTests; i++) { FootstepNode node = FootstepNode.generateRandomFootstepNode(random, 5.0); FootstepNodeTools.getNodeTransform(node, nodeToWorldTransform); RigidBodyTransform regionToWorld = new RigidBodyTransform(nodeToWorldTransform); double xRotation = EuclidCoreRandomTools.nextDouble(random, 0.15 * Math.PI); double yRotation = EuclidCoreRandomTools.nextDouble(random, 0.15 * Math.PI); regionToWorld.setRotationEuler(xRotation, yRotation, 0.0); regionToWorld.setTranslationZ(EuclidCoreRandomTools.nextDouble(random, 2.0)); doAFullFootholdTest(regionToWorld, node); } }
controlFrameTransform.setRotationEuler(Math.PI / 4.0, 0.0, Math.PI / 2.0); controlFrameTransform.setTranslation(-0.2, 0.2, -0.1); ReferenceFrame controlFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("ControlFrame", footFixedFrame, controlFrameTransform);