public void setPosition(FramePoint2d position) { this.position.set(position); }
private void setConditionsForFeedbackOnlyControl() { solver.submitProblemConditions(0, false, true, false); setFeedbackConditions(); finalICPRecursion.set(desiredICP); cmpOffsetRecursionEffect.setToZero(); stanceCMPProjection.setToZero(); beginningOfStateICPProjection.setToZero(); }
/** * 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; }
/** * Compute the capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeCapturePointPosition(double omega0, double time, FramePoint2d initialCapturePoint, FramePoint2d initialCMP, FramePoint2d desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); } }
public void getCoMXYPosition(FramePoint2d comXYPositionToPack) { comXYPositionToPack.set(this.comXYPosition); }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
public void setCoMXYPosition(FramePoint2d comXYPosition) { this.comXYPosition.set(comXYPosition); }
public void updateReachabilityConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver) { solver.setNumberOfReachabilityVertices(4); double lateralInnerLimit = lateralReachabilityInnerLimit.getDoubleValue(); double lateralOuterLimit = lateralReachabilityOuterLimit.getDoubleValue(); lateralInnerLimit = supportSide.negateIfLeftSide(lateralInnerLimit); lateralOuterLimit = supportSide.negateIfLeftSide(lateralOuterLimit); ReferenceFrame supportSoleFrame = bipedSupportPolygons.getSoleZUpFrames().get(supportSide); tempVertex.setToZero(supportSoleFrame); tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralInnerLimit); solver.setReachabilityVertex(0, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralOuterLimit); solver.setReachabilityVertex(1, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralInnerLimit); solver.setReachabilityVertex(2, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralOuterLimit); solver.setReachabilityVertex(3, tempVertex, supportSoleFrame); }
public void set(FramePose2d pose) { position.set(pose.position); orientation.set(pose.orientation); }
public void computeCMPOffsetRecursionEffect(FramePoint2d cmpOffsetRecursionEffectToPack, ArrayList<YoFramePoint2d> upcomingFootstepLocations, int numberOfFootstepsToConsider) { computeTwoCMPOffsets(upcomingFootstepLocations, numberOfFootstepsToConsider); cmpOffsetRecursionEffectToPack.setToZero(); for (int i = 0; i < numberOfFootstepsToConsider; i++) { totalOffsetEffect.set(exitOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); totalOffsetEffect.set(entryOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionEntryMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); } cmpOffsetRecursionEffectToPack.scale(footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier()); }
public void computeBeginningOfStateICPProjection(FramePoint2d beginningOfStateICPProjectionToPack, FramePoint2d beginningOfStateICP) { beginningOfStateICPProjectionToPack.set(beginningOfStateICP); beginningOfStateICPProjectionToPack.scale(footstepRecursionMultiplierCalculator.getInitialICPProjectionMultiplier()); }
public void computeAchievedCMP(FrameVector achievedLinearMomentumRate, FramePoint2d achievedCMPToPack) { if (achievedLinearMomentumRate.containsNaN()) return; centerOfMass2d.setToZero(centerOfMassFrame); centerOfMass2d.changeFrame(worldFrame); achievedCoMAcceleration2d.setByProjectionOntoXYPlaneIncludingFrame(achievedLinearMomentumRate); achievedCoMAcceleration2d.scale(1.0 / totalMass); achievedCoMAcceleration2d.changeFrame(worldFrame); achievedCMPToPack.set(achievedCoMAcceleration2d); achievedCMPToPack.scale(-1.0 / (omega0 * omega0)); achievedCMPToPack.add(centerOfMass2d); }
public void set(CoMXYTimeDerivativesData comXYData) { this.comXYPosition.set(comXYData.comXYPosition); this.comXYVelocity.set(comXYData.comXYVelocity); this.comXYAcceleration.set(comXYData.comXYAcceleration); }
private void updateCapturePointError() { tempFramePoint2d.set(yoDesiredCapturePoint.getX(), yoDesiredCapturePoint.getY()); double error = Math.abs(yoCapturePoint.distance(tempFramePoint2d)); // if (error > maxObservedCapturePointError) // { // maxObservedCapturePointError = error; //// System.out.println("TurnValveBehavior: Max Capture Point Error : " + maxObservedCapturePointError); // } icpError.set(error); } }
/** * Given a capture point trajectory line and the actual * capture point position, project the current capture point * onto the capture point trajectory line. * * @param actualICP * @param capturePointTrajectoryLine * @param projectedCapturePoint */ public static void computeCapturePointOnTrajectoryAndClosestToActualCapturePoint(FramePoint actualICP, FrameLine2d capturePointTrajectoryLine, FramePoint2d projectedCapturePoint) { projectedCapturePoint.set(actualICP.getX(), actualICP.getY()); capturePointTrajectoryLine.orthogonalProjection(projectedCapturePoint); }
/** * This function computes the position of the capture point after the time dt has passed given * the current capture point and the Cop which is assumed to be at constant position over dt. */ public static void predictCapturePoint(FramePoint2d ICP, FramePoint2d CoP, double dt, double omega0, FramePoint2d predictedICPtoPack) { // make sure everything is in the same frame: ICP.checkReferenceFrameMatch(CoP); ICP.checkReferenceFrameMatch(predictedICPtoPack); // CP = (ICP - CoP) * exp(omega0*dt) + CoP predictedICPtoPack.set(ICP); predictedICPtoPack.sub(CoP); predictedICPtoPack.scale(Math.exp(omega0 * dt)); predictedICPtoPack.add(CoP); }
@Override public void orthogonalProjection(FramePoint2d point) { checkReferenceFrameMatch(point); Point2d projected = line.orthogonalProjectionCopy(point.getPoint()); point.set(projected.getX(), projected.getY()); }
public FramePoint2d getCurrentPoint() { point2dInConvexPolygon2d.setAngle(angle.getDoubleValue()); point2dInConvexPolygon2d.setEccentricity(eccentricity.getDoubleValue()); framePoint2d.set(point2dInConvexPolygon2d.getX(), point2dInConvexPolygon2d.getY()); return framePoint2d; }
public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue) { if (supportSide != supportLegPreviousTick.getEnumValue()) { icpProportionalController.reset(); } desiredCMP.set(icpProportionalController.doProportionalControl(desiredCMPPreviousValue, capturePoint, desiredCapturePoint, finalDesiredCapturePoint, desiredCapturePointVelocity, perfectCMP, omega0)); yoUnprojectedDesiredCMP.set(desiredCMP); // do projection here: if (!areaToProjectInto.isEmpty()) { desiredCMPinSafeArea.set(safeArea.isPointInside(desiredCMP)); if (safeArea.isPointInside(desiredCMP)) { supportPolygon.setIncludingFrameAndUpdate(bipedSupportPolygons.getSupportPolygonInMidFeetZUp()); areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon); } cmpProjector.projectCMPIntoSupportPolygonIfOutside(capturePoint, areaToProjectInto, finalDesiredCapturePoint, desiredCMP); if (cmpProjector.getWasCMPProjected()) icpProportionalController.bleedOffIntegralTerm(); } }
/** * Will return a point on a circle around the origin. The point will be in between the given directions and at * a position specified by the alpha value. E.g. an alpha value of 0.5 will result in the point being in the * middle of the given directions. */ public void getPointBetweenVectorsAtDistanceFromOriginCircular(FrameVector2d directionA, FrameVector2d directionB, double alpha, double radius, FramePoint2d midpoint, FramePoint2d pointToPack) { directionA.checkReferenceFrameMatch(directionB.getReferenceFrame()); directionA.checkReferenceFrameMatch(midpoint.getReferenceFrame()); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); double angleBetweenDirections = directionA.angle(directionB); double angleBetweenDirectionsToSetLine = angleBetweenDirections * alpha; rotatedFromA.setToZero(directionA.getReferenceFrame()); rotatedFromA.set(directionA.getX(), directionA.getY(), 0.0); axisAngle.set(negZRotationAxis, angleBetweenDirectionsToSetLine); rotation.setRotation(axisAngle); rotatedFromA.applyTransform(rotation); rotatedFromA.normalize(); rotatedFromA.scale(radius); pointToPack.changeFrame(rotatedFromA.getReferenceFrame()); pointToPack.set(rotatedFromA.getX(), rotatedFromA.getY()); pointToPack.add(midpoint); } }