How to use

appendPitchRotation

method

in

us.ihmc.euclid.tuple4D.Quaternion

  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);