public void transform(RigidBodyTransform transform) { for (int i = 0; i < numberOfScanPoints; i++) transform.transform(scanPoints[i]); }
public void transform(RigidBodyTransform transform) { for (int i = 0; i < numberOfPoints; i++) transform.transform(pointCloud[i]); }
public List<Point3D> toVerticesInWorld(RigidBodyTransform transformToWorld) { List<Point3D> vertices3d = toVertices3d(0.0); vertices3d.forEach(vertex -> transformToWorld.transform(vertex)); return vertices3d; }
public boolean contains(Point3D point) { transform.transform(point, testPoint); return shape.contains(testPoint); } }
private static Point3D applyTransform(RigidBodyTransform transform, Point2D point2D) { Point3D point3D = new Point3D(point2D); transform.transform(point3D); return point3D; }
private void generateExpectedAngularVelocity() { expectedAngularVelocityInIMUFrame.set(randomAngularVelocity); // in joint/body frame transformJointToIMU.transform(expectedAngularVelocityInIMUFrame); }
private Point3D toWorld3D(Point3DReadOnly pointInLocal) { Point3D pointInWorld = new Point3D(pointInLocal); transformToWorld.transform(pointInWorld); return pointInWorld; }
private void showCordinateSystem(JointDescription scsJoint, RigidBodyTransform offsetFromLink) { if (SHOW_SENSOR_REFERENCE_FRAMES) { Graphics3DObject linkGraphics = scsJoint.getLink().getLinkGraphics(); linkGraphics.identity(); linkGraphics.transform(offsetFromLink); linkGraphics.addCoordinateSystem(1.0); linkGraphics.identity(); } }
public static void assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(RigidBodyTransform snapTransform, RigidBodyTransform planarRegionTransform) { Vector3D expectedSurfaceNormal = new Vector3D(0.0, 0.0, 1.0); planarRegionTransform.transform(expectedSurfaceNormal); Vector3D actualSurfaceNormal = new Vector3D(0.0, 0.0, 1.0); snapTransform.transform(actualSurfaceNormal); EuclidCoreTestTools.assertTuple3DEquals(expectedSurfaceNormal, actualSurfaceNormal, 1e-7); Vector3D xAxis = new Vector3D(1.0, 0.0, 0.0); snapTransform.transform(xAxis); assertEquals(0.0, xAxis.getY(), 1e-7); }
public static Ray3d transformRay3d(Ray3d ray3d, RigidBodyTransform transformToHere) { Point3D transformedPoint = new Point3D(); Vector3D transformedVector = new Vector3D(); transformToHere.transform(ray3d.getPoint(), transformedPoint); transformToHere.transform(ray3d.getVector(), transformedVector); return new Ray3d(transformedPoint, transformedVector); }
private void transformAndCheck(RigidBodyTransform trans, Point3D expected, Point3D result) { double epsilon = 0.00001; Point3D exp = new Point3D(expected); trans.transform(exp); assertTrue(result.epsilonEquals(exp, epsilon)); }
protected Point3D getPoint(int index, float range) { Point3D p = new Point3D(range, 0.0, 0.0); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
protected Point3D32 getPoint3f(int index, float range) { Point3D32 p = new Point3D32(range, 0.0f, 0.0f); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
private static Point3D transformFromAngledToWorldFrame(Ramp3D ramp, Point3D point) { RigidBodyTransform angleTransform = new RigidBodyTransform(); angleTransform.setRotationPitchAndZeroTranslation(-ramp.getRampIncline()); angleTransform.transform(point); ramp.transformToWorld(point); return new Point3D(point); }
private Point2D projectOnPlane(RigidBodyTransform plane, Vector3D point) { RigidBodyTransform planeInv = new RigidBodyTransform(plane); planeInv.invert(); planeInv.transform(point); return new Point2D(point.getX(), point.getY()); }
private double getAngleToPelvis(Point3D point, Point3D lidarOrigin) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3D tfPoint = new Point3D(point); tf.transform(tfPoint); return Math.atan2(tfPoint.getY(), tfPoint.getX()); }
private static void setTranslationSettingZAndPreservingXAndY(Point3D highestVertex, RigidBodyTransform transformToReturn) { Vector3D newTranslation = new Vector3D(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
private boolean isAheadOfPelvis(Point3D point) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3D tfPoint = new Point3D(point); tf.transform(tfPoint); return tfPoint.getX() > parameters.xCutoffPelvis; }
private static void setupCamera(DRCSimulationTestHelper drcSimulationTestHelper) { OffsetAndYawRobotInitialSetup startingLocationOffset = location.getStartingLocationOffset(); Point3D cameraFocus = new Point3D(startingLocationOffset.getAdditionalOffset()); cameraFocus.addZ(1.0); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(startingLocationOffset.getYaw()); Point3D cameraPosition = new Point3D(10.0, 5.0, cameraFocus.getZ() + 2.0); transform.transform(cameraPosition); drcSimulationTestHelper.setupCameraForUnitTest(cameraFocus, cameraPosition); }
private static void addNodeDataToFootstepPlan(FootstepPlan footstepPlan, FootstepNodeDataMessage nodeData) { RobotSide robotSide = RobotSide.fromByte(nodeData.getRobotSide()); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(nodeData.getYawIndex() * FootstepNode.gridSizeYaw); footstepPose.setTranslationX(nodeData.getXIndex() * FootstepNode.gridSizeXY); footstepPose.setTranslationY(nodeData.getYIndex() * FootstepNode.gridSizeXY); RigidBodyTransform snapTransform = new RigidBodyTransform(); snapTransform.set(nodeData.getSnapRotation(), nodeData.getSnapTranslation()); snapTransform.transform(footstepPose); footstepPlan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); }