public ConnectionPoint3D applyTransform(Transform transform) { Point3D transformed = new Point3D(this); transformed.applyTransform(transform); return new ConnectionPoint3D(transformed, regionId); }
/** * 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); }
@Override public void transform(AdjustFootstepMessage object, RigidBodyTransform rigidBodyTransformToApply) { object.getLocation().applyTransform(rigidBodyTransformToApply); object.getOrientation().applyTransform(rigidBodyTransformToApply); } });
@Override public void transform(FootstepDataMessage message, RigidBodyTransform rigidBodyTransformToApply) { if (message.getLocation() != null) message.getLocation().applyTransform(rigidBodyTransformToApply); if (message.getOrientation() != null) message.getOrientation().applyTransform(rigidBodyTransformToApply); if (message.getCustomPositionWaypoints() != null) { for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++) message.getCustomPositionWaypoints().get(i).applyTransform(rigidBodyTransformToApply); } } });
@Test public void testApplyTransform() throws Exception { Random random = new Random(234234L); for (int i = 0; i < ITERATIONS; i++) { LineSegment3D lineSegment3d = EuclidGeometryRandomTools.nextLineSegment3D(random, EPSILON); Transform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Point3D expectedFirstEndPoint = new Point3D(lineSegment3d.getFirstEndpoint()); Point3D expectedSecondEndpoint = new Point3D(lineSegment3d.getSecondEndpoint()); lineSegment3d.applyTransform(transform); expectedFirstEndPoint.applyTransform(transform); expectedSecondEndpoint.applyTransform(transform); EuclidCoreTestTools.assertTuple3DEquals(expectedFirstEndPoint, lineSegment3d.getFirstEndpoint(), EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedSecondEndpoint, lineSegment3d.getSecondEndpoint(), EPSILON); } }
observerPosition.applyTransform(transform); spatialVector.applyTransform(transform); addCrossToLinearPart(observerPosition, getAngularPart());
observerPosition.applyTransform(transform); spatialVector.applyTransform(transform); addCrossToLinearPart(observerPosition, getAngularPart());
@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)]; Point3D expectedPoint = EuclidCoreRandomTools.nextPoint3D(random); FramePoint3D framePoint = new FramePoint3D(initialFrame, expectedPoint); RigidBodyTransform transform = initialFrame.getTransformToDesiredFrame(anotherFrame); expectedPoint.applyTransform(transform); framePoint.changeFrame(anotherFrame); assertTrue(anotherFrame == framePoint.getReferenceFrame()); EuclidCoreTestTools.assertTuple3DEquals(expectedPoint, framePoint, 10.0 * EPSILON); ReferenceFrame differentRootFrame = ReferenceFrameTools.constructARootFrame("anotherRootFrame"); try { framePoint.changeFrame(differentRootFrame); fail("Should have thrown a RuntimeException"); } catch (RuntimeException e) { // good } } }
pointOfApplication.applyTransform(transform); // p spatialVector.applyTransform(transform);
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyTransform(transform); centerOfMassOffset.applyTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, false, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
concaveHullVertexHome.applyTransform(transformFromOtherToHome); rawPointsInLocal.add(concaveHullVertexHome);
@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); } }
@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); } }
pointOnPlane.applyTransform(regionToWorld); intersectionWithPlane.setToZero(); intersectionWithPlane.set(intersectionInPlaneFrame2D); intersectionWithPlane.applyTransform(regionToWorld);
@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); } }
/** * Will return the intersection point between a line and a single planar region. If the line does * not intersect the region this method will return null. */ public static Point3D intersectRegionWithLine(PlanarRegion region, Line3D projectionLineInWorld) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3DReadOnly planeNormal = new Vector3D(0.0, 0.0, 1.0); Point3DReadOnly pointOnPlane = new Point3D(region.getConvexPolygon(0).getVertex(0)); Point3DBasics pointOnLineInLocal = new Point3D(projectionLineInWorld.getPoint()); Vector3DBasics directionOfLineInLocal = new Vector3D(projectionLineInWorld.getDirection()); pointOnLineInLocal.applyInverseTransform(regionToWorld); directionOfLineInLocal.applyInverseTransform(regionToWorld); Point3D intersectionWithPlaneInLocal = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, pointOnLineInLocal, directionOfLineInLocal); if (intersectionWithPlaneInLocal == null) { return null; } if (region.isPointInside(intersectionWithPlaneInLocal.getX(), intersectionWithPlaneInLocal.getY())) { intersectionWithPlaneInLocal.applyTransform(regionToWorld); return intersectionWithPlaneInLocal; } return null; }
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; }
@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); }
assertFalse(linearVelocity.epsilonEquals(simpleEuclideanTrajectoryPoint.getLinearVelocityCopy(), 1e-10)); position.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
assertFalse(angularVelocity.epsilonEquals(simpleSE3TrajectoryPoint.getAngularVelocityCopy(), 1e-10)); position.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); orientation.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));