break; case EAST: quaternion.appendRollRotation(-cinderBlockTiltRadians); break; case WEST: quaternion.appendRollRotation(cinderBlockTiltRadians); break; case SOUTH: break; case EAST: quaternion.appendRollRotation(cinderBlockTiltRadians); break; case WEST: quaternion.appendRollRotation(-cinderBlockTiltRadians); break; case SOUTH:
@Test public void testAppendRollRotation() { Random random = new Random(4353); for (int i = 0; i < ITERATIONS; i++) { T original = createRandomYawPitchRoll(random); double roll = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); T actual = createYawPitchRoll(original); actual.appendRollRotation(roll); Quaternion q = new Quaternion(original); q.appendRollRotation(roll); T expected = createYawPitchRoll(q); EuclidCoreTestTools.assertYawPitchRollGeometricallyEquals(expected, actual, getEpsilon()); } }
@Test public void testAppendRollRotation() 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 roll = EuclidCoreRandomTools.nextDouble(random, 4.0 * Math.PI); Quaternion q = new Quaternion(); q.set(original); q.appendRollRotation(roll); expected.set(q); YawPitchRollTools.appendRollRotation(original, roll, 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; }
orientation.appendYawRotation(yaw); orientation.appendPitchRotation(pitch); orientation.appendRollRotation(roll); FrameQuaternion frameOrientation = new FrameQuaternion(pelvisFrame, orientation); frameOrientation.changeFrame(worldFrame);
orientation.appendRollRotation(-cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation)); orientation.appendRollRotation(cinderBlockTiltRadians); FootstepDataMessage footstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, location, orientation); footstep.setSwingHeight(0.18); orientation.appendRollRotation(cinderBlockTiltRadians); footstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation); footstep.setSwingHeight(0.18); orientation.appendRollRotation(-cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, location, orientation)); orientation.appendRollRotation(-cinderBlockTiltRadians); footstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, location, orientation); orientation.appendRollRotation(cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, location, orientation)); orientation.appendRollRotation(cinderBlockTiltRadians); message.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, location, orientation));
lookLeftQuat.appendYawRotation(Math.PI / 4.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookLeft.changeFrame(ReferenceFrame.getWorldFrame()); lookRightQuat.appendYawRotation(-Math.PI / 4.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat); lookRight.changeFrame(ReferenceFrame.getWorldFrame());
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookLeft.changeFrame(ReferenceFrame.getWorldFrame()); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat); lookRight.changeFrame(ReferenceFrame.getWorldFrame());
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookLeft.changeFrame(ReferenceFrame.getWorldFrame()); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat); lookRight.changeFrame(ReferenceFrame.getWorldFrame());
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); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookLeft.changeFrame(ReferenceFrame.getWorldFrame()); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat); lookRight.changeFrame(ReferenceFrame.getWorldFrame());
lookLeftQuat.appendYawRotation(Math.PI / 8.0); lookLeftQuat.appendPitchRotation(Math.PI / 16.0); lookLeftQuat.appendRollRotation(-Math.PI / 16.0); FrameQuaternion lookLeft = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookLeftQuat); lookLeft.changeFrame(ReferenceFrame.getWorldFrame()); lookRightQuat.appendYawRotation(-Math.PI / 8.0); lookRightQuat.appendPitchRotation(-Math.PI / 16.0); lookRightQuat.appendRollRotation(Math.PI / 16.0); FrameQuaternion lookRight = new FrameQuaternion(humanoidReferenceFrames.getPelvisZUpFrame(), lookRightQuat); lookRight.changeFrame(ReferenceFrame.getWorldFrame());