public YoArtifactPosition(String name, DoubleYoVariable x, DoubleYoVariable y, GraphicType type, Color color, double radius) { this(name, new YoFramePoint2d(x, y, ReferenceFrame.getWorldFrame()), type, color, radius); }
public void setPosition(Point2d point2d) { point.set(point2d); } }
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 YoGraphicPosition(String name, YoFramePoint2d framePoint, double scale, AppearanceDefinition appearance, GraphicType type) { super(name); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); this.x = framePoint.getYoX(); this.y = framePoint.getYoY(); this.z = null; this.scale = scale; this.type = type; this.appearance = appearance; }
stanceExitCMP2d.setByProjectionOntoXYPlane(stanceExitCMP); this.previousStanceExitCMP.set(previousStanceExitCMP2d); this.stanceEntryCMP.set(stanceEntryCMP2d); this.stanceExitCMP.set(stanceExitCMP2d); stanceExitCMP2d.setByProjectionOntoXYPlane(stanceExitCMP); this.previousStanceExitCMP.setToNaN(); this.stanceEntryCMP.set(stanceEntryCMP2d); this.stanceExitCMP.set(stanceExitCMP2d); stanceExitCMP2d.setByProjectionOntoXYPlane(stanceExitCMP); this.previousStanceExitCMP.set(previousStanceExitCMP2d); this.stanceEntryCMP.setToNaN(); this.stanceExitCMP.set(stanceExitCMP2d); stanceExitCMP2d.setByProjectionOntoXYPlane(stanceExitCMP); this.previousStanceExitCMP.setToNaN(); this.stanceEntryCMP.setToNaN(); this.stanceExitCMP.set(stanceExitCMP2d);
private void submitFootstepConditionsToSolver(int footstepIndex) { getTransformedWeights(footstepWeights, forwardFootstepWeight.getDoubleValue(), lateralFootstepWeight.getDoubleValue()); scaledFootstepWeights.set(footstepWeights); if (localScaleUpcomingStepWeights) scaledFootstepWeights.scale(1.0 / (footstepIndex + 1)); double footstepRecursionMultiplier; if (localUseTwoCMPs) { double entryMutliplier = footstepRecursionMultiplierCalculator.getCMPRecursionEntryMultiplier(footstepIndex); double exitMutliplier = footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(footstepIndex); footstepRecursionMultiplier = entryMutliplier + exitMutliplier; } else { footstepRecursionMultiplier = footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(footstepIndex); } footstepRecursionMultiplier *= footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier(); solver.setFootstepAdjustmentConditions(footstepIndex, footstepRecursionMultiplier, scaledFootstepWeights.getX(), scaledFootstepWeights.getY(), upcomingFootstepLocations.get(footstepIndex).getFrameTuple2d()); }
public ExplorationHelper(ContactableFoot contactableFoot, FootControlHelper footControlHelper, String prefix, YoVariableRegistry registry) { footholdExplorationActive = new BooleanYoVariable(prefix + "FootholdExplorationActive", registry); timeExploring = new DoubleYoVariable(prefix + "TimeExploring", registry); startTime = new DoubleYoVariable(prefix + "StartTime", registry); yoCurrentCorner = new IntegerYoVariable(prefix + "CurrentCornerExplored", registry); centerOfPressureCommand.setContactingRigidBody(contactableFoot.getRigidBody()); explorationParameters = footControlHelper.getWalkingControllerParameters().getOrCreateExplorationParameters(registry); if (explorationParameters != null) copCommandWeight = explorationParameters.getCopCommandWeight(); else copCommandWeight = null; soleFrame = footControlHelper.getContactableFoot().getSoleFrame(); partialFootholdControlModule = footControlHelper.getPartialFootholdControlModule(); YoGraphicsListRegistry graphicObjectsListRegistry = footControlHelper.getMomentumBasedController().getDynamicGraphicObjectsListRegistry(); if (graphicObjectsListRegistry != null) { yoDesiredCop = new YoFramePoint2d(prefix + "DesiredExplorationCop", ReferenceFrame.getWorldFrame(), registry); String name = prefix + "Desired Center of Pressure for Exploration"; YoArtifactPosition artifact = new YoArtifactPosition(name, yoDesiredCop.getYoX(), yoDesiredCop.getYoY(), GraphicType.BALL, Color.BLUE, 0.003); graphicObjectsListRegistry.registerArtifact(prefix + getClass().getSimpleName(), artifact); } else { yoDesiredCop = null; } }
public FramePoint2d getPreviousStanceExitCMP() { return previousStanceExitCMP.getFrameTuple2d(); }
public YoPlaneContactState(String namePrefix, RigidBody rigidBody, ReferenceFrame planeFrame, List<FramePoint2d> contactFramePoints, double coefficientOfFriction, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.inContact = new BooleanYoVariable(namePrefix + "InContact", registry); this.coefficientOfFriction = new DoubleYoVariable(namePrefix + "CoefficientOfFriction", registry); this.coefficientOfFriction.set(coefficientOfFriction); this.rigidBody = rigidBody; this.planeFrame = planeFrame; parentRegistry.addChild(registry); this.contactNormalFrameVector = new FrameVector(planeFrame, 0.0, 0.0, 1.0); contactPoints = new ArrayList<YoContactPoint>(contactFramePoints.size()); for (int i = 0; i < contactFramePoints.size(); i++) { YoContactPoint contactPoint = new YoContactPoint(namePrefix, i, contactFramePoints.get(i), this, registry); contactPoint.setInContact(true); contactPoints.add(contactPoint); } inContact.set(true); totalNumberOfContactPoints = contactPoints.size(); contactPointCentroid = new YoFramePoint2d(namePrefix + "ContactPointCentroid", planeFrame, registry); contactPointCentroid.setToNaN(); hasContactStateChanged = new BooleanYoVariable(namePrefix + "HasChanged", registry); }
ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); String namePrefix = soleFrame.getName() + "DesiredCoP"; YoFramePoint2d yoDesiredCenterOfPressure = new YoFramePoint2d(namePrefix, soleFrame, registry); footDesiredCenterOfPressures.put(contactableFoot, yoDesiredCenterOfPressure); yoGraphicsListRegistry.registerArtifact(graphicListName, capturePointViz.createArtifact()); YoArtifactPosition copViz = new YoArtifactPosition("Controller CoP", yoCenterOfPressure.getYoX(), yoCenterOfPressure.getYoY(), GraphicType.DIAMOND, Color.BLACK , 0.005); yoGraphicsListRegistry.registerArtifact(graphicListName, copViz); yoCenterOfPressure.setToNaN();
public void getContactPointCentroid(FramePoint2d centroidToPack) { this.contactPointCentroid.getFrameTuple2dIncludingFrame(centroidToPack); }
pushRecoveryControlModule.updateForSingleSupport(desiredCapturePoint2d, capturePoint2d, omega0); yoDesiredCapturePoint.set(desiredCapturePoint2d); yoDesiredICPVelocity.set(desiredCapturePointVelocity2d); yoFinalDesiredICP.getFrameTuple2dIncludingFrame(finalDesiredCapturePoint2d); yoAdjustedDesiredCapturePoint.set(adjustedDesiredCapturePoint2d); desiredCapturePoint2d.setIncludingFrame(adjustedDesiredCapturePoint2d); linearMomentumRateOfChangeControlModule.setPerfectCMP(perfectCMP); linearMomentumRateOfChangeControlModule.setSupportLeg(supportLeg); yoDesiredCMP.getFrameTuple2d(desiredCMP); linearMomentumRateOfChangeControlModule.compute(desiredCMP, desiredCMP); yoDesiredCMP.set(desiredCMP);
public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2d unfilteredPoint) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); return ret; }
String listName = getClass().getSimpleName(); YoFramePoint2d cop2d = new YoFramePoint2d(copName + "2d", "", contactableBody.getSoleFrame(), registry); centersOfPressure2d.put(contactableBody, cop2d); cops.put(contactableBody, footCenter2d); YoFramePoint2d yoCop = new YoFramePoint2d(contactableBody.getName() + "CoP", contactableBody.getSoleFrame(), registry); yoCop.set(footCenter2d); yoCops.put(contactableBody, yoCop);
copsRawInFootFrame.get(trustedFoot).getFrameTuple2dIncludingFrame(tempCoP2d); copsRawInFootFrame.get(trustedFoot).set(tempCoP2d);
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(); } }
public YoFrameConvexPolygon2d(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, int maxNumberOfVertices, YoVariableRegistry registry) { this.numVertices = new IntegerYoVariable(namePrefix + "NumVertices" + nameSuffix, registry); numVertices.addVariableChangedListener(this); this.referenceFrame = referenceFrame; for (int i = 0; i < maxNumberOfVertices; i++) { YoFramePoint2d point = new YoFramePoint2d(namePrefix + "_" + i + "_", nameSuffix, referenceFrame, registry); point.attachVariableChangedListener(this); yoFramePoints.add(point); } convexPolygon2dForReading = new FrameConvexPolygon2d(referenceFrame); convexPolygon2dForWriting = new FrameConvexPolygon2d(referenceFrame); }
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 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(); }