public YoFramePose(String prefix, String suffix, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint(prefix, suffix, frame, registry); orientation = new YoFrameOrientation(prefix, suffix, frame, registry); }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, linkGraphics, new YoFramePoint(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale); }
public YoGraphicCoordinateSystem(String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition arrowColor) { this(namePrefix + nameSuffix, new YoFramePoint(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale); this.arrowColor = arrowColor; }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2d convexPolygon2d, String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale, appearance); }
public YoGraphicText3D(String name, String text, String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, text, new YoFramePoint(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale, appearance); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2d convexPolygon2d, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint(name + "Position", ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(name + "Orientation", ReferenceFrame.getWorldFrame(), registry), scale, appearance); }
public YoGraphicPolygon(String name, int maxNumberOfVertices, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, new YoFrameConvexPolygon2d(name + "ConvexPolygon2d", ReferenceFrame.getWorldFrame(), maxNumberOfVertices, registry), new YoFramePoint(name + "Position", ReferenceFrame.getWorldFrame(), registry), new YoFrameOrientation(name + "Orientation", ReferenceFrame.getWorldFrame(), registry), scale, appearance); }
private void addText(SimulationConstructionSet scs, YoGraphicsListRegistry yoGraphicsListRegistry, String string) { if (string == null) return; YoGraphicsList yoGraphicsList = new YoGraphicsList("TextDescription"); textPoint = new YoFramePoint("Desc", worldFrame, registry); textPoint.set(focusX, focusY, -.003); YoFrameOrientation orientation = new YoFrameOrientation("Desc", worldFrame, registry); double scale = 0.5; YoGraphicText desc = new YoGraphicText("FootstepDescription", string, textPoint, orientation, scale, Color.WHITE, Color.BLACK); yoGraphicsList.add(desc); if (yoGraphicsListRegistry != null) yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
public UserDesiredChestOrientationControllerCommandGenerator(final CommandInputManager controllerCommandInputManager, double defaultTrajectoryTime, YoVariableRegistry parentRegistry) userDesiredChestOrientation = new YoFrameOrientation("userDesiredChest", ReferenceFrame.getWorldFrame(), registry);
public IMUBasedPelvisRotationalStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, List<? extends IMUSensorReadOnly> imuProcessedOutputs, IMUBiasProvider imuBiasProvider, IMUYawDriftEstimator imuYawDriftEstimator, double dt, YoVariableRegistry parentRegistry) { this.imuBiasProvider = imuBiasProvider; this.imuYawDriftEstimator = imuYawDriftEstimator; checkNumberOfSensors(imuProcessedOutputs); imuProcessedOutput = imuProcessedOutputs.get(0); rootJoint = inverseDynamicsStructure.getRootJoint(); rootJointFrame = rootJoint.getFrameAfterJoint(); twistCalculator = inverseDynamicsStructure.getTwistCalculator(); measurementFrame = imuProcessedOutput.getMeasurementFrame(); measurementLink = imuProcessedOutput.getMeasurementLink(); yoRootJointFrameOrientation = new YoFrameOrientation("estimatedRootJoint", worldFrame, registry); yoRootJointFrameQuaternion = new YoFrameQuaternion("estimatedRootJoint", worldFrame, registry); rootJointYawOffsetFromFrozenState = new DoubleYoVariable("rootJointYawOffsetFromFrozenState", registry); yoRootJointAngularVelocity = new YoFrameVector("estimatedRootJointAngularVelocity", rootJointFrame, registry); yoRootJointAngularVelocityInWorld = new YoFrameVector("estimatedRootJointAngularVelocityWorld", worldFrame, registry); yoRootJointAngularVelocityMeasFrame = new YoFrameVector("estimatedRootJointAngularVelocityMeasFrame", measurementFrame, registry); yoRootJointAngularVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector("estimatedRootJointAngularVelocityFromFD", yoRootJointFrameQuaternion, dt, registry); parentRegistry.addChild(registry); angularVelocityRootJointFrameRelativeToWorld = new FrameVector(rootJointFrame); }
YoFrameOrientation comOrientation = new YoFrameOrientation("rigidBodyOrientation", rigidBodyName, worldFrame, registry); RigidBodyVisualizationData comData = new RigidBodyVisualizationData(currentRigidBody, comPosition, comOrientation); centerOfMassData.add(comData);
initialOrientationForViz = new YoFrameOrientation(namePrefix + "InitialOrientationForViz", ReferenceFrame.getWorldFrame(), registry); finalOrientationForViz = new YoFrameOrientation(namePrefix + "FinalOrientationForViz", ReferenceFrame.getWorldFrame(), registry); currentOrientationForViz = new YoFrameOrientation(namePrefix + "CurrentOrientationForViz", ReferenceFrame.getWorldFrame(), registry);
YoFrameOrientation frameOrientation = new YoFrameOrientation((DoubleYoVariable) vars[i++], (DoubleYoVariable) vars[i++], (DoubleYoVariable) vars[i++], referenceFrame);
rotationMagnitude = new YoFrameOrientation(name + "TotationMagnitude", ReferenceFrame.getWorldFrame(), registry);
orientation = new YoFrameOrientation(sensorName, fusedMeasurementFrame, registry); angularVelocity = new YoFrameVector("qd_w", sensorName, fusedMeasurementFrame, registry); linearAcceleration = new YoFrameVector("qdd_", sensorName, fusedMeasurementFrame, registry);
rotationMagnitude = new YoFrameOrientation(name + "TotationMagnitude", ReferenceFrame.getWorldFrame(), registry);
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(); }
yoInterpolatedOffsetFramePoint_Translation = new YoFramePoint("yoInterpolatedOffsetFramePoint_Translation", startOffsetErrorReferenceFrame_Translation, registry); yoGoalOffsetFrameOrientation_Rotation = new YoFrameOrientation("yoGoalOffsetFrameOrientation_Rotation", startOffsetErrorReferenceFrame_Rotation, registry); yoInterpolatedOffsetFrameOrientation_Rotation = new YoFrameOrientation("yoInterpolatedOffsetFrameOrientation_Rotation", startOffsetErrorReferenceFrame_Rotation, registry);
footPolygon.setIncludingFrameAndUpdate(contactableFoot.getContactPoints2d()); String namePrefix = contactableFoot.getName(); desiredHoldOrientation = new YoFrameOrientation(namePrefix + "DesiredHoldOrientation", worldFrame, registry); desiredHoldPosition = new YoFramePoint(namePrefix + "DesiredHoldPosition", worldFrame, registry); doSmartHoldPosition = new BooleanYoVariable(namePrefix + "DoSmartHoldPosition", registry);
String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); YoFrameOrientation currentUpperArmOrientation = new YoFrameOrientation(sidePrefix + "CurrentUpperArm", chestFrame, registry); currentUpperArmOrientations.put(robotSide, currentUpperArmOrientation); YoFrameOrientation currentHandOrientation = new YoFrameOrientation(sidePrefix + "CurrentHand", lowerArmsFrames.get(robotSide), registry); currentHandOrientations.put(robotSide, currentHandOrientation);