public ManualDesiredVelocityControlModule(ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { desiredVelocity = new YoFrameVector2d("desiredVelocity", "", referenceFrame, registry); parentRegistry.addChild(registry); }
private YoArtifactOval(String name, DoubleYoVariable centerX, DoubleYoVariable centerY, DoubleYoVariable radiusX, DoubleYoVariable radiusY, Color color) { this(name, new YoFramePoint2d(centerX, centerY, ReferenceFrame.getWorldFrame()), new YoFrameVector2d(radiusX, radiusY, ReferenceFrame.getWorldFrame()), color); }
public BipedalStepAdjustmentCostCalculator(YoVariableRegistry parentRegistry, YoGraphicsListRegistry graphicsRegistry) { String prefix = "StepAdjustment"; forwardCostVector = new YoFrameVector2d(prefix + "ForwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); backwardCostVector = new YoFrameVector2d(prefix + "BackwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); inwardCostVector = new YoFrameVector2d(prefix + "InwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); outwardCostVector = new YoFrameVector2d(prefix + "OutwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); upwardCostVector = new YoFrameVector(prefix + "UpwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); downwardVector = new YoFrameVector(prefix + "DownwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); forwardCostScalar = new DoubleYoVariable(prefix + "ForwardCostScalar", parentRegistry); backwardCostScalar = new DoubleYoVariable(prefix + "BackwardCostScalar", parentRegistry); inwardCostScalar = new DoubleYoVariable(prefix + "InwardCostScalar", parentRegistry); outwardCostScalar = new DoubleYoVariable(prefix + "OutwardCostScalar", parentRegistry); upwardCostScalar = new DoubleYoVariable(prefix + "UpwardCostScalar", parentRegistry); downwardCostScalar = new DoubleYoVariable(prefix + "DownwardCostScalar", parentRegistry); stancePitchDownwardCostScalar = new DoubleYoVariable(prefix + "StancePitchDownwardCostScalar", parentRegistry); angularCostScalar = new DoubleYoVariable(prefix + "AngularCostScalar", parentRegistry); negativeFootholdLinearCostScalar = new DoubleYoVariable(prefix + "NegativeFootholdLinearCostScalar", parentRegistry); footstepBaseCost = new DoubleYoVariable(prefix + "FootstepBaseCost", parentRegistry); idealToCandidateVector = new YoFrameVector(prefix + "IdealToCandidateVector", ReferenceFrame.getWorldFrame(), parentRegistry); idealToCandidateOrientation = new YoFrameOrientation(prefix + "IdealToCandidateOrientation", ReferenceFrame.getWorldFrame(), parentRegistry); tempFrameVectorForDot = new FrameVector(); setDefaultValues(); }
public RotationVerificator(String namePrefix, ContactablePlaneBody foot, ExplorationParameters explorationParameters, YoVariableRegistry parentRegistry) { soleFrame = foot.getSoleFrame(); registry = new YoVariableRegistry(namePrefix + name); parentRegistry.addChild(registry); yoCopError = new YoFrameVector2d(namePrefix + "CopError", "", soleFrame, registry); perpendicularCopError = new DoubleYoVariable(namePrefix + "PerpendicularCopError", registry); perpendicluarCopErrorThreshold = explorationParameters.getPerpendicluarCopErrorThreshold(); perpendicularCopErrorAboveThreshold = new BooleanYoVariable(namePrefix + "PerpendicularCopErrorAboveThreshold", registry); angleBetweenCopErrorAndLine = new DoubleYoVariable(namePrefix + "AngleBetweenCopErrorAndLine", registry); angleThreshold = explorationParameters.getCopAllowedAreaOpeningAngle(); angleOkay = new BooleanYoVariable(namePrefix + "AngleOkay", registry); desiredCopOnCorrectSide = new BooleanYoVariable(namePrefix + "DesiredCopOnCorrectSide", registry); }
public ICPOptimizationSolutionHandler(ICPOptimizationParameters icpOptimizationParameters, FootstepRecursionMultiplierCalculator footstepRecursionMultiplierCalculator, boolean visualize, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { this.footstepRecursionMultiplierCalculator = footstepRecursionMultiplierCalculator; actualEndOfStateICP = new YoFramePoint2d("actualEndOfStateICP", worldFrame, registry); controllerReferenceICP = new YoFramePoint2d("controllerReferenceICP", worldFrame, registry); controllerReferenceICPVelocity = new YoFrameVector2d("controllerReferenceICPVelocity", worldFrame, registry); controllerReferenceCMP = new YoFramePoint2d("controllerReferenceCMP", worldFrame, registry); nominalEndOfStateICP = new YoFramePoint2d("nominalEndOfStateICP", worldFrame, registry); nominalReferenceICP = new YoFramePoint2d("nominalReferenceICP", worldFrame, registry); nominalReferenceICPVelocity = new YoFrameVector2d("nominalReferenceICPVelocity", worldFrame, registry); nominalReferenceCMP = new YoFramePoint2d("nominalReferenceCMP", worldFrame, registry); controllerCostToGo = new DoubleYoVariable("costToGo", registry); controllerFootstepCostToGo = new DoubleYoVariable("footstepCostToGo", registry); controllerFootstepRegularizationCostToGo = new DoubleYoVariable("footstepRegularizationCostToGo", registry); controllerFeedbackCostToGo = new DoubleYoVariable("feedbackCostToGo", registry); controllerFeedbackRegularizationCostToGo = new DoubleYoVariable("feedbackRegularizationCostToGo", registry); controllerDynamicRelaxationCostToGo = new DoubleYoVariable("dynamicRelaxationCostToGo", registry); footstepDeadband = new DoubleYoVariable("footstepDeadband", registry); footstepSolutionResolution = new DoubleYoVariable("footstepSolutionResolution", registry); useDiscontinuousDeadband = new BooleanYoVariable("useDiscontinuousDeadband", registry); footstepWasAdjusted = new BooleanYoVariable("footstepWasAdjusted", registry); footstepDeadband.set(icpOptimizationParameters.getAdjustmentDeadband()); footstepSolutionResolution.set(icpOptimizationParameters.getFootstepSolutionResolution()); useDiscontinuousDeadband.set(icpOptimizationParameters.useDiscontinuousDeadband()); //if (yoGraphicsListRegistry != null) // setupVisualizers(yoGraphicsListRegistry, visualize); }
YoFrameVector2d entryCMPUserOffset = new YoFrameVector2d(namePrefix + sidePrefix + "EntryCMPConstantOffsets", null, registry); entryCMPUserOffsets.put(robotSide, entryCMPUserOffset); YoFrameVector2d exitCMPUserOffset = new YoFrameVector2d(namePrefix + sidePrefix + "ExitCMPConstantOffsets", null, registry); exitCMPUserOffsets.put(robotSide, exitCMPUserOffset); supportFootPolygonsInSoleZUpFrame.put(robotSide, bipedSupportPolygons.getFootPolygonInSoleZUpFrame(robotSide));
new YoFrameVector2d(robotSide.getCamelCaseNameForStartOfExpression() + "FootCoPError", feet.get(robotSide).getSoleFrame(), registry));
copCommandWeightVector = new YoFrameVector2d(footName + "CopCommandWeight", null, registry); copCommandWeightVector.set(copCommandWeight.getDoubleValue(), copCommandWeight.getDoubleValue());
areCurrentCoPIndicesValid = new BooleanYoVariable(namePrefix + "IsCurrentCoPIndicesValid", registry); cellSize = new YoFrameVector2d(namePrefix + "CellSize", soleFrame, registry); cellArea = new DoubleYoVariable(namePrefix + "CellArea", registry);
doPelvisAndChestYaw.set(true); pelvisShiftScaleFactor = new YoFrameVector2d("DiagnosticPelvisShiftScaleFactor", null, registry); pelvisShiftScaleFactor.set(0.4, 0.7);