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() { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); // create simulation test helper FlatGroundEnvironment emptyEnvironment = new FlatGroundEnvironment(); String className = getClass().getSimpleName(); DRCRobotModel robotModel = getRobotModel(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, robotModel); drcSimulationTestHelper.setTestEnvironment(emptyEnvironment); drcSimulationTestHelper.createSimulation(className); Vector3D forcePointOffset = new Vector3D(0.0, 0.0, 0.1); pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), drcSimulationTestHelper.getRobot().getRootJoint().getName(), forcePointOffset); allowUpperBodyMomentumInSingleSupport = (YoBoolean) drcSimulationTestHelper.getYoVariable("allowUpperBodyMomentumInSingleSupport"); allowUpperBodyMomentumInDoubleSupport = (YoBoolean) drcSimulationTestHelper.getYoVariable("allowUpperBodyMomentumInDoubleSupport"); allowUsingHighMomentumWeight = (YoBoolean) drcSimulationTestHelper.getYoVariable("allowUsingHighMomentumWeight"); ThreadTools.sleep(1000); }
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); }
private DRCRobotModel setup(DRCStartingLocation startingLocation) throws SimulationExceededMaximumTimeException { String className = getClass().getSimpleName(); FlatGroundEnvironment environment = new FlatGroundEnvironment(); DRCRobotModel robotModel = getRobotModel(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, robotModel, environment); drcSimulationTestHelper.setStartingLocation(startingLocation); drcSimulationTestHelper.createSimulation(className); drcSimulationTestHelper.getSimulationConstructionSet().setCameraPosition(0.0, -3.0, 1.0); drcSimulationTestHelper.getSimulationConstructionSet().setCameraFix(0.0, 0.0, 0.2); pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), robotModel.createFullRobotModel().getChest().getParentJoint().getName(), new Vector3D(0, 0, 0.15)); ThreadTools.sleep(1000); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.5)); return robotModel; }
@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)); } }
FullHumanoidRobotModel fullRobotModel = getRobotModel().createFullRobotModel(); pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, getPushPositionZHeightInChestFrame())); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer());
pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, z)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer());
pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), robotModel.createFullRobotModel().getChest().getParentJoint().getName(), new Vector3D(0, 0, z));
pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, z)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer());
pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), robotModel.createFullRobotModel().getChest().getParentJoint().getName(), new Vector3D(0, 0, z));
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)); } }
double forceMagnitude = fullRobotModel.getTotalMass() * 2.0; String pushJointName = fullRobotModel.getPelvis().getParentJoint().getName(); PushRobotController pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), pushJointName, new Vector3D(), 1.0 / forceMagnitude); drcSimulationTestHelper.getSimulationConstructionSet().addYoGraphic(pushController.getForceVisualizer());
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); }
PushRobotController pushRobotController = new PushRobotController(robot, fullRobotModel);
double zOffset = 0.3; String pushJointName = fullRobotModel.getChest().getParentJoint().getName(); PushRobotController pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), pushJointName, new Vector3D(0, 0, zOffset), 1.0 / forceMagnitude); drcSimulationTestHelper.getSimulationConstructionSet().addYoGraphic(pushController.getForceVisualizer());
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); }
swingTime = robotModel.getWalkingControllerParameters().getDefaultSwingTime(); transferTime = robotModel.getWalkingControllerParameters().getDefaultTransferTime(); pushRobotController = new PushRobotController(simulationStarter.getSDFRobot(), fullRobotModel);
pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, z));
drcSimulationTestHelper.createSimulation(testName); double totalMass = fullRobotModel.getTotalMass(); PushRobotController pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet();