public YoFramePoseUsingYawPitchRoll(String prefix, String suffix, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint3D(prefix, suffix, frame, registry); orientation = new YoFrameYawPitchRoll(prefix, suffix, frame, registry); }
public YoGraphicCoordinateSystem(String namePrefix, String nameSuffix, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition arrowAppearance) { this(namePrefix + nameSuffix, new YoFramePoint3D(namePrefix, nameSuffix, worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(namePrefix, nameSuffix, worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(namePrefix, nameSuffix, worldFrame, registry), scale, arrowAppearance); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D convexPolygon2d, String namePrefix, String nameSuffix, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint3D(namePrefix, nameSuffix, worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(namePrefix, nameSuffix, worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(namePrefix, nameSuffix, worldFrame, registry), scale, DEFAULT_HEIGHT, appearance); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D convexPolygon2d, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint3D(name + "Position", worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(name + "Orientation", worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(name + "Orientation", worldFrame, registry), scale, DEFAULT_HEIGHT, appearance); }
public YoGraphicText3D(String name, String text, String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, text, new YoFramePoint3D(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameYawPitchRoll(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale, appearance); }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, linkGraphics, new YoFramePoint3D(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), new YoFrameYawPitchRoll(namePrefix, nameSuffix, ReferenceFrame.getWorldFrame(), registry), scale); }
public GroundContactPointsSlipper(String registryPrefix) { registry = new YoVariableRegistry(registryPrefix + getClass().getSimpleName()); groundContactPointsToSlip = new ArrayList<GroundContactPoint>(); slipAmount = new YoFrameVector3D("slipAmount", ReferenceFrame.getWorldFrame(), registry); slipRotation = new YoFrameYawPitchRoll("slipRotation", ReferenceFrame.getWorldFrame(), registry); percentToSlipPerTick = new YoDouble("percentToSlipPerTick", registry); doSlip = new YoBoolean("doSlip", registry); }
YoDouble pitch = (YoDouble) yoVariables[yoIndex++]; YoDouble roll = (YoDouble) yoVariables[yoIndex++]; yoFrameYawPitchRoll = new YoFrameYawPitchRoll(yaw, pitch, roll, worldFrame); yoFrameQuaternion = null;
@Before public void setUp() { referenceFrame =ReferenceFrame.constructARootFrame("rootFrame"); registry = new YoVariableRegistry("yoVariableRegistry"); yoFrameOrientation = new YoFrameYawPitchRoll(namePrefix, referenceFrame, registry); frameOrientationToPack = new FrameQuaternion(); }
YoDouble pitch = (YoDouble) yoVariables[yoIndex++]; YoDouble roll = (YoDouble) yoVariables[yoIndex++]; yawPitchRoll = new YoFrameYawPitchRoll(yaw, pitch, roll, worldFrame); quaternion = null;
private void addText(SimulationConstructionSet scs, YoGraphicsListRegistry yoGraphicsListRegistry, String string) { if (string == null) return; YoGraphicsList yoGraphicsList = new YoGraphicsList("TextDescription"); textPoint = new YoFramePoint3D("Desc", worldFrame, registry); textPoint.set(focusX, focusY, -.003); YoFrameYawPitchRoll orientation = new YoFrameYawPitchRoll("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); }
/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFrameYawPitchRoll duplicate(YoVariableRegistry newRegistry) { YoDouble yaw = (YoDouble) newRegistry.getVariable(this.yaw.getFullNameWithNameSpace()); YoDouble pitch = (YoDouble) newRegistry.getVariable(this.pitch.getFullNameWithNameSpace()); YoDouble roll = (YoDouble) newRegistry.getVariable(this.roll.getFullNameWithNameSpace()); return new YoFrameYawPitchRoll(yaw, pitch, roll, getReferenceFrame()); }
nextTranslationToSlip = new YoFrameVector3D(name + "NextTranslationToSlip", ReferenceFrame.getWorldFrame(), registry); maxRotationToSlipNextStep = new YoFrameYawPitchRoll(name + "MaxRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); minRotationToSlipNextStep = new YoFrameYawPitchRoll(name + "MinRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); nextRotationToSlip = new YoFrameYawPitchRoll(name + "NextRotationToSlip", ReferenceFrame.getWorldFrame(), registry);
yoOrientation = new YoFrameYawPitchRoll("EKFOrientation", ReferenceFrame.getWorldFrame(), registry); yoAngularVelocity = new YoFrameVector3D("EKFAngularVelocityInIMUFrame", imuFrame, parentRegistry); yoAngularAcceleration = new YoFrameVector3D("EKFAngularAccelerationInIMUFrame", imuFrame, parentRegistry);
public SlipOnNextStepPerturber(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { super(robotSide.getCamelCaseNameForStartOfExpression() + "SlipOnEachStepPerturber"); String sideString = robotSide.getCamelCaseNameForStartOfExpression(); this.robot = robot; this.touchdownTimeForSlip = new YoDouble(sideString + "TouchdownTimeForSlip", registry); this.slipAfterTimeDelta = new YoDouble(sideString + "SlipAfterTimeDelta", registry); this.slipNextStep = new YoBoolean(sideString + "SlipNextStep", registry); amountToSlipNextStep = new YoFrameVector3D(sideString + "AmountToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); rotationToSlipNextStep = new YoFrameYawPitchRoll(sideString + "RotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); slipState = new YoEnum<SlipState>(sideString + "SlipState", registry, SlipState.class); slipState.set(SlipState.NOT_SLIPPING); groundContactPoints = robot.getFootGroundContactPoints(robotSide); groundContactPointsSlipper = new GroundContactPointsSlipper(robotSide.getLowerCaseName()); groundContactPointsSlipper.addGroundContactPoints(groundContactPoints); groundContactPointsSlipper.setPercentToSlipPerTick(0.05); this.addRobotController(groundContactPointsSlipper); }
YoDouble roll = new YoDouble("roll", registry); YoGraphicCoordinateSystem worldCoordinateSystem = new YoGraphicCoordinateSystem("Example", worldOrigin, new YoFrameYawPitchRoll(yaw, pitch, roll, ReferenceFrame.getWorldFrame()), 2.0);
YoDouble yoRollHand = new YoDouble("yoRollHand", registry); YoFramePoint3D yoFramePointHand = new YoFramePoint3D(yoPXHand, yoPYHand, yoPZHand, worldFrame); YoFrameYawPitchRoll yoFrameYawPitchRollHand = new YoFrameYawPitchRoll(yoYawHand, yoPitchHand, yoRollHand, worldFrame); YoDouble yoRoll = new YoDouble("yoRoll", registry); YoFramePoint3D yoFramePoint = new YoFramePoint3D(yoPX, yoPY, yoPZ, worldFrame); YoFrameYawPitchRoll yoFrameYawPitchRoll = new YoFrameYawPitchRoll(yoYaw, yoPitch, yoRoll, worldFrame);
rotationMagnitude = new YoFrameYawPitchRoll(name + "TotationMagnitude", ReferenceFrame.getWorldFrame(), registry);
lidarRange.set(30.0); final YoFrameYawPitchRoll lidarYawPitchRoll = new YoFrameYawPitchRoll("lidar", null, registry); lidarYawPitchRoll.attachVariableChangedListener(new VariableChangedListener()
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 YoFrameVector3D(prefix + "UpwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); downwardVector = new YoFrameVector3D(prefix + "DownwardCostVector", ReferenceFrame.getWorldFrame(), parentRegistry); forwardCostScalar = new YoDouble(prefix + "ForwardCostScalar", parentRegistry); backwardCostScalar = new YoDouble(prefix + "BackwardCostScalar", parentRegistry); inwardCostScalar = new YoDouble(prefix + "InwardCostScalar", parentRegistry); outwardCostScalar = new YoDouble(prefix + "OutwardCostScalar", parentRegistry); upwardCostScalar = new YoDouble(prefix + "UpwardCostScalar", parentRegistry); downwardCostScalar = new YoDouble(prefix + "DownwardCostScalar", parentRegistry); stancePitchDownwardCostScalar = new YoDouble(prefix + "StancePitchDownwardCostScalar", parentRegistry); angularCostScalar = new YoDouble(prefix + "AngularCostScalar", parentRegistry); negativeFootholdLinearCostScalar = new YoDouble(prefix + "NegativeFootholdLinearCostScalar", parentRegistry); footstepBaseCost = new YoDouble(prefix + "FootstepBaseCost", parentRegistry); idealToCandidateVector = new YoFrameVector3D(prefix + "IdealToCandidateVector", ReferenceFrame.getWorldFrame(), parentRegistry); idealToCandidateOrientation = new YoFrameYawPitchRoll(prefix + "IdealToCandidateOrientation", ReferenceFrame.getWorldFrame(), parentRegistry); tempFrameVectorForDot = new FrameVector3D(); setDefaultValues(); }