@Override public void applyTransform(Transform transform) { orientation.applyTransform(transform); angularVelocity.applyTransform(transform); }
@Override public void transform(Wrench object, RigidBodyTransform rigidBodyTransformToApply) { object.getForce().applyTransform(rigidBodyTransformToApply); object.getTorque().applyTransform(rigidBodyTransformToApply); } });
/** * Transforms this plane using the given homogeneous transformation matrix. * * @param transform the transform to apply on this plane's point and normal. Not modified. * @throws RuntimeException if this plane has not been initialized yet. */ @Override public void applyTransform(Transform transform) { checkHasBeenInitialized(); point.applyTransform(transform); normal.applyTransform(transform); }
@Test public void testChangeFrame() throws Exception { Random random = new Random(43563); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] referenceFrames = EuclidFrameRandomTools.nextReferenceFrameTree(random); ReferenceFrame initialFrame = referenceFrames[random.nextInt(referenceFrames.length)]; ReferenceFrame anotherFrame = referenceFrames[random.nextInt(referenceFrames.length)]; Vector3D expected = EuclidCoreRandomTools.nextVector3D(random); FrameVector3D actual = new FrameVector3D(initialFrame, expected); RigidBodyTransform transform = initialFrame.getTransformToDesiredFrame(anotherFrame); expected.applyTransform(transform); actual.changeFrame(anotherFrame); assertTrue(anotherFrame == actual.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(expected, actual, EPSILON); ReferenceFrame differentRootFrame = ReferenceFrameTools.constructARootFrame("anotherRootFrame"); try { actual.changeFrame(differentRootFrame); fail("Should have thrown a RuntimeException"); } catch (RuntimeException e) { // good } } }
normal.applyTransform(new RigidBodyTransform(new AxisAngle(orthogonal, EPSILON), new Vector3D()));
@Test public void testApplyTransform() throws Exception { Random random = new Random(234234L); for (int i = 0; i < ITERATIONS; i++) { Line3D line3d = EuclidGeometryRandomTools.nextLine3D(random, EPSILON); Transform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Point3D expectedPoint = new Point3D(line3d.getPoint()); Vector3D expectedDirection = new Vector3D(line3d.getDirection()); line3d.applyTransform(transform); expectedPoint.applyTransform(transform); expectedDirection.applyTransform(transform); EuclidCoreTestTools.assertTuple3DEquals(expectedPoint, line3d.getPoint(), EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedDirection, line3d.getDirection(), EPSILON); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testChangeFrame() throws Exception { double epsilon = 1.0e-10; Random random = new Random(21651016L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame expectedFrame = worldFrame; double expectedTime = RandomNumbers.nextDouble(random, 0.0, 1000.0); Point3D expectedPosition = RandomGeometry.nextPoint3D(random, 10.0, 10.0, 10.0); Vector3D expectedLinearVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); SimpleEuclideanTrajectoryPoint testedSimpleEuclideanTrajectoryPoint = new SimpleEuclideanTrajectoryPoint(expectedTime, expectedPosition, expectedLinearVelocity); for (int i = 0; i < 10000; i++) { expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame); expectedPosition.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedLinearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); testedSimpleEuclideanTrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); assertTrajectoryPointContainsExpectedData(expectedTime, expectedPosition, expectedLinearVelocity, testedSimpleEuclideanTrajectoryPoint, epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout = 30000) public void testChangeFrame() throws Exception { double epsilon = 1.0e-10; Random random = new Random(21651016L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame expectedFrame = worldFrame; double expectedTime = RandomNumbers.nextDouble(random, 0.0, 1000.0); Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random)); Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); SimpleSO3TrajectoryPoint testedSimpleSO3TrajectoryPoint = new SimpleSO3TrajectoryPoint(expectedTime, expectedOrientation, expectedAngularVelocity); for (int i = 0; i < 10000; i++) { expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame); expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); testedSimpleSO3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); assertTrajectoryPointContainsExpectedData(expectedTime, expectedOrientation, expectedAngularVelocity, testedSimpleSO3TrajectoryPoint, epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout = 30000) public void testChangeFrame() throws Exception { double epsilon = 1.0e-10; Random random = new Random(21651016L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame expectedFrame = worldFrame; double expectedTime = RandomNumbers.nextDouble(random, 0.0, 1000.0); Point3D expectedPosition = new Point3D(RandomGeometry.nextPoint3D(random, 10.0, 10.0, 10.0)); Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random)); Vector3D expectedLinearVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); SimpleSE3TrajectoryPoint testedSimpleSE3TrajectoryPoint = new SimpleSE3TrajectoryPoint(expectedTime, expectedPosition, expectedOrientation, expectedLinearVelocity, expectedAngularVelocity); for (int i = 0; i < 10000; i++) { expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame); expectedPosition.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedLinearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); testedSimpleSE3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); assertTrajectoryPointContainsExpectedData(expectedTime, expectedPosition, expectedOrientation, expectedLinearVelocity, expectedAngularVelocity, testedSimpleSE3TrajectoryPoint, epsilon); } }
public static Point3D intersectRegionWithRay(PlanarRegion region, Point3D rayStart, Vector3D rayDirection) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3D planeNormal = new Vector3D(0.0, 0.0, 1.0); planeNormal.applyTransform(regionToWorld); Point3D pointOnPlane = new Point3D(); pointOnPlane.set(region.getConvexPolygon(0).getVertex(0)); pointOnPlane.applyTransform(regionToWorld); Point3D intersectionWithPlane = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, rayStart, rayDirection); if (intersectionWithPlane == null) { return null; } Point3D intersectionInPlaneFrame = new Point3D(intersectionWithPlane); intersectionInPlaneFrame.applyInverseTransform(regionToWorld); // checking convex hull here - might be better to check all polygons to avoid false positive if (!region.getConvexHull().isPointInside(intersectionInPlaneFrame.getX(), intersectionInPlaneFrame.getY())) { return null; } Vector3D rayToIntersection = new Vector3D(); rayToIntersection.sub(intersectionWithPlane, rayStart); if (rayToIntersection.dot(rayDirection) < 0.0) { return null; } return intersectionWithPlane; }
planeNormal.applyTransform(regionToWorld);
direction.applyTransform(new RigidBodyTransform(new AxisAngle(orthogonal, epsilon * 0.99), new Vector3D())); secondLine.setDirection(direction); direction.applyTransform(new RigidBodyTransform(new AxisAngle(orthogonal, epsilon * 1.01), new Vector3D())); secondLine.setDirection(direction);
Vector3D rotationAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, normal, true); double rotationAngle = 0.99 * epsilon; normal.applyTransform(new RigidBodyTransform(new AxisAngle(rotationAxis, rotationAngle), new Vector3D())); secondPlane.setNormal(normal); assertTrue(firstPlane.geometricallyEquals(secondPlane, epsilon)); normal.applyTransform(new RigidBodyTransform(new AxisAngle(rotationAxis, rotationAngle), new Vector3D())); secondPlane.setNormal(normal); assertFalse(firstPlane.geometricallyEquals(secondPlane, epsilon));
linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));