PushRobotController pushRobotController = new PushRobotController(robot, fullRobotModel); pushRobotController.addPushButtonToSCS(track.getSimulationConstructionSet()); scs.addYoGraphic(pushRobotController.getForceVisualizer()); pushRobotController.setPushDuration(defaultForceDurationInSeconds); pushRobotController.setPushForceMagnitude(defaultForceMagnitude); pushRobotController.setPushForceDirection(defaultForceDirection);
public void applyForceDelayed(StateTransitionCondition pushCondition, double timeDelay, Vector3DReadOnly direction, double magnitude, double duration) { this.pushCondition = pushCondition; setPushDuration(duration); setPushForceDirection(direction); setPushForceMagnitude(magnitude); setPushDelay(timeDelay); applyForce(); }
public void applyForce(Vector3DReadOnly direction, double magnitude, double duration) { applyForceDelayed(null, 0.0, direction, magnitude, duration); }
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()); } } }
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()); pushController.applyForce(new Vector3D(0.0, 0.0, 1.0), forceMagnitude, Double.POSITIVE_INFINITY);
private boolean standAndPush() throws SimulationExceededMaximumTimeException { assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0)); pushController.applyForce(new Vector3D(1.0, 0.0, 0.0), getDoubleSupportPushMagnitude(), getDoubleSupportPushDuration()); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10.0); }
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); }
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()); pushController.applyForce(new Vector3D(1.0, 0.0, 0.0), forceMagnitude, Double.POSITIVE_INFINITY);
@ContinuousIntegrationTest(estimatedDuration = 0.1, categoriesOverride = IntegrationCategory.EXCLUDE) @Test(timeout=300000) public void TestPushTowardsTheFront() throws SimulationExceededMaximumTimeException, InterruptedException, ControllerFailureException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); setupTest(getRobotModel()); // setup all parameters Vector3D forceDirection = new Vector3D(0.5, 1.0, 0.0); double magnitude = 8000.0; double duration = 0.05; // double percentInSwing = 0.4; // RobotSide side = RobotSide.LEFT; // apply the push // testPush(forceDirection, magnitude, duration, percentInSwing, side, swingStartConditions, swingTime); blockingSimulationRunner.simulateAndBlock(8.0); pushRobotController.applyForce(forceDirection, magnitude, duration); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
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)); } }
public void applyForce(Vector3d direction, double magnitude, double duration) { applyForceDelayed(null, 0.0, direction, magnitude, duration); }
public void applyForceDelayed(StateTransitionCondition pushCondition, double timeDelay, Vector3d direction, double magnitude, double duration) { this.pushCondition = pushCondition; setPushDuration(duration); setPushForceDirection(direction); setPushForceMagnitude(magnitude); setPushDelay(timeDelay); applyForce(); }
drcSimulationTestHelper.createSimulation(testName); double totalMass = fullRobotModel.getTotalMass(); PushRobotController pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer()); double magnitude = percentWeight * totalMass * 9.81; double duration = 2.0; pushRobotController.applyForce(forceDirection, magnitude, duration);
pushRobotController.applyForce(forceDirection, forceMagnitude, forceDuration); pushRobotController.applyForce(forceDirection, forceMagnitude, forceDuration);
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)); } }
private void testPush(Vector3D forceDirection, double magnitude, double duration, double percentInState, RobotSide side, SideDependentList<StateTransitionCondition> condition, double stateTime) throws SimulationExceededMaximumTimeException, InterruptedException { System.out.println("DRCHumanoidBehaviorICPFaultDetectionFault: testPush called."); double delay = stateTime * percentInState; pushRobotController.applyForceDelayed(null, 800, forceDirection, magnitude, duration); }
pushRobotController.applyForce(forceDirection, forceMagnitude, forceDuration); pushRobotController.applyForce(forceDirection, forceMagnitude, forceDuration);
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());