public static void rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, FramePose3D framePoseToPack) { GeometryTools.rotatePoseAboutAxis(rotationAxisFrame, rotationAxis, angle, false, false, framePoseToPack); }
public static void rotatePoseAboutAxis(FrameVector3D rotatationAxis, FramePoint3D rotationAxisOrigin, double angle, FramePose3D framePoseToPack) { ReferenceFrame frameWhoseZAxisIsRotationAxis = constructReferenceFrameFromPointAndZAxis("rotationAxisFrame", rotationAxisOrigin, rotatationAxis); rotatePoseAboutAxis(frameWhoseZAxisIsRotationAxis, Axis.Z, angle, framePoseToPack); } }
public void addCrossBar() { double height = 2.0 * steeringWheelRadius; double radius = 0.015; double heightAboveWheel = 0.1; FramePose3D crossBar = new FramePose3D(steeringWheelFrame); GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.X, Math.PI / 2.0, crossBar); GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.Z, Math.PI / 2.0, crossBar); crossBar.setPosition(new Vector3D(-height/2.0, 0.0, heightAboveWheel)); RigidBodyTransform transform = new RigidBodyTransform(); crossBar.get(transform); FrameCylinder3d spinnerHandleCylinder = new FrameCylinder3d(steeringWheelFrame, transform, height, radius); spokesCylinders.add(spinnerHandleCylinder); steeringWheelLinkGraphics.transform(transform); steeringWheelLinkGraphics.addCylinder(height, radius, YoAppearance.IndianRed()); transform.invert(); steeringWheelLinkGraphics.transform(transform); steeringWheelLink.setLinkGraphics(steeringWheelLinkGraphics); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotateAndUnrotatePoseAboutCollinearAxis() { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); double angleToRotate = Math.toRadians(90.0); FramePose3D framePose = new FramePose3D(worldFrame); framePose.setPosition(0.0, 0.0, 1.0); FramePose3D framePoseCopy = new FramePose3D(framePose); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate , framePose); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, -0.5 * angleToRotate, framePose); double positionDistance = framePose.getPositionDistance(framePoseCopy); double orientationDistance = framePose.getOrientationDistance(framePoseCopy); assertTrue("Reference Frame shoud not have changed. Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: " + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame); assertEquals("Change in FramePose Position after rotation is wrong.", 0.0, positionDistance, 1e-3); assertEquals("Change in FramePose Orientation after rotation is wrong.", 0.0, orientationDistance, Math.toRadians(0.1)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotatePoseAboutCollinearAxisIncrementally() { Random random = new Random(1179L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0)); FramePose3D framePose = new FramePose3D(worldFrame); framePose.setPosition(0.0, 0.0, 1.0); FramePose3D framePoseCopy = new FramePose3D(framePose); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate, framePose); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate, framePose); double positionDistance = framePose.getPositionDistance(framePoseCopy); double orientationDistance = AngleTools.trimAngleMinusPiToPi(framePose.getOrientationDistance(framePoseCopy)); double desiredOrientationDistance = AngleTools.trimAngleMinusPiToPi(angleToRotate); assertTrue("Reference Frame shoud not have changed. Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: " + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame); assertEquals("Change in FramePose Position after rotation is wrong.", 0.0, positionDistance, 1e-3); assertEquals("Change in FramePose Orientation after rotation is wrong.", desiredOrientationDistance, orientationDistance, Math.toRadians(0.1)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotatePoseAboutZAxisAndCheckOrientation() { Random random = new Random(1179L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePose3D initialPose = new FramePose3D(worldFrame); FramePose3D rotatedPose = new FramePose3D(worldFrame); double angleToRotate = Math.toRadians(-720.01); AxisAngle desiredRotationAxisAngle = new AxisAngle(0.0, 0.0, 1.0, angleToRotate); while (angleToRotate < Math.toRadians(720.0)) { initialPose.setPosition(0.0, 0.0, RandomNumbers.nextDouble(random, 10.0)); rotatedPose.setIncludingFrame(initialPose); desiredRotationAxisAngle.setAngle(angleToRotate); GeometryTools.rotatePoseAboutAxis(rotatedPose.getReferenceFrame(), Axis.Z, angleToRotate, rotatedPose); PoseReferenceFrame initialPoseFrame = new PoseReferenceFrame("initialPoseFrame", initialPose); rotatedPose.changeFrame(initialPoseFrame); AxisAngle actualRotationAxisAngle = new AxisAngle(rotatedPose.getOrientation()); assertTrue("Actual rotation: " + actualRotationAxisAngle + " does not match desired: " + desiredRotationAxisAngle, RotationTools.axisAngleEpsilonEquals(desiredRotationAxisAngle, actualRotationAxisAngle, 1e-5, AxisAngleComparisonMode.IGNORE_FLIPPED_AXES_ROTATION_DIRECTION_AND_COMPLETE_ROTATIONS)); angleToRotate += Math.toRadians(5.0); } }
rotatedPose.setIncludingFrame(initialPose); GeometryTools.rotatePoseAboutAxis(worldFrame, Axis.Z, angleToRotate, lockPosition, lockOrientation, rotatedPose); actualPosition.set(rotatedPose.getPosition());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotatePoseAboutCollinearAxisAndCheckTranslation() { Random random = new Random(1179L); double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0)); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePose3D framePose = new FramePose3D(worldFrame); framePose.setPosition(1.0, 0.0, 1.0); framePose.setOrientation(RandomGeometry.nextQuaternion(random)); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, angleToRotate, framePose); Point3D actualPosePositionAfterRotation = new Point3D(framePose.getPosition()); Point3D desiredPosition = new Point3D(Math.cos(angleToRotate), Math.sin(angleToRotate), 1.0); Vector3D positionError = new Vector3D(); positionError.sub(desiredPosition, actualPosePositionAfterRotation); assertTrue("Reference Frame shoud not have changed. Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: " + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame); assertEquals("FramePose Position after rotation is wrong. Desired position: " + desiredPosition + ", actual position: " + actualPosePositionAfterRotation, 0.0, positionError.length(), 1e-3); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotatePoseAboutOffsetAxisAndCheckTranslation() { Random random = new Random(1179L); double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0)); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePose3D framePose = new FramePose3D(worldFrame); framePose.setPosition(1.0, 0.0, 1.0); framePose.setOrientation(RandomGeometry.nextQuaternion(random)); FrameVector3D rotationAxis = new FrameVector3D(worldFrame, 0.0, 0.0, 1.0); FramePoint3D rotationAxisOrigin = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0); GeometryTools.rotatePoseAboutAxis(rotationAxis, rotationAxisOrigin, angleToRotate, framePose); Point3D actualPosePositionAfterRotation = new Point3D(framePose.getPosition()); Point3D desiredPosition = new Point3D(Math.cos(angleToRotate), Math.sin(angleToRotate), 1.0); Vector3D positionError = new Vector3D(); positionError.sub(desiredPosition, actualPosePositionAfterRotation); assertTrue("Reference Frame shoud not have changed. Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: " + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame); assertEquals("FramePose Position after rotation is wrong. Desired position: " + desiredPosition + ", actual position: " + actualPosePositionAfterRotation, 0.0, positionError.length(), 1e-3); }
@Override protected void setBehaviorInput() { publishTextToSpeack("rotate Valve"); FramePose3D point = offsetPointFromValveInWorldFrame(0.0, valveRadius + valveRadiusfinalOffset, distanceFromValve, 1.5708, 1.5708, -3.14159); GeometryTools.rotatePoseAboutAxis(valvePose, Axis.Z, degrees, point); uiPositionCheckerPacketpublisher.publish(MessageTools.createUIPositionCheckerPacket(point.getPosition())); HandTrajectoryMessage handTrajectoryMessage = HumanoidMessageTools.createHandTrajectoryMessage(RobotSide.RIGHT, 2, point.getPosition(), point.getOrientation(), CommonReferenceFrameIds.CHEST_FRAME.getHashId()); handTrajectoryMessage.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame)); atlasPrimitiveActions.rightHandTrajectoryBehavior.setInput(handTrajectoryMessage); } };
GeometryTools.rotatePoseAboutAxis(rotatedPose.getReferenceFrame(), Axis.Z, angleToRotate, lockPosition, lockOrientation, rotatedPose);