/** * @deprecated Use {@link #setRotationPitchAndZeroTranslation(double)} instead. * @param angle */ public void rotY(double angle) { setRotationPitchAndZeroTranslation(angle); }
/** * Add a rotation to the current transform. */ public final void applyRotationY(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationPitchAndZeroTranslation(angle); multiply(temp); }
public RigidBodyTransform getSweepTransform(int i) { if (i >= params.pointsPerSweep * params.scanHeight) { throw new IndexOutOfBoundsException("Index " + i + " greater than or equal to pointsPerSweep " + params.pointsPerSweep + " * scanHeight " + params.scanHeight); } double yawPerIndex = (params.sweepYawMax - params.sweepYawMin) / (params.pointsPerSweep - 1); double pitchPerIndex = (params.heightPitchMax - params.heightPitchMin) / (params.scanHeight - 1); RigidBodyTransform sweepTransform = new RigidBodyTransform(); if (params.pointsPerSweep > 1) { //sweepTransform.rotZ(params.sweepYawMin + yawPerIndex * i); sweepTransform.setRotationYawAndZeroTranslation(params.sweepYawMin + yawPerIndex * (i % params.pointsPerSweep)); } if (params.scanHeight > 1) { sweepTransform.setRotationPitchAndZeroTranslation(params.heightPitchMin + pitchPerIndex * (i / params.pointsPerSweep)); } return sweepTransform; }
public static RigidBodyTransform yawPitchDegreesTransform(Vector3d center, double yawCCWDegrees, double pitchDownDegrees) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawCCWDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(Math.toRadians(pitchDownDegrees)); location.multiply(tilt); location.setTranslation(center); return location; }
public static void rotate(RigidBodyTransform transform, double angle, Axis axis) { RigidBodyTransform rotator = new RigidBodyTransform(); if (axis == Axis.X) { rotator.setRotationRollAndZeroTranslation(angle); } else if (axis == Axis.Y) { rotator.setRotationPitchAndZeroTranslation(angle); } else if (axis == Axis.Z) { rotator.setRotationYawAndZeroTranslation(angle); } transform.multiply(rotator); }
private static void setUpSlopedCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(new Vector3d(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject( new Box3d(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
tInit.setRotationPitchAndZeroTranslation(Math.PI / 4.0);
private void setUpSlopedCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(new Vector3d(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject(new Box3d(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
public static RectangularContactableBody createTypicalContactablePlaneBodyForTests(RigidBody rigidBody, ReferenceFrame endEffectorFrame) { RigidBodyTransform transform3D = new RigidBodyTransform(); transform3D.setTranslation(0.1, 0.2, -0.5); transform3D.setRotationPitchAndZeroTranslation(Math.PI / 2.0); ReferenceFrame soleFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(rigidBody.getName() + "SoleFrame", endEffectorFrame, transform3D); double forward = 0.2; double back = -0.1; double left = 0.1; double right = -0.15; RectangularContactableBody rectangularContactableBody = new RectangularContactableBody(rigidBody, soleFrame, forward, back, left, right); return rectangularContactableBody; }
private static void setUpSlopedCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-cinderBlockTiltRadians); location.multiply(tilt); double zCenter = (cinderBlockHeight * Math.cos(cinderBlockTiltRadians) + cinderBlockLength * Math.sin(cinderBlockTiltRadians)) / 2; location.setTranslation(new Vector3d(xCenter, yCenter, zCenter + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject(new Box3d(location, cinderBlockLength, cinderBlockWidth, cinderBlockHeight), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3d(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3d(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3d(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
public static RigidBodyTransform computeHandstepTransform(boolean rotateZIntoX, Tuple3d position, Vector3d surfaceNormal, double rotationAngleAboutNormal) { surfaceNormal.normalize(); AxisAngle4d rotationAxisAngle = new AxisAngle4d(); GeometryTools.getAxisAngleFromZUpToVector(surfaceNormal, rotationAxisAngle); AxisAngle4d rotationAboutNormal = new AxisAngle4d(surfaceNormal, rotationAngleAboutNormal); RigidBodyTransform transformOne = new RigidBodyTransform(); transformOne.setRotationAndZeroTranslation(rotationAboutNormal); RigidBodyTransform transformTwo = new RigidBodyTransform(); transformTwo.setRotationAndZeroTranslation(rotationAxisAngle); transformOne.multiply(transformTwo); if (rotateZIntoX) { RigidBodyTransform transformThree = new RigidBodyTransform(); transformThree.setRotationPitchAndZeroTranslation(Math.PI / 2.0); transformOne.multiply(transformThree); } transformOne.setTranslation(new Vector3d(position)); return transformOne; } }
if (ROTATE_IMU_FRAMES) imu2Offset.setRotationPitchAndZeroTranslation(Math.PI / 8.0); rootJointPostionAndRotation.setRotationPitchAndZeroTranslation(Math.PI*0.9); rootJointPostionAndRotation.setTranslation(new Vector3d(0.0, 0.0, 0.4)); rootJoint.setRotationAndTranslation(rootJointPostionAndRotation);
RigidBodyTransform zRotationDueToAccountForElbowAxis = new RigidBodyTransform(); FrameVector elbowJointAxis = elbowJoint.getJointAxis(); zRotationDueToAccountForElbowAxis.setRotationPitchAndZeroTranslation(-elbowJointAxis.angle(expectedElbowAxis)); armZeroJointAngleConfigurationOffset.multiply(zRotationDueToAccountForElbowAxis);