@Test public void testAppendYawRotation() { Random random = new Random(4353); for (int i = 0; i < ITERATIONS; i++) { T original = createRandomYawPitchRoll(random); double yaw = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); T actual = createYawPitchRoll(original); actual.appendYawRotation(yaw); Quaternion q = new Quaternion(original); q.appendYawRotation(yaw); T expected = createYawPitchRoll(q); EuclidCoreTestTools.assertYawPitchRollGeometricallyEquals(expected, actual, getEpsilon()); } }
@Test public void testAppendYawRotation() 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 yaw = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); Quaternion q = new Quaternion(); q.set(original); q.appendYawRotation(yaw); expected.set(q); YawPitchRollTools.appendYawRotation(original, yaw, 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); }
public static ChestTrajectoryMessage createChestMessage(HumanoidReferenceFrames referenceFrames) { ChestTrajectoryMessage message = new ChestTrajectoryMessage(); Quaternion orientation = new Quaternion(); orientation.appendYawRotation(Math.toRadians(-10.0)); orientation.appendRollRotation(Math.toRadians(10.0)); SO3TrajectoryMessage so3Trajectory = message.getSo3Trajectory(); so3Trajectory.getFrameInformation().setTrajectoryReferenceFrameId(MessageTools.toFrameId(referenceFrames.getPelvisZUpFrame())); SO3TrajectoryPointMessage trajectoryPoint = so3Trajectory.getTaskspaceTrajectoryPoints().add(); trajectoryPoint.setTime(0.2); trajectoryPoint.getOrientation().set(orientation); trajectoryPoint.getAngularVelocity().set(new Vector3D()); trajectoryPoint = so3Trajectory.getTaskspaceTrajectoryPoints().add(); trajectoryPoint.setTime(0.4); trajectoryPoint.getOrientation().set(new Quaternion()); trajectoryPoint.getAngularVelocity().set(new Vector3D()); return message; }
Point3D circleCenter = new Point3D(0.55, 0.2, 1.0); Quaternion circleOrientation = new Quaternion(); circleOrientation.appendYawRotation(Math.PI * 0.0); Quaternion handOrientation = new Quaternion(circleOrientation);
SideDependentList<Point3D> circleCenters = new SideDependentList<>(new Point3D(0.6, 0.35, 1.0), new Point3D(0.6, -0.35, 1.0)); Quaternion circleOrientation = new Quaternion(); circleOrientation.appendYawRotation(Math.PI * 0.05); Quaternion handOrientation = new Quaternion(circleOrientation);
SideDependentList<Point3D> circleCenters = new SideDependentList<>(new Point3D(0.55, 0.4, 0.9), new Point3D(0.55, -0.4, 0.9)); Quaternion circleOrientation = new Quaternion(); circleOrientation.appendYawRotation(Math.PI * 0.0); Quaternion handOrientation = new Quaternion(circleOrientation);
orientation.appendYawRotation(yaw); orientation.appendPitchRotation(pitch); orientation.appendRollRotation(roll);
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 26.9) @Test(timeout = 130000) public void testGoHome() throws SimulationExceededMaximumTimeException { double epsilon = 1.0e-5; double yaw = Math.toRadians(15.0); double trajectoryTime = 0.5; Quaternion orientation = new Quaternion(); orientation.appendYawRotation(yaw); ReferenceFrame midFootZUpGroundFrame = humanoidReferenceFrames.getMidFootZUpGroundFrame(); humanoidReferenceFrames.updateFrames(); FrameQuaternion pelvisOrientation = new FrameQuaternion(midFootZUpGroundFrame, orientation); pelvisOrientation.changeFrame(worldFrame); PelvisOrientationTrajectoryMessage message = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(trajectoryTime, pelvisOrientation); drcSimulationTestHelper.publishToController(message); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 0.25)); String pelvisName = fullRobotModel.getPelvis().getName(); EndToEndTestTools.assertCurrentDesiredsMatchWaypoint(pelvisName, message.getSo3Trajectory().getTaskspaceTrajectoryPoints().get(0), scs, epsilon); GoHomeMessage goHomeMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.PELVIS, trajectoryTime); drcSimulationTestHelper.publishToController(goHomeMessage); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 0.5)); humanoidReferenceFrames.updateFrames(); FrameQuaternion homeOrientation = new FrameQuaternion(midFootZUpGroundFrame, new Quaternion()); homeOrientation.changeFrame(worldFrame); SO3TrajectoryPointMessage home = HumanoidMessageTools.createSO3TrajectoryPointMessage(trajectoryTime, homeOrientation, zeroVector); EndToEndTestTools.assertCurrentDesiredsMatchWaypoint(pelvisName, home, scs, epsilon); }
lookLeftQuat.appendYawRotation(Math.PI / 4.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); lookRightQuat.appendYawRotation(-Math.PI / 4.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0);
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0);
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0);
Point3D position = new Point3D(0.5, robotSide.negateIfRightSide(0.5), 1.0); Quaternion orientation = new Quaternion(); orientation.appendYawRotation(robotSide.negateIfRightSide(-Math.PI / 2.0)); orientation.appendRollRotation(-Math.PI / 4.0);
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0);
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0);
handOrientation.appendYawRotation(-Math.PI / 2.0); handOrientation.appendPitchRotation(Math.PI / 2.0);