public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, FloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector .createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED, 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionsInWorld.get(foot), 0.005, YoAppearance.DeepPink()); YoArtifactPosition artifact = copInWorld.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
public CenterOfPressureVisualizer(Map<RigidBody, FootSwitchInterface> footSwitches, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.footSwitches = footSwitches; footRigidBodies = footSwitches.keySet(); for (RigidBody rigidBody : footRigidBodies) { String rigidBodyName = rigidBody.getName(); rigidBodyName = WordUtils.capitalize(rigidBodyName); YoFramePoint rawCoPPositionInWorld = new YoFramePoint("raw" + rigidBodyName + "CoPPositionsInWorld", worldFrame, registry); footRawCoPPositionsInWorld.put(rigidBody, rawCoPPositionInWorld); YoGraphicPosition copDynamicGraphic = new YoGraphicPosition("Meas " + rigidBodyName + "CoP", rawCoPPositionInWorld, 0.008, YoAppearance.DarkRed(), GraphicType.DIAMOND); YoArtifactPosition copArtifact = copDynamicGraphic.createArtifact(); yoGraphicsListRegistry.registerArtifact("StateEstimator", copArtifact); footList.add(rigidBody); } overallRawCoPPositionInWorld = new YoFramePoint("overallRawCoPPositionInWorld", worldFrame, registry); YoGraphicPosition overallRawCoPDynamicGraphic = new YoGraphicPosition("Meas CoP", overallRawCoPPositionInWorld, 0.015, YoAppearance.DarkRed(), GraphicType.DIAMOND); YoArtifactPosition overallRawCoPArtifact = overallRawCoPDynamicGraphic.createArtifact(); overallRawCoPArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", overallRawCoPArtifact); parentRegistry.addChild(registry); }
yoGraphicsListRegistry.registerYoGraphic(listName, copViz); YoArtifactPosition artifact = copViz.createArtifact(); artifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, artifact);
new YoGraphicPosition(caption, yoPlanePoint, 0.005, YoAppearance.Blue(), GraphicType.SOLID_BALL); YoArtifactPosition planePointArtifact = planePointViz.createArtifact(); planePointArtifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, planePointArtifact);
YoGraphicPosition unprojectedDesiredCMPViz = new YoGraphicPosition("Unprojected Desired CMP", yoUnprojectedDesiredCMP, 0.008, Purple(), YoGraphicPosition.GraphicType.BALL_WITH_ROTATED_CROSS); YoArtifactPosition artifact = unprojectedDesiredCMPViz.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, artifact);
yoGraphicsListRegistry.registerArtifact(graphicListName, achievedCMPViz.createArtifact()); YoArtifactPosition perfectCMPArtifact = perfectCMPViz.createArtifact(); perfectCMPArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, perfectCMPArtifact);