private void setupYoRegistries() { yoGraphicsListRegistry = new YoGraphicsListRegistry(); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); yoGraphicsListRegistryForDetachedOverhead = new YoGraphicsListRegistry(); }
private YoGraphicsListRegistry createYoGraphicsListRegistryWithObject() { YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoGraphicsList yoGraphicsList = new YoGraphicsList(yoGraphicsListName); yoGraphicsList.add(yoGraphic); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); return yoGraphicsListRegistry; }
private void initializeCollisionManager() { if (useShapeCollision) { double coefficientOfRestitution = 0.0; double coefficientOfFriction = 0.9; HybridImpulseSpringDamperCollisionHandler collisionHandler = new HybridImpulseSpringDamperCollisionHandler(coefficientOfRestitution, coefficientOfFriction, simulationConstructionSet.getRootRegistry(), new YoGraphicsListRegistry()); collisionHandler.setKp(100000); collisionHandler.setKd(500); CollisionManager collisionManager = new CollisionManager(commonAvatarEnvironment.get().getTerrainObject3D(), collisionHandler); simulationConstructionSet.initializeShapeCollision(collisionManager); } }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithYoVariableServer(double dtForViz, FullRobotModel fullRobotModel, LogModelProvider logModelProvider, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame, String namePrefix) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("PlannerScheduler"); YoVariableServer yoVariableServer = new YoVariableServer(namePrefix + PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName(), scheduler, logModelProvider, LogSettings.FOOTSTEP_PLANNER, dtForViz); yoVariableServer.setSendKeepAlive(true); footstepPlannerVisualizer.setTickAndUpdatable(yoVariableServer); yoVariableServer.setMainRegistry(registry, fullRobotModel, graphicsListRegistry); yoVariableServer.start(); return footstepPlannerVisualizer; }
private void setupVisualization() { this.graphicsListRegistry = new YoGraphicsListRegistry(); setupTrackBallsVisualization(); setupCornerPointBallsVisualization(); setupNextFootstepVisualization(); setupCurrentFootPoseVisualization(); setupPositionGraphics(); setupSCS(); }
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; } }
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 FootstepPlanningToolboxController(DRCRobotModel drcRobotModel, FullHumanoidRobotModel fullHumanoidRobotModel, StatusMessageOutputManager statusOutputManager, PacketCommunicator packetCommunicator, YoVariableRegistry parentRegistry, double dt) { super(statusOutputManager, parentRegistry); this.packetCommunicator = packetCommunicator; this.contactPointParameters = drcRobotModel.getContactPointParameters(); this.walkingControllerParameters = drcRobotModel.getWalkingControllerParameters(); this.dt = dt; packetCommunicator.attachListener(PlanarRegionsListMessage.class, createPlanarRegionsConsumer()); // SideDependentList<ConvexPolygon2d> footPolygons = new SideDependentList<>(); // for (RobotSide side : RobotSide.values) // footPolygons.set(side, new ConvexPolygon2d(contactPointParameters.getFootContactPoints().get(side))); SideDependentList<ConvexPolygon2d> planningPolygonsInSoleFrame = FootstepPlannerForBehaviorsHelper.createDefaultFootPolygonsForAnytimePlannerAndPlannerToolbox(contactPointParameters); SideDependentList<ConvexPolygon2d> controllerPolygonsInSoleFrame = FootstepPlannerForBehaviorsHelper.createDefaultFootPolygons(contactPointParameters, 1.0, 1.0); humanoidReferenceFrames = createHumanoidReferenceFrames(fullHumanoidRobotModel); footstepDataListWithSwingOverTrajectoriesAssembler = new FootstepDataListWithSwingOverTrajectoriesAssembler(humanoidReferenceFrames, walkingControllerParameters, parentRegistry, new YoGraphicsListRegistry()); plannerMap.put(Planners.PLANAR_REGION_BIPEDAL, createPlanarRegionBipedalPlanner(planningPolygonsInSoleFrame, controllerPolygonsInSoleFrame)); plannerMap.put(Planners.PLAN_THEN_SNAP, new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), planningPolygonsInSoleFrame)); activePlanner.set(Planners.PLANAR_REGION_BIPEDAL); usePlanarRegions.set(true); isDone.set(true); }
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform, false, true, true); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithSimulationConstructionSet(double dtForViz, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); footstepPlannerVisualizer.setTickAndUpdatable(scs); scs.changeBufferSize(32000); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(graphicsListRegistry); scs.setDT(dtForViz, 1); scs.setGroundVisible(false); scs.startOnAThread(); return footstepPlannerVisualizer; }
public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale, boolean addYoGraphicForceVectorsForceVectors) { YoGraphicsListRegistry yoGraphicsListRegistry = null; if (addYoGraphicForceVectorsForceVectors) yoGraphicsListRegistry = new YoGraphicsListRegistry(); for (int i = 0; i < totalContactPointsAvailable; i++) { GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + i, robot.getRobotsYoVariableRegistry()); getJoint().addGroundContactPoint(groupIdentifier, contactPoint); allGroundContactPoints.add(contactPoint); YoBoolean contactAvailable = new YoBoolean("contact_" + name + "_" + i + "_avail", robot.getRobotsYoVariableRegistry()); contactAvailable.set(true); contactsAvailable.add(contactAvailable); if (addYoGraphicForceVectorsForceVectors) { YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + i, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" + i, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector); } } if (addYoGraphicForceVectorsForceVectors) { robot.addYoGraphicsListRegistry(yoGraphicsListRegistry); } }
private YoGraphicsListRegistry createStartAndGoalGraphics(FramePose3D initialStancePose, FramePose3D goalPose) { YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); YoGraphicsList graphicsList = new YoGraphicsList("testViz"); YoFramePoseUsingYawPitchRoll yoInitialStancePose = new YoFramePoseUsingYawPitchRoll("initialStancePose", initialStancePose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoInitialStancePose.set(initialStancePose); YoFramePoseUsingYawPitchRoll yoGoalPose = new YoFramePoseUsingYawPitchRoll("goalStancePose", goalPose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoGoalPose.set(goalPose); YoGraphicCoordinateSystem startPoseGraphics = new YoGraphicCoordinateSystem("startPose", yoInitialStancePose, 13.0); YoGraphicCoordinateSystem goalPoseGraphics = new YoGraphicCoordinateSystem("goalPose", yoGoalPose, 13.0); graphicsList.add(startPoseGraphics); graphicsList.add(goalPoseGraphics); return graphicsListRegistry; }
public ValkyrieSDFLoadingDemo() { ValkyrieRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); FloatingRootJointRobot valkyrieRobot = robotModel.createHumanoidFloatingRootJointRobot(false); valkyrieRobot.setPositionInWorld(new Vector3D()); if (SHOW_ELLIPSOIDS) { addIntertialEllipsoidsToVisualizer(valkyrieRobot); } if (SHOW_COORDINATES_AT_JOINT_ORIGIN) addJointAxis(valkyrieRobot); FullRobotModel fullRobotModel = robotModel.createFullRobotModel(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); CommonInertiaEllipsoidsVisualizer inertiaVis = new CommonInertiaEllipsoidsVisualizer(fullRobotModel.getElevator(), yoGraphicsListRegistry); inertiaVis.update(); scs = new SimulationConstructionSet(valkyrieRobot); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.setGroundVisible(false); scs.startOnAThread(); }
public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description) { nullRobot = new Robot("FootstepVisualizerRobot"); if (groundProfile != null) { GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry()); gcModel.setGroundProfile3D(groundProfile); nullRobot.setGroundContactModel(gcModel); } scs = new SimulationConstructionSet(nullRobot); if (linkGraphics != null) { scs.setGroundVisible(false); scs.addStaticLinkGraphics(linkGraphics); } printifdebug("Attaching exit listener"); scs.setDT(1, 1); yoGraphicsListRegistry = new YoGraphicsListRegistry(); bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry); addText(scs, yoGraphicsListRegistry, description); }
public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description) { nullRobot = new Robot("FootstepVisualizerRobot"); if (groundProfile != null) { GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry()); gcModel.setGroundProfile3D(groundProfile); nullRobot.setGroundContactModel(gcModel); } scs = new SimulationConstructionSet(nullRobot); if (linkGraphics != null) { scs.setGroundVisible(false); scs.addStaticLinkGraphics(linkGraphics); } printifdebug("Attaching exit listener"); scs.setDT(1, 1); yoGraphicsListRegistry = new YoGraphicsListRegistry(); bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry); addText(scs, yoGraphicsListRegistry, description); }
public static void showPlotter(WaypointDefinedBodyPathPlanner plan, String testName) { int markers = 5; YoVariableRegistry registry = new YoVariableRegistry(testName); YoGraphicsListRegistry graphicsList = new YoGraphicsListRegistry(); for (int i = 0; i < markers; i++) { double alpha = (double) i / (double) (markers - 1); Pose2D pose = new Pose2D(); plan.getPointAlongPath(alpha, pose); YoFramePoint3D yoStartPoint = new YoFramePoint3D("PointStart" + i, worldFrame, registry); yoStartPoint.set(pose.getX(), pose.getY(), 0.0); double length = 0.1; YoFrameVector3D direction = new YoFrameVector3D("Direction" + i, worldFrame, registry); direction.set(length * Math.cos(pose.getYaw()), length * Math.sin(pose.getYaw()), 0.0); YoFramePoint3D yoEndPoint = new YoFramePoint3D("PointEnd" + i, worldFrame, registry); yoEndPoint.set(yoStartPoint); yoEndPoint.add(direction); YoGraphicPosition poseStartVis = new YoGraphicPosition("PointStart" + i, yoStartPoint, 0.01, YoAppearance.Blue()); YoGraphicPosition poseEndVis = new YoGraphicPosition("PointEnd" + i, yoEndPoint, 0.01, YoAppearance.Red()); graphicsList.registerArtifact(testName, poseStartVis.createArtifact()); graphicsList.registerArtifact(testName, poseEndVis.createArtifact()); } showPlotter(graphicsList, testName); }
public void startSimulation() throws IOException { SimpleLidarRobot robot = new SimpleLidarRobot(); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, RealtimeTools.nextPowerOfTwo(200000)); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); double simDT = 0.0001; double controlDT = 0.01; scs.setDT(simDT, 10); scs.setSimulateDoneCriterion(() -> false); Graphics3DAdapter graphics3dAdapter = scs.getGraphics3dAdapter(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); SimpleLidarRobotController controller = new SimpleLidarRobotController(robot, controlDT, ros2Node, graphics3dAdapter, yoGraphicsListRegistry); robot.setController(controller, (int) (controlDT / simDT)); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); createGroundTypeListener(scs); scs.setGroundVisible(false); scs.startOnAThread(); scs.simulate(); }
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<ArrayList<Point2D>>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }