public void getScanPoint(int index, FramePoint scanPointToPack) { index *= 3; scanPointToPack.setToZero(ReferenceFrame.getWorldFrame()); scanPointToPack.setX(scan[index++]); scanPointToPack.setY(scan[index++]); scanPointToPack.setZ(scan[index++]); }
private void moveUpSlightlyToEnsureVisible(YoGraphicPolygon footstepToExpandViz) { FramePoint framePointToPack = new FramePoint(worldFrame); footstepToExpandViz.getPosition(framePointToPack); framePointToPack.setZ(framePointToPack.getZ() + 0.0025); footstepToExpandViz.setPosition(framePointToPack); }
public void getBasePosition(FramePoint framePoint) { framePoint.setX(this.baseX.getDoubleValue()); framePoint.setY(this.baseY.getDoubleValue()); framePoint.setZ(this.baseZ.getDoubleValue()); }
public void getBasePosition(FramePoint framePoint) { framePoint.setX(this.baseX.getDoubleValue()); framePoint.setY(this.baseY.getDoubleValue()); framePoint.setZ(this.baseZ.getDoubleValue()); }
private void findProjectionOntoPlaneFrame(ReferenceFrame planeFrame, FramePoint2d pointInWorld, FramePoint pointProjectedOntoPlaneFrameToPack) { pointInWorld.checkReferenceFrameMatch(worldFrame); double z = getPlaneZGivenXY(planeFrame, pointInWorld.getX(), pointInWorld.getY()); pointProjectedOntoPlaneFrameToPack.setXYIncludingFrame(pointInWorld); pointProjectedOntoPlaneFrameToPack.setZ(z); }
public static List<FramePoint> getWaypointsAtSpecifiedGroundClearance(FramePoint initialPosition, FramePoint finalPosition, double groundClearance, double[] proportionsThroughTrajectoryForGroundClearance) { List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearance); } return waypoints; }
public void getVoxel(FramePoint voxelLocationToPack, int xIndex, int yIndex, int zIndex) { voxelLocationToPack.setToZero(referenceFrame); voxelLocationToPack.setX(getCoordinateFromIndex(xIndex)); voxelLocationToPack.setY(getCoordinateFromIndex(yIndex)); voxelLocationToPack.setZ(getCoordinateFromIndex(zIndex)); }
waypoint.setZ(zSwingHeight);
private List<FramePoint> getWaypointsAtGroundClearances(double[] groundClearances, double[] proportionsThroughTrajectoryForGroundClearance) { FramePoint initialPosition = allPositions[0].getFramePointCopy(); FramePoint finalPosition = allPositions[3].getFramePointCopy(); positionSources[0].getPosition(initialPosition); positionSources[1].getPosition(finalPosition); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearances[i]); } return waypoints; }
public static void computePseudoCMP3d(FramePoint pseudoCMP3dToPack, FramePoint centerOfMass, FramePoint2d cmp, double fZ, double totalMass, double omega0) { double zCMP = centerOfMass.getZ() - fZ / (totalMass * MathTools.square(omega0)); pseudoCMP3dToPack.setIncludingFrame(cmp.getReferenceFrame(), cmp.getX(), cmp.getY(), 0.0); pseudoCMP3dToPack.changeFrame(centerOfMass.getReferenceFrame()); pseudoCMP3dToPack.setZ(zCMP); }
framePoint.setZ(listOfPoints[2][i].getY());
public static FramePoint computeMinZPointInFrame(RigidBodyTransform footToWorldTransform, List<FramePoint> footPoints, ReferenceFrame bodyFrame, ReferenceFrame frame) { FramePoint minFramePoint = new FramePoint(frame); minFramePoint.setZ(Double.POSITIVE_INFINITY); FramePoint tempFramePoint = new FramePoint(ReferenceFrame.getWorldFrame()); boolean pointFound = false; for (FramePoint footPoint : footPoints) { tempFramePoint.setIncludingFrame(footPoint); tempFramePoint.changeFrame(bodyFrame); tempFramePoint.changeFrameUsingTransform(ReferenceFrame.getWorldFrame(), footToWorldTransform); tempFramePoint.changeFrame(frame); if (tempFramePoint.getZ() < minFramePoint.getZ()) { minFramePoint.set(tempFramePoint); pointFound = true; } } if (!pointFound) throw new RuntimeException(); return minFramePoint; }
public void getClosestVoxel(FramePoint voxelLocationToPack, FramePoint inputPoint) { checkReferenceFrameMatch(inputPoint); if (!boundingBox.isInside(inputPoint.getPoint())) throw new RuntimeException("The given point is outside the grid"); voxelLocationToPack.setToZero(getReferenceFrame()); voxelLocationToPack.setX(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getX()))); voxelLocationToPack.setY(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getY()))); voxelLocationToPack.setZ(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getZ()))); }
public void initialize(FramePoint initialPosition, FramePoint finalPosition, double heightAtParameter, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); FramePoint intermediatePosition = new FramePoint(referenceFrame); intermediatePosition.setX(initialPosition.getX() + q * (finalPosition.getX() - initialPosition.getX())); intermediatePosition.setY(initialPosition.getY() + q * (finalPosition.getY() - initialPosition.getY())); intermediatePosition.setZ(heightAtParameter); initialize(initialPosition, intermediatePosition, finalPosition, q); }
private void initializeRobotState() { reinitialize.set(false); centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassPosition); tempCenterOfMassPosition.changeFrame(worldFrame); yoInitialCenterOfMassPosition.set(tempCenterOfMassPosition); if (!initializeToActual && DRCKinematicsBasedStateEstimator.INITIALIZE_HEIGHT_WITH_FOOT) { RigidBody foot = feet.get(0); footPositionInWorld.setToZero(footFrames.get(foot)); footPositionInWorld.changeFrame(worldFrame); yoInitialFootPosition.set(footPositionInWorld); double footToCoMZ = tempCenterOfMassPosition.getZ() - footPositionInWorld.getZ(); yoInitialComHeight.set(footToCoMZ); tempCenterOfMassPosition.setZ(tempCenterOfMassPosition.getZ() - footToCoMZ); } tempFrameVector.setIncludingFrame(tempCenterOfMassPosition); rootJointPosition.set(centerOfMassPosition); rootJointPosition.sub(tempFrameVector); yoRootJointPosition.set(rootJointPosition); rootJointVelocity.setToZero(worldFrame); yoRootJointVelocity.setToZero(); kinematicsBasedLinearStateCalculator.initialize(rootJointPosition); }
private void submitDesiredPelvisHeight(boolean parallelize, double offsetHeight) { FloatingInverseDynamicsJointReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint(); FramePoint desiredPelvisPosition = new FramePoint(frameAfterRootJoint); desiredPelvisPosition.setZ(offsetHeight); desiredPelvisPosition.changeFrame(worldFrame); PelvisHeightTrajectoryTask comHeightTask = new PelvisHeightTrajectoryTask(desiredPelvisPosition.getZ(), pelvisHeightTrajectoryBehavior, trajectoryTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, comHeightTask); pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(comHeightTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }
pointOnX.setZ(trajectoryOrigin.getZ()); pointOnY.setIncludingFrame(trajectoryOrigin); direction.sub(this.finalPosition, this.initialPosition);