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); }
public ReferenceFrame copyAndAimAxisAtPoint(Axis axisToAlign, FramePoint targetToAimAt) { ReferenceFrame initialFrame = targetToAimAt.getReferenceFrame(); targetToAimAt.changeFrame(this); FrameVector targetRelativeToCurrentFrame = new FrameVector(this, targetToAimAt.getX(), targetToAimAt.getY(), targetToAimAt.getZ()); targetToAimAt.changeFrame(initialFrame); return copyAndAlignAxisWithVector(axisToAlign, targetRelativeToCurrentFrame); }
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 setPoints(FramePoint p1, FramePoint p2, FramePoint p3) { this.p1.setIncludingFrame(p1); this.p2.setIncludingFrame(p2); this.p3.setIncludingFrame(p3); this.p1.changeFrame(parentFrame); this.p2.changeFrame(parentFrame); this.p3.changeFrame(parentFrame); }
public static FramePoint getMidPoint(FramePoint point1, FramePoint point2) { point1.checkReferenceFrameMatch(point2); FramePoint midPoint = new FramePoint(point1.referenceFrame); midPoint.interpolate(point1, point2, 0.5); return midPoint; }
public void getCenterOfMassOffset(FramePoint centerOfMassOffsetToPack) { centerOfMassOffsetToPack.setIncludingFrame(expressedInframe, crossPart); centerOfMassOffsetToPack.scale(-1.0 / mass); // comOffset = -1/m * c }
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 static FramePoint getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter) { FramePoint stanceCenter = new FramePoint(stance.getSoleReferenceFrame()); stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame()); stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter); stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter)); stanceCenter.changeFrame(worldFrame); return stanceCenter; }
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 pitchAboutPoint(FramePoint pointToPitchAbout, FramePoint resultToPack, double pitch) { checkReferenceFrameMatch(pointToPitchAbout); double tempX = getX() - pointToPitchAbout.getX(); double tempY = getY() - pointToPitchAbout.getY(); double tempZ = getZ() - pointToPitchAbout.getZ(); double cosAngle = Math.cos(pitch); double sinAngle = Math.sin(pitch); double x = cosAngle * tempX + sinAngle * tempZ; tempZ = -sinAngle * tempX + cosAngle * tempZ; tempX = x; resultToPack.setIncludingFrame(pointToPitchAbout); resultToPack.add(tempX, tempY, tempZ); }
public void setReachabilityVertex(int vertexIndex, FramePoint2d vertexLocation, ReferenceFrame frame) { tmpPoint.setToZero(frame); tmpPoint.setXY(vertexLocation); tmpPoint.changeFrame(worldFrame); reachabilityVertexLocations.get(vertexIndex).set(0, 0, tmpPoint.getX()); reachabilityVertexLocations.get(vertexIndex).set(1, 0, tmpPoint.getY()); }
/** * Add a vertex to this polygon after doing {@code newVertex2d.changeFrameAndProjectToXYPlane(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint} the new vertex (it is not modified). */ public void addVertexByProjectionOntoXYPlane(FramePoint newVertex) { tempPoint.setIncludingFrame(newVertex); tempPoint.changeFrame(referenceFrame); addVertex(referenceFrame, tempPoint.getX(), tempPoint.getY()); }
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); }
/** * Creates a new FramePoint2d based on the x and y components of this FramePoint */ public FramePoint2d toFramePoint2d() { return new FramePoint2d(this.getReferenceFrame(), this.getX(), this.getY()); }
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())); } }
public static Point3d getTransformedPoint(Point3d point3d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FramePoint framePoint = new FramePoint(starting, point3d); framePoint.changeFrame(ending); return framePoint.getPoint(); }
private void interpolatePointFromInitialToFinalFrame(FramePoint pointTrajectoryFrameToPack, FramePoint pointInitialFrame, FramePoint pointFinalFrame, double percentOfFinal) { pointA.setIncludingFrame(pointInitialFrame); pointB.setIncludingFrame(pointFinalFrame); pointA.changeFrame(trajectoryFrame); pointB.changeFrame(trajectoryFrame); pointTrajectoryFrameToPack.setToZero(trajectoryFrame); pointTrajectoryFrameToPack.interpolate(pointA, pointB, percentOfFinal); }
private void moveUpSlightlyToEnsureVisible(YoGraphicPolygon footstepToExpandViz) { FramePoint framePointToPack = new FramePoint(worldFrame); footstepToExpandViz.getPosition(framePointToPack); framePointToPack.setZ(framePointToPack.getZ() + 0.0025); footstepToExpandViz.setPosition(framePointToPack); }