private void createPushRobotController() { if (usePushRobotController.get()) { PushRobotController bodyPushRobotController = new PushRobotController(sdfRobot.get(), "body", new Vector3d(0.0, -0.00057633, 0.0383928)); yoGraphicsListRegistry.registerYoGraphic("PushRobotControllers", bodyPushRobotController.getForceVisualizer()); for (QuadrupedJointName quadrupedJointName : modelFactory.get().getQuadrupedJointNames()) { String jointName = modelFactory.get().getSDFNameForJointName(quadrupedJointName); PushRobotController jointPushRobotController = new PushRobotController(sdfRobot.get(), jointName, new Vector3d(0.0, 0.0, 0.0)); yoGraphicsListRegistry.registerYoGraphic("PushRobotControllers", jointPushRobotController.getForceVisualizer()); } } }
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); }
@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)); } }
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());
scs.addYoGraphic(pushRobotController.getForceVisualizer());