quaternion.appendPitchRotation(-cinderBlockTiltRadians); break; case EAST: break; case SOUTH: quaternion.appendPitchRotation(cinderBlockTiltRadians); break; quaternion.appendPitchRotation(cinderBlockTiltRadians); break; case EAST: break; case SOUTH: quaternion.appendPitchRotation(-cinderBlockTiltRadians); break;
@Test public void testAppendPitchRotation() { Random random = new Random(4353); for (int i = 0; i < ITERATIONS; i++) { T original = createRandomYawPitchRoll(random); double pitch = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); T actual = createYawPitchRoll(original); actual.appendPitchRotation(pitch); Quaternion q = new Quaternion(original); q.appendPitchRotation(pitch); T expected = createYawPitchRoll(q); EuclidCoreTestTools.assertYawPitchRollGeometricallyEquals(expected, actual, getEpsilon()); } }
@Test public void testAppendPitchRotation() throws Exception { Random random = new Random(97); for (int i = 0; i < ITERATIONS; i++) { YawPitchRoll original = EuclidCoreRandomTools.nextYawPitchRoll(random); YawPitchRoll actual = new YawPitchRoll(); YawPitchRoll expected = new YawPitchRoll(); double pitch = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); Quaternion q = new Quaternion(); q.set(original); q.appendPitchRotation(pitch); expected.set(q); YawPitchRollTools.appendPitchRotation(original, pitch, actual); EuclidCoreTestTools.assertYawPitchRollEquals(expected, actual, EPSILON); } }
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 25.5) @Test(timeout = 130000) public void testSingleTrajectoryPoint() throws SimulationExceededMaximumTimeException { double epsilon = 1.0e-10; double yaw = Math.toRadians(5.0); double pitch = Math.toRadians(-6.0); double roll = Math.toRadians(-5.0); double trajectoryTime = 0.5; Quaternion orientation = new Quaternion(); orientation.appendYawRotation(yaw); orientation.appendPitchRotation(pitch); orientation.appendRollRotation(roll); ReferenceFrame midFootZUpGroundFrame = humanoidReferenceFrames.getMidFootZUpGroundFrame(); FrameQuaternion pelvisOrientation = new FrameQuaternion(midFootZUpGroundFrame, orientation); pelvisOrientation.changeFrame(worldFrame); PelvisOrientationTrajectoryMessage message = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(trajectoryTime, pelvisOrientation); SO3TrajectoryPointMessage waypoint = message.getSo3Trajectory().getTaskspaceTrajectoryPoints().get(0); drcSimulationTestHelper.publishToController(message); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(4.0 * getRobotModel().getControllerDT()); String pelvisName = fullRobotModel.getPelvis().getName(); String postFix = "Orientation"; EndToEndTestTools.assertNumberOfPoints(pelvisName, postFix, 2, scs); EndToEndTestTools.assertWaypointInGeneratorMatches(pelvisName, 1, waypoint, scs, epsilon); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime); EndToEndTestTools.assertCurrentDesiredsMatchWaypoint(pelvisName, waypoint, scs, epsilon); drcSimulationTestHelper.createVideo(getSimpleRobotName(), 2); }
orientation.appendPitchRotation(pitch); orientation.appendRollRotation(roll); FrameQuaternion frameOrientation = new FrameQuaternion(pelvisFrame, orientation);
desiredChestOrientation.appendPitchRotation(pitch); FrameQuaternion frameChestOrientation = new FrameQuaternion(chestFrame, desiredChestOrientation); frameChestOrientation.changeFrame(worldFrame);
lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat);
lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat);
lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat);
orientation.appendPitchRotation(-cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation)); orientation.appendPitchRotation(cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation)); orientation.appendPitchRotation(cinderBlockTiltRadians); footstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation); footstep.setSwingHeight(0.18); orientation.appendPitchRotation(cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, location, orientation)); orientation.appendPitchRotation(-cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation));
chestOrientation.appendPitchRotation(Math.PI / 4.0); ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(1.0, chestOrientation, worldFrame, pelvisZUpFrame); drcSimulationTestHelper.publishToController(chestTrajectoryMessage); handOrientation.appendPitchRotation(Math.PI / 2.0);
lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat);
lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat);