public ReachabilitySphereMapCalculator(OneDoFJointBasics[] robotArmJoints, SimulationConstructionSet scs) { this.scs = scs; solver = new ReachabilityMapSolver(robotArmJoints, null, registry); FramePose3D gridFramePose = new FramePose3D(ReferenceFrame.getWorldFrame(), robotArmJoints[0].getFrameBeforeJoint().getTransformToWorldFrame()); gridFramePose.appendTranslation(getGridSizeInMeters() / 2.5, 0.0, 0.0); setGridFramePose(gridFramePose); scs.addStaticLinkGraphics(ReachabilityMapTools.createReachibilityColorScale()); scs.addYoGraphic(gridFrameViz); scs.addYoGraphic(currentEvaluationPose); scs.addYoGraphic(currentEvaluationPosition); scs.addYoVariableRegistry(registry); }
assertNotNull(graphics3DNodeFromSCS3); GraphicsDynamicGraphicsObject graphicsDynamicGraphicsObjectFromSCS = scs.addYoGraphic(yoGraphic); assertNotNull(graphicsDynamicGraphicsObjectFromSCS); GraphicsDynamicGraphicsObject graphicsDynamicGraphicsObjectFromSCS2 = scs.addYoGraphic(yoGraphic, true); assertNotNull(graphicsDynamicGraphicsObjectFromSCS2); GraphicsDynamicGraphicsObject graphicsDynamicGraphicsObjectFromSCS3 = scs.addYoGraphic(yoGraphic, false); assertNotNull(graphicsDynamicGraphicsObjectFromSCS3);
scs.addYoGraphic(coordinateSystem);
private void setupTest(DRCRobotModel robotModel) throws SimulationExceededMaximumTimeException, InterruptedException { boolean runMultiThreaded = false; setupTrack(runMultiThreaded, robotModel); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); pushRobotController = new PushRobotController(drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(), fullRobotModel); if (VISUALIZE_FORCE) { drcFlatGroundWalkingTrack.getSimulationConstructionSet().addYoGraphic(pushRobotController.getForceVisualizer()); } SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); YoBoolean enable = (YoBoolean) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery"); // enable push recovery enable.set(true); }
scs.addYoGraphic(coordinateSystem);
@Before public void setup() { MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test."); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); String className = getClass().getSimpleName(); robotModel = getRobotModel(); fullRobotModel = robotModel.createFullRobotModel(); PrintTools.debug("simulationTestingParameters.getKeepSCSUp " + simulationTestingParameters.getKeepSCSUp()); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.setStartingLocation(getStartingLocation()); drcSimulationTestHelper.createSimulation(className); double z = getForcePointOffsetZInChestFrame(); pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getPelvis().getParentJoint().getName(), new Vector3D(0, 0, z)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer()); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); @SuppressWarnings("unchecked") final YoEnum<ConstraintType> footConstraintType = (YoEnum<ConstraintType>) scs .getVariable(sidePrefix + "FootControlModule", sidePrefix + "FootCurrentState"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs .getVariable(WalkingHighLevelHumanoidController.class.getSimpleName(), "walkingCurrentState"); singleSupportStartConditions.put(robotSide, new SingleSupportStartCondition(footConstraintType)); } }
public PolygonSnapperVisualizer(ConvexPolygon2D snappingPolygonShape) { Robot robot = new Robot("Robot"); scs = new SimulationConstructionSet(robot); scs.setDT(0.1, 1); polygonToSnap = new YoFrameConvexPolygon2D("polygonToSnap", ReferenceFrame.getWorldFrame(), 4, registry); snappedPolygon = new YoFrameConvexPolygon2D("snappedPolygon", ReferenceFrame.getWorldFrame(), 4, registry); polygonToSnap.set(snappingPolygonShape); snappedPolygon.set(snappingPolygonShape); polygonToSnapPose = new YoFramePoseUsingYawPitchRoll("polygonToSnapPose", ReferenceFrame.getWorldFrame(), registry); snappedPolygonPose = new YoFramePoseUsingYawPitchRoll("snappedPolygonPose", ReferenceFrame.getWorldFrame(), registry); polygonToSnapPose.setToNaN(); snappedPolygonPose.setToNaN(); polygonToSnapViz = new YoGraphicPolygon("polygonToSnapViz", polygonToSnap, polygonToSnapPose, 1.0, YoAppearance.Green()); snappedPolygonViz = new YoGraphicPolygon("snappedPolygonViz", polygonToSnap, snappedPolygonPose, 1.0, YoAppearance.Red()); polygonToSnapViz.update(); snappedPolygonViz.update(); scs.addYoGraphic(polygonToSnapViz); scs.addYoGraphic(snappedPolygonViz); scs.addYoVariableRegistry(registry); scs.setGroundVisible(false); scs.startOnAThread(); }
scs.addYoGraphic(pushRobotController.getForceVisualizer());
scs.addYoGraphic(pushRobotController.getForceVisualizer());
scs.addYoGraphic(pushRobotController.getForceVisualizer());
scs.addYoGraphic(pushRobotController.getForceVisualizer());
scs.addYoGraphic(pushRobotController.getForceVisualizer());
protected void setupTest(DRCRobotModel robotModel) throws SimulationExceededMaximumTimeException, InterruptedException { boolean runMultiThreaded = false; setupTrack(runMultiThreaded, robotModel); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); // pushRobotController = new PushRobotController(robot, fullRobotModel); pushRobotController = new PushRobotController(robot, fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, getPushPointFromChestZOffset())); if (VISUALIZE_FORCE) { drcFlatGroundWalkingTrack.getSimulationConstructionSet().addYoGraphic(pushRobotController.getForceVisualizer()); } SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); YoBoolean enable = (YoBoolean) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery"); // enable push recovery enable.set(true); for (RobotSide robotSide : RobotSide.values) { String prefix = fullRobotModel.getFoot(robotSide).getName(); scs.getVariable(prefix + "FootControlModule", prefix + "State"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs.getVariable("WalkingHighLevelHumanoidController", "walkingState"); doubleSupportStartConditions.put(robotSide, new DoubleSupportStartCondition(walkingState, robotSide)); } }
PushRobotController pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), pushJointName, new Vector3D(), 1.0 / forceMagnitude); drcSimulationTestHelper.getSimulationConstructionSet().addYoGraphic(pushController.getForceVisualizer()); pushController.applyForce(new Vector3D(0.0, 0.0, 1.0), forceMagnitude, Double.POSITIVE_INFINITY);
private void setupTest() throws SimulationExceededMaximumTimeException { DRCRobotModel robotModel = getRobotModel(); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, robotModel, flatGround); drcSimulationTestHelper.createSimulation("DRCSimpleFlatGroundScriptTest"); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer()); drcSimulationTestHelper.setupCameraForUnitTest(new Point3D(0.6, 0.0, 0.6), new Point3D(10.0, 3.0, 3.0)); // get YoVariables for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); String footPrefix = sidePrefix + "Foot"; @SuppressWarnings("unchecked") final YoEnum<ConstraintType> footConstraintType = (YoEnum<ConstraintType>) scs.getVariable(sidePrefix + "FootControlModule", footPrefix + "CurrentState"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs.getVariable("WalkingHighLevelHumanoidController", "walkingCurrentState"); swingStartConditions.put(robotSide, new SingleSupportStartCondition(footConstraintType)); swingFinishConditions.put(robotSide, new DoubleSupportStartCondition(walkingState, robotSide)); } assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0)); YoBoolean enable = (YoBoolean) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery"); enable.set(true); }
scs.addYoGraphic(pushRobotController.getForceVisualizer());
PushRobotController pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), pushJointName, new Vector3D(0, 0, zOffset), 1.0 / forceMagnitude); drcSimulationTestHelper.getSimulationConstructionSet().addYoGraphic(pushController.getForceVisualizer()); pushController.applyForce(new Vector3D(1.0, 0.0, 0.0), forceMagnitude, Double.POSITIVE_INFINITY);
protected void setupAndRunTest(FootstepDataListMessage message) throws SimulationExceededMaximumTimeException, ControllerFailureException { FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.createSimulation("DRCSimpleFlatGroundScriptTest"); FullHumanoidRobotModel fullRobotModel = getRobotModel().createFullRobotModel(); totalMass = fullRobotModel.getTotalMass(); double z = getForcePointOffsetZInChestFrame(); pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, z)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer()); drcSimulationTestHelper.simulateAndBlock(0.5); drcSimulationTestHelper.publishToController(message); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); String footPrefix = sidePrefix + "Foot"; @SuppressWarnings("unchecked") final YoEnum<ConstraintType> footConstraintType = (YoEnum<ConstraintType>) scs.getVariable(sidePrefix + "FootControlModule", footPrefix + "CurrentState"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs.getVariable("WalkingHighLevelHumanoidController", "walkingCurrentState"); singleSupportStartConditions.put(robotSide, new SingleSupportStartCondition(footConstraintType)); doubleSupportStartConditions.put(robotSide, new DoubleSupportStartCondition(walkingState, robotSide)); } setupCamera(); ThreadTools.sleep(1000); }
scs.addYoGraphic(pushRobotController.getForceVisualizer());
scs.addYoGraphic(pushRobotController.getForceVisualizer());