public void setPosition(Point2d point2d) { point.set(point2d); } }
public void initialize() { update(); yoFinalDesiredICP.set(Double.NaN, Double.NaN); momentumBasedController.getCapturePoint(tempCapturePoint); yoDesiredCapturePoint.setByProjectionOntoXYPlane(tempCapturePoint); icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), tempCapturePoint); icpPlanner.initializeForStanding(yoTime.getDoubleValue()); linearMomentumRateOfChangeControlModule.initializeForStanding(); }
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 void setDesiredCenterOfPressure(ContactablePlaneBody contactablePlaneBody, FramePoint2d desiredCoP) { YoFramePoint2d cop = footDesiredCenterOfPressures.get(contactablePlaneBody); if (cop != null) cop.set(desiredCoP); }
public void setValuesForFeedbackOnly(FramePoint2d desiredICP, FrameVector2d desiredICPVelocity, double omega0) { CapturePointTools.computeDesiredCentroidalMomentumPivot(desiredICP, desiredICPVelocity, omega0, tmpCMP); controllerReferenceICP.set(desiredICP); controllerReferenceICPVelocity.set(desiredICPVelocity); controllerReferenceCMP.set(tmpCMP); nominalReferenceICP.set(desiredICP); nominalReferenceICPVelocity.set(desiredICPVelocity); nominalReferenceCMP.set(tmpCMP); }
public void addFootstepToPlan(Footstep footstep) { if (footstep != null) { upcomingFootsteps.add(footstep); footstep.getPosition2d(tmpFramePoint2d); upcomingFootstepLocations.get(upcomingFootsteps.size() - 1).set(tmpFramePoint2d); inputHandler.addFootstepToPlan(footstep); footstepSolutions.get(upcomingFootsteps.size() - 1).set(tmpFramePoint2d); } }
public void setDesiredDestination(FramePoint2d desiredDestinationInWorld) { numberBlindStepsInPlace.set(0); this.desiredDestination.set(desiredDestinationInWorld); }
public void setContactPoints(List<Point2d> contactPointLocations) { int contactPointLocationsSize = contactPointLocations.size(); if (contactPointLocationsSize != totalNumberOfContactPoints) throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints"); for (int i = 0; i < contactPointLocationsSize; i++) { Point2d contactPointLocation = contactPointLocations.get(i); YoContactPoint yoContactPoint = contactPoints.get(i); yoContactPoint.setPosition2d(contactPointLocation); } contactPointsPolygon.setIncludingFrameAndUpdate(planeFrame, contactPointLocations); this.contactPointCentroid.set(contactPointsPolygon.getCentroid()); }
public void computeReferenceFromSolutions(ArrayList<YoFramePoint2d> footstepSolutions, ArrayList<FrameVector2d> entryOffsets, ArrayList<FrameVector2d> exitOffsets, FramePoint2d previousStanceExitCMP, FramePoint2d stanceEntryCMP, FramePoint2d stanceExitCMP, FramePoint finalICP, YoFramePoint2d beginningOfStateICP, double omega0, int numberOfFootstepsToConsider) { finalICP.getFrameTuple2d(finalICP2d); footstepRecursionMultiplierCalculator.computeICPPoints(finalICP2d, footstepSolutions, entryOffsets, exitOffsets, previousStanceExitCMP, stanceEntryCMP, stanceExitCMP, beginningOfStateICP.getFrameTuple2d(), numberOfFootstepsToConsider, tmpEndPoint, tmpReferencePoint, tmpReferenceVelocity); CapturePointTools.computeDesiredCentroidalMomentumPivot(tmpReferencePoint, tmpReferenceVelocity, omega0, tmpCMP); actualEndOfStateICP.set(tmpEndPoint); controllerReferenceICP.set(tmpReferencePoint); controllerReferenceICPVelocity.set(tmpReferenceVelocity); controllerReferenceCMP.set(tmpCMP); }
public void computeNominalValues(ArrayList<YoFramePoint2d> upcomingFootstepLocations, ArrayList<FrameVector2d> entryOffsets, ArrayList<FrameVector2d> exitOffsets, FramePoint2d previousStanceExitCMP, FramePoint2d stanceEntryCMP, FramePoint2d stanceExitCMP, FramePoint finalICP, YoFramePoint2d beginningOfStateICP, double omega0, int numberOfFootstepsToConsider) { finalICP.getFrameTuple2d(finalICP2d); footstepRecursionMultiplierCalculator.computeICPPoints(finalICP2d, upcomingFootstepLocations, entryOffsets, exitOffsets, previousStanceExitCMP, stanceEntryCMP, stanceExitCMP, beginningOfStateICP.getFrameTuple2d(), numberOfFootstepsToConsider, tmpEndPoint, tmpReferencePoint, tmpReferenceVelocity); CapturePointTools.computeDesiredCentroidalMomentumPivot(tmpReferencePoint, tmpReferenceVelocity, omega0, tmpCMP); nominalEndOfStateICP.set(tmpEndPoint); nominalReferenceICP.set(tmpReferencePoint); nominalReferenceICPVelocity.set(tmpReferenceVelocity); nominalReferenceCMP.set(tmpCMP); }
public void setContactFramePoints(List<FramePoint2d> contactPointLocations) { int contactPointLocationsSize = contactPointLocations.size(); if (contactPointLocationsSize != totalNumberOfContactPoints) throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints"); for (int i = 0; i < contactPointLocationsSize; i++) { FramePoint2d contactPointLocation = contactPointLocations.get(i); YoContactPoint yoContactPoint = contactPoints.get(i); yoContactPoint.setPosition(contactPointLocation); } contactPointsPolygon.setIncludingFrameAndUpdate(contactPointLocations); this.contactPointCentroid.set(contactPointsPolygon.getCentroid()); }
public void getProjectedOntoXYPlane(YoFramePoint2d positionToPack) { positionToPack.set(currentPosition.getX(), currentPosition.getY()); }
private void getYoValuesFromFrameConvexPolygon2d() { numVertices.set(convexPolygon2dForWriting.getNumberOfVertices()); try { for (int i = 0; i < numVertices.getIntegerValue(); i++) { yoFramePoints.get(i).checkReferenceFrameMatch(convexPolygon2dForWriting); yoFramePoints.get(i).set(convexPolygon2dForWriting.getVertex(i)); } } catch (Exception e) { System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling getYoValuesFromFrameConvexPolygon2d()."); e.printStackTrace(); } }
private void scaleFeedbackWeightWithGain() { getTransformedWeights(feedbackWeights, feedbackForwardWeight.getDoubleValue(), feedbackLateralWeight.getDoubleValue()); scaledFeedbackWeight.set(feedbackWeights); if (scaleFeedbackWeightWithGain.getBooleanValue()) { getTransformedFeedbackGains(feedbackGains); double alpha = Math.sqrt(Math.pow(feedbackGains.getX(), 2) + Math.pow(feedbackGains.getY(), 2)); scaledFeedbackWeight.scale(1.0 / alpha); } }
public void extractFootstepSolutions(ArrayList<YoFramePoint2d> footstepSolutionsToPack, ArrayList<YoFramePoint2d> referenceFootstepLocations, ArrayList<Footstep> upcomingFootsteps, int numberOfFootstepsToConsider, ICPOptimizationSolver solver) { boolean firstStepAdjusted = false; for (int i = 0; i < numberOfFootstepsToConsider; i++) { solver.getFootstepSolutionLocation(i, locationSolution); upcomingFootsteps.get(i).getPosition2d(upcomingFootstepLocation); ReferenceFrame deadbandFrame = upcomingFootsteps.get(i).getSoleReferenceFrame(); boolean footstepWasAdjusted = applyLocationDeadband(locationSolution, upcomingFootstepLocation, referenceFootstepLocations.get(i).getFrameTuple2d(), deadbandFrame, footstepDeadband.getDoubleValue(), footstepSolutionResolution.getDoubleValue()); if (i == 0) firstStepAdjusted = footstepWasAdjusted; footstepSolutionsToPack.get(i).set(locationSolution); } this.footstepWasAdjusted.set(firstStepAdjusted); }
public void setCenterOfPressureCommand(CenterOfPressureCommand command) { desiredCoPCommandInSoleFrame.set(command.getDesiredCoPInSoleFrame()); desiredCoPCommandWeightInSoleFrame.set(command.getWeightInSoleFrame()); hasReceivedCenterOfPressureCommand.set(true); }
@Override public void processDataAtControllerRate() { logDataProcessorHelper.update(); admissibleGroundReactionWrench.setToZero(centerOfMassFrame, centerOfMassFrame); admissibleDesiredGroundReactionTorque.getFrameTupleIncludingFrame(tempVector); admissibleGroundReactionWrench.setAngularPart(tempVector); admissibleDesiredGroundReactionForce.getFrameTupleIncludingFrame(tempVector); admissibleGroundReactionWrench.setLinearPart(tempVector); centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(tempCoP, admissibleGroundReactionWrench, worldFrame); admissibleDesiredCenterOfPressure.set(tempCoP); }
@Override public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue) { icpOptimizationController.compute(yoTime.getDoubleValue(), desiredCapturePoint, desiredCapturePointVelocity, capturePoint, omega0); icpOptimizationController.getDesiredCMP(desiredCMP); 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); } }
public void compute(double time, boolean footholdWasUpdated) { if (!footholdExplorationActive.getBooleanValue() || partialFootholdControlModule == null) { reset(); return; } if (timeExploring.isNaN()) startTime.set(time); timeExploring.set(time - startTime.getDoubleValue()); computeDesiredCenterOfPressure(time, footholdWasUpdated); desiredCopInWorld.setIncludingFrame(desiredCenterOfPressure); desiredCopInWorld.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); if (yoDesiredCop != null) yoDesiredCop.set(desiredCopInWorld); centerOfPressureCommand.setDesiredCoP(desiredCenterOfPressure.getPoint()); commandWeight.set(copCommandWeight.getDoubleValue(), copCommandWeight.getDoubleValue()); centerOfPressureCommand.setWeight(commandWeight); }
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }