@Override public void getPosition2d(Point2d position2d) { position.setXY(position2d); }
@Override public void setPosition2d(FramePoint2d position2d) { position.setXY(position2d); }
/** FramePoint <p/> A normal point associated with a specific reference frame. */ public FramePoint(FrameTuple2d<?, ?> frameTuple2d) { super(frameTuple2d.referenceFrame, new TransformablePoint3d(), frameTuple2d.name); setXY(frameTuple2d); }
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()); }
private void computeToePoints(RobotSide supportSide) { FrameConvexPolygon2d footDefaultPolygon = footDefaultPolygons.get(supportSide); toePoints[0].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); toePoints[1].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); for (int i = 0; i < footDefaultPolygon.getNumberOfVertices(); i++) { footDefaultPolygon.getFrameVertex(i, footPoint); if (footPoint.getX() > toePoints[0].getX()) { toePoints[1].set(toePoints[0]); toePoints[0].setXY(footPoint); } else if (footPoint.getX() > toePoints[1].getX()) { toePoints[1].setXY(footPoint); } } middleToePoint.setToZero(footDefaultPolygon.getReferenceFrame()); middleToePoint.interpolate(toePoints[0], toePoints[1], 0.5); }
public void setSupportPolygonVertex(int vertexIndex, FramePoint2d vertexLocation, ReferenceFrame frame, double xBuffer, double yBuffer) { tmpPoint.setToZero(frame); tmpPoint.setXY(vertexLocation); if (tmpPoint.getX() > 0.0) tmpPoint.setX(tmpPoint.getX() + xBuffer); else tmpPoint.setX(tmpPoint.getX() - xBuffer); if (tmpPoint.getY() > 0.0) tmpPoint.setY(tmpPoint.getY() + yBuffer); else tmpPoint.setY(tmpPoint.getY() - yBuffer); tmpPoint.changeFrame(worldFrame); cmpVertexLocations.get(vertexIndex).set(0, 0, tmpPoint.getX()); cmpVertexLocations.get(vertexIndex).set(1, 0, tmpPoint.getY()); }
public void requestICPPlannerToHoldCurrentCoM() { centerOfMassPosition.setToZero(centerOfMassFrame); FrameConvexPolygon2d supportPolygonInMidFeetZUp = bipedSupportPolygons.getSupportPolygonInMidFeetZUp(); convexPolygonShrinker.shrinkConstantDistanceInto(supportPolygonInMidFeetZUp, distanceToShrinkSupportPolygonWhenHoldingCurrent.getDoubleValue(), shrunkSupportPolygon); centerOfMassPosition.changeFrame(shrunkSupportPolygon.getReferenceFrame()); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); shrunkSupportPolygon.orthogonalProjection(centerOfMassPosition2d); centerOfMassPosition.setXY(centerOfMassPosition2d); centerOfMassPosition.changeFrame(worldFrame); icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), centerOfMassPosition); }
public void updateCoP() { readSensorData(footWrench); if (Math.abs(footWrench.getLinearPartZ()) < MIN_FORCE_TO_COMPUTE_COP) { yoResolvedCoP.setToNaN(); resolvedCoP3d.setToNaN(ReferenceFrame.getWorldFrame()); resolvedCoP.setToNaN(); } else { copResolver.resolveCenterOfPressureAndNormalTorque(resolvedCoP, footWrench, contactablePlaneBody.getSoleFrame()); yoResolvedCoP.set(resolvedCoP); resolvedCoP3d.setToZero(resolvedCoP.getReferenceFrame()); resolvedCoP3d.setXY(resolvedCoP); resolvedCoP3d.changeFrame(ReferenceFrame.getWorldFrame()); } }
/** * All math in polygon frame. */ public boolean checkIfIntersectionExists(FrameSphere3d sphere, FrameConvexPolygon2d polygon) { ReferenceFrame originalSphereFrame = sphere.getReferenceFrame(); sphere.changeFrame(polygon.getReferenceFrame()); sphere.getCenter(closestPointOnPolygon); closestPointOnPolygon2d.setByProjectionOntoXYPlaneIncludingFrame(closestPointOnPolygon); Point2d pointUnsafe = closestPointOnPolygon2d.getPoint(); ConvexPolygon2dCalculator.orthogonalProjection(pointUnsafe, polygon.getConvexPolygon2d()); closestPointOnPolygon2d.set(pointUnsafe.getX(), pointUnsafe.getY()); closestPointOnPolygon.setXY(closestPointOnPolygon2d); boolean isInsideOrOnSurface = sphere.getSphere3d().isInsideOrOnSurface(closestPointOnPolygon.getPoint()); closestPointOnPolygon.changeFrame(ReferenceFrame.getWorldFrame()); sphere.changeFrame(originalSphereFrame); return isInsideOrOnSurface; }