@Override public void computeAndPackCoP(FramePoint2d copToPack) { copToPack.setToNaN(getMeasurementFrame()); }
@Override public void setToNaN() { position.setToNaN(); orientation.setToNaN(); }
@Override public void computeAndPackCoP(FramePoint2d copToPack) { copToPack.setToNaN(getMeasurementFrame()); }
@Override public void computeAndPackCoP(FramePoint2d copToPack) { copToPack.setToNaN(getMeasurementFrame()); }
public void intersectionWith(FrameLine2d otherLine, Pair<FramePoint2d, FramePoint2d> intersection) { checkReferenceFrameMatch(otherLine); int numberOfIntersections = ConvexPolygon2dCalculator.intersectionWithLine(otherLine.getLine2d(), intersection.getFirst().getPoint(), intersection.getSecond().getPoint(), convexPolygon); if (numberOfIntersections < 2) { intersection.getSecond().setToNaN(); if (numberOfIntersections < 1) intersection.getFirst().setToNaN(); } }
public void compute(Map<RigidBody, Wrench> externalWrenches) { for (int i = 0; i < contactablePlaneBodies.size(); i++) { ContactablePlaneBody contactablePlaneBody = contactablePlaneBodies.get(i); FramePoint2d cop = cops.get(contactablePlaneBody); YoFramePoint2d yoCop = yoCops.get(contactablePlaneBody); yoCop.getFrameTuple2d(cop); Wrench wrench = externalWrenches.get(contactablePlaneBody.getRigidBody()); if (wrench != null) { wrench.getLinearPartIncludingFrame(tempForce); double normalTorque = centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(cop, wrench, contactablePlaneBody.getSoleFrame()); centersOfPressure2d.get(contactablePlaneBody).set(cop); tempCoP3d.setXYIncludingFrame(cop); centersOfPressureWorld.get(contactablePlaneBody).setAndMatchFrame(tempCoP3d); groundReactionForceMagnitudes.get(contactablePlaneBody).set(tempForce.length()); normalTorques.get(contactablePlaneBody).set(normalTorque); } else { groundReactionForceMagnitudes.get(contactablePlaneBody).set(0.0); centersOfPressureWorld.get(contactablePlaneBody).setToNaN(); cop.setToNaN(); } yoCop.set(cop); desiredCenterOfPressureDataHolder.setCenterOfPressure(cop, contactablePlaneBody.getRigidBody()); } }
@Override public void doTransitionOutOfAction() { super.doTransitionOutOfAction(); toeOffDesiredPitchAngle.set(Double.NaN); toeOffDesiredPitchVelocity.set(Double.NaN); toeOffDesiredPitchAcceleration.set(Double.NaN); toeOffCurrentPitchAngle.set(Double.NaN); toeOffCurrentPitchVelocity.set(Double.NaN); exitCMP2d.setToNaN(); }
footCenter2d.setToNaN(); cops.put(contactableBody, footCenter2d);
lineOfRotation.negateDirection(); dummyDesiredCop.setToNaN(unsafePolygon.getReferenceFrame()); computeShrunkFoothold(dummyDesiredCop);
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()); } }
public ReferenceCentroidalMomentumPivotLocationsCalculator(String namePrefix, BipedSupportPolygons bipedSupportPolygons, SideDependentList<? extends ContactablePlaneBody> contactableFeet, int numberFootstepsToConsider, YoVariableRegistry parentRegistry) firstEntryCMPForSingleSupport.setToNaN();
this.finalCapturePoint.setIncludingFrame(finalCapturePoint); else this.finalCapturePoint.setToNaN(); this.finalCapturePoint.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame());
orientationFeedbackControlCommand.setSelectionMatrix(selectionMatrix); exitCMP2d.setToNaN(soleFrame); exitCMPRayDirection2d.setIncludingFrame(soleFrame, 1.0, 0.0); rayThroughExitCMP.setToNaN(soleFrame);