public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) { quaternionUnfiltered.setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); update(quaternionUnfiltered); }
private RigidBodyTransform[] createYawOnlyCorrectionTargets(Random random, int numTargets) { RigidBodyTransform[] targets = new RigidBodyTransform[numTargets]; Quaternion rot = new Quaternion(); for (int i = 0; i < 10; i++) { targets[i] = new RigidBodyTransform(); rot.setYawPitchRoll((random.nextDouble() * Math.PI * 2) - Math.PI, 0, 0); targets[i].setRotation(rot); } return targets; }
private FramePose3D createFramePose(double x, double y, double z, double yaw_degrees) { FramePose3D framePose = new FramePose3D(ReferenceFrame.getWorldFrame()); Point3D position = new Point3D(x, y, z); Quaternion orientation = new Quaternion(); orientation.setYawPitchRoll(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0)); framePose.set(position, orientation); return framePose; } }
@Test public void testSingularitiesYawPitchRoll() throws Exception { Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(0.5 * Math.PI, 0.0, 1e-9 - 0.5 * Math.PI); double[] yawPitchRoll = new double[3]; quaternion.getYawPitchRoll(yawPitchRoll); } }
private void setupStaticTransformsForRos() { Quaternion orientation = new Quaternion(); Vector3D translation = new Vector3D(0.183585961, 0.0, 0.075353826); orientation.setYawPitchRoll(0.0, 0.130899694, -Math.PI); RigidBodyTransform staticTransform = new RigidBodyTransform(orientation, translation); ImmutableTriple<String, String, RigidBodyTransform> headToHeadRootStaticTransform = new ImmutableTriple<String, String, RigidBodyTransform>("upperNeckPitchLink", "multisense/head", staticTransform); staticTranformsForRos.add(headToHeadRootStaticTransform); } }
private void createValve(String valveRobotName, ValveType valveType, double x, double y, double z, double yaw_degrees, double forceVectorScale) { FramePose3D valvePose = new FramePose3D(ReferenceFrame.getWorldFrame()); Point3D position = new Point3D(x, y, z); Quaternion orientation = new Quaternion(); orientation.setYawPitchRoll(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0)); valvePose.set(position, orientation); ContactableValveRobot valve = new ContactableValveRobot(valveRobotName, valveType, 0.5, valvePose); valve.createValveRobot(); valve.createAvailableContactPoints(1, 30, forceVectorScale, true); valveRobot.add(valve); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testQuaternionStuff() { Quaternion q = new Quaternion(); int nTests = 100; for (int i = 0; i < nTests; i++) { double pi = Math.PI; double yaw = (random.nextDouble() - 0.5) * 2.0 * pi; double pitch = (random.nextDouble() - 0.5) * 0.5 * pi; double roll = (random.nextDouble() - 0.5) * 2.0 * pi; double[] yawPitchRoll = {yaw, pitch, roll}; q.setYawPitchRoll(yaw, pitch, roll); double[] yawPitchRollBack = new double[3]; q.getYawPitchRoll(yawPitchRollBack); double epsilon = 1e-8; localAssertArrayEquals(yawPitchRoll, yawPitchRollBack, epsilon); } }
public IndustrialDebrisEnvironment(Tuple3DBasics robotInitialPosition, double robotInitialYaw) { combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(DefaultCommonAvatarEnvironment.setUpGround("Ground")); Quaternion robotInitialOrientation = new Quaternion(); robotInitialOrientation.setYawPitchRoll(robotInitialYaw, 0.0, 0.0); Point3D robotPosition = new Point3D(robotInitialPosition); FramePose3D robotInitialPose = new FramePose3D(constructionWorldFrame, robotPosition, robotInitialOrientation); this.robotInitialPoseReferenceFrame = new PoseReferenceFrame("robotInitialPoseReferenceFrame", robotInitialPose); }
public void initialize(DenseMatrix64F accel, DenseMatrix64F pqr, double heading) { setMatrix(bias, pqr); // euler = accel2euler(accel, heading); accel2euler(accel, heading); Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(eulerAngles); quaternion.get(q); }
private FramePose3D generateDebrisPose(Point3D positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose3D debrisPose = new FramePose3D(robotInitialPoseReferenceFrame); Quaternion orientation = new Quaternion(); orientation.setYawPitchRoll(debrisYaw, debrisPitch, debrisRoll); debrisPose.set(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; }
private void createValve() { double forceVectorScale = 1.0 / 50.0; String valveRobotName = "ValveRobot"; ValveType valveType = ValveType.SMALL_VALVE; double x = -0.758; double y = -8.515; double z = 1.067; double yaw_degrees = -135; FramePose3D valvePose = new FramePose3D(ReferenceFrame.getWorldFrame()); Point3D position = new Point3D(x, y, z); Quaternion orientation = new Quaternion(); orientation.setYawPitchRoll(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0)); valvePose.set(position, orientation); ContactableValveRobot valve = new ContactableValveRobot(valveRobotName, valveType, 0.5, valvePose); valve.createValveRobot(); valve.createAvailableContactPoints(1, 30, forceVectorScale, true); contactableRobots.add(valve); }
double[] quaternions = new double[4]; Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(yawPitchRoll); quaternions[0] = quaternion.getS(); quaternions[1] = quaternion.getX();
quaternion.setYawPitchRoll(tempYawPitchRollAngles); quaternions[0] = quaternion.getS(); quaternions[1] = quaternion.getX();
quaternion.setYawPitchRoll(yawPitchRoll); quaternions[0] = quaternion.getS(); quaternions[1] = quaternion.getX();
private void setYawPitchRoll() { Quaternion q = new Quaternion(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). q.setYawPitchRoll(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue()); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.multiply(qprev); q_qs.set(q.getS()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate() throws Exception { double dt = 1.0e-8; for (int i = 0; i < 1000; i++) { double yaw = RandomNumbers.nextDouble(random, Math.PI); double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0); double roll = RandomNumbers.nextDouble(random, Math.PI); double yawRate = RandomNumbers.nextDouble(random, 1.0); double pitchRate = RandomNumbers.nextDouble(random, 1.0); double rollRate = RandomNumbers.nextDouble(random, 1.0); double previousYaw = yaw - yawRate * dt; double previousPitch = pitch - pitchRate * dt; double previousRoll = roll - rollRate * dt; Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Quaternion previousRotation = new Quaternion(); previousRotation.setYawPitchRoll(previousYaw, previousPitch, previousRoll); Vector3D expectedAngularVelocity = new Vector3D(); Quaternion difference = new Quaternion(); difference.difference(previousRotation, rotation); difference.getRotationVector(expectedAngularVelocity); expectedAngularVelocity.scale(1.0 / dt); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, 1.0e-7); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate() throws Exception { for (int i = 0; i < 1000; i++) { double yaw = RandomNumbers.nextDouble(random, Math.PI); double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0); double roll = RandomNumbers.nextDouble(random, Math.PI); double yawRate = RandomNumbers.nextDouble(random, 1.0); double pitchRate = RandomNumbers.nextDouble(random, 1.0); double rollRate = RandomNumbers.nextDouble(random, 1.0); Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Vector3D expectedAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, expectedAngularVelocity); rotation.transform(expectedAngularVelocity); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, EPSILON); } }
@Test public void testApplyTransform2() { Box3D box3d = new Box3D(); RigidBodyTransform transform = new RigidBodyTransform(); Point3D point = new Point3D(); point.set(1.0, 1.0, 1.0); transform.setTranslation(point); box3d.applyTransform(transform); box3d.getCenter(point); Point3D expectedPosition = new Point3D(1.0, 1.0, 1.0); EuclidCoreTestTools.assertTuple3DEquals(expectedPosition, point, EPSILON); Quaternion quat = new Quaternion(); quat.setYawPitchRoll(1.0, 1.0, 1.0); Quaternion quat2 = new Quaternion(quat); transform.setRotationAndZeroTranslation(quat); box3d.applyTransform(transform); box3d.getPosition(point); box3d.getOrientation(quat); expectedPosition.applyTransform(transform); EuclidCoreTestTools.assertTuple3DEquals(expectedPosition, point, EPSILON); EuclidCoreTestTools.assertQuaternionEquals(quat2, quat, EPSILON); }
actualQuaternion.setYawPitchRoll(yawPitchRoll); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedQuaternion, actualQuaternion, EPS);
offsetOrientationChest.setYawPitchRoll(0.3, 0.0, 0.1); ReferenceFrame chestControlFrame = drcBehaviorTestHelper.getControllerFullRobotModel().getChest().getBodyFixedFrame(); FrameQuaternion desiredChestOrientation = new FrameQuaternion(chestControlFrame);