public Ray3d(Point3DReadOnly point, Vector3DReadOnly vector) { this.point = new Point3D(point); this.vector = new Vector3D(vector); }
public Point3D getPositionCopy() { Point3D positionCopy = new Point3D(); getPosition(positionCopy); return positionCopy; }
/** * Add a cube to this builder. * @param size edge length of the cube. * @param xOffset x-coordinate of the cube's center. * @param yOffset y-coordinate of the cube's center. * @param zOffset z-coordinate of the cube's center. */ public void addCube(double size, double xOffset, double yOffset, double zOffset) { addBox(size, size, size, new Point3D(xOffset, yOffset, zOffset)); }
private static Point3D computeCoM(Robot robot) { Point3D com = new Point3D(); robot.computeCenterOfMass(com); return com; }
private void hideRobot() { robot.setPositionInWorld(new Point3D(-100.0, -100.0, -100.0)); robot.update(); }
public static Point3D nextPoint3DWithEdgeCases(Random random, double probabilityForEdgeCase) { double x = RandomNumbers.nextDouble(random, probabilityForEdgeCase); double y = RandomNumbers.nextDouble(random, probabilityForEdgeCase); double z = RandomNumbers.nextDouble(random, probabilityForEdgeCase); return new Point3D(x, y, z); }
@Test// timeout=300000 public void testGetYoPosition() { YoFramePoint3D yoPosition = kinematicPoint.getYoPosition(); String frameName = yoPosition.getReferenceFrame().getName(); assertEquals("( 0.000, 0.000, 0.000 )-" + frameName, yoPosition.toString()); yoPosition.set(new Point3D(5.0, 5.1, 5.2)); assertEquals("( 5.000, 5.100, 5.200 )-" + frameName, yoPosition.toString()); }
public Point3D getCameraPosition() { Vector3f position = new Vector3f(getLocation()); JMEGeometryUtils.transformFromJMECoordinatesToZup(position); return new Point3D(position.getX(), position.getY(), position.getZ()); }
protected RobotSide determineClosestFootSideToEnd(SideDependentList<Footstep> lastFootsteps) { FramePose2D endPose = getPath().getPoseAtS(1); Point3D endPoint = new Point3D(endPose.getX(), endPose.getY(), 0.0); RobotSide closestSideToEnd = FootstepUtils.getFrontFootRobotSideFromFootsteps(lastFootsteps, new FramePoint3D(ReferenceFrame.getWorldFrame(), endPoint)); return closestSideToEnd; }
public double fitPlaneToPoints(Point2DBasics center, List<? extends Point3DReadOnly> pointList, Plane3D planeToPack) { double squareError = fitPlaneToPoints(pointList, planeToPack); if (planeToPack.containsNaN()) return Double.POSITIVE_INFINITY; double centerZ = planeToPack.getZOnPlane(center.getX(), center.getY()); Point3D planePoint = new Point3D(center.getX(), center.getY(), centerZ); planeToPack.setPoint(planePoint); return squareError; }
public static Point3D nextPoint3D(Random random, Tuple3DReadOnly min, Tuple3DReadOnly max) { double x = RandomNumbers.nextDouble(random, min.getX(), max.getX()); double y = RandomNumbers.nextDouble(random, min.getY(), max.getY()); double z = RandomNumbers.nextDouble(random, min.getZ(), max.getZ()); return new Point3D(x, y, z); }
public FootstepDataMessage corruptFootstepData(FootstepDataMessage footstepData) { FootstepDataMessage ret = new FootstepDataMessage(footstepData); Point3D location = new Point3D(ret.getLocation()); Quaternion orientation = new Quaternion(ret.getOrientation()); corruptOrientation(orientation); ret.getOrientation().set(orientation); corruptLocationVector(location); ret.getLocation().set(location); return ret; }
public void computeFootTrajectoryMessage(RobotSide robotSide) { checkIfDataHasBeenSet(); Point3D desiredPosition = new Point3D(); Quaternion desiredOrientation = new Quaternion(); ReferenceFrame footFrame = fullRobotModelToUseForConversion.getEndEffectorFrame(robotSide, LimbName.LEG); FramePose3D desiredFootPose = new FramePose3D(footFrame); desiredFootPose.changeFrame(worldFrame); desiredFootPose.get(desiredPosition, desiredOrientation); FootTrajectoryMessage footTrajectoryMessage = robotSide == RobotSide.LEFT ? output.getLeftFootTrajectoryMessage() : output.getRightFootTrajectoryMessage(); footTrajectoryMessage.set(HumanoidMessageTools.createFootTrajectoryMessage(robotSide, trajectoryTime, desiredPosition, desiredOrientation)); }
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 89.2) @Test(timeout = 450000) public void testWalkingUpRampUsingSnapFootsteps() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); doUpRampTest(); Point3D center = new Point3D(7.579638943201888, 0.020725665285290903, 1.46537366331119); Vector3D plusMinusVector = new Vector3D(0.2, 0.2, 0.5); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }