public void assertRobotDidNotFall() { Point3D center = new Point3D(0.0, 0.0, 0.8882009563211146); Vector3D plusMinusVector = new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox, drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot()); } }
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 DRCFlatGroundWalkingTrack setupWalkingSim() { DRCRobotModel robotModel = getRobotModel(); DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setDrawGroundProfile(false); externalPelvisPosePublisher = new ExternalPelvisPoseCreator(); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotModel.getDefaultRobotInitialSetup(0.0, 0.0), guiInitialSetup, scsInitialSetup, true, false, getRobotModel(), externalPelvisPosePublisher); simulationConstructionSet = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); registry = robot.getRobotsYoVariableRegistry(); YoBoolean walk = (YoBoolean) simulationConstructionSet.getVariable("walkCSG"); walk.set(true); return drcFlatGroundWalkingTrack; }
private void enablePartialFootholdDetectionAndResponse(DRCSimulationTestHelper drcSimulationTestHelper, double chickenPercentage) HumanoidFloatingRootJointRobot simulatedRobot = drcSimulationTestHelper.getAvatarSimulation().getHumanoidFloatingRootJointRobot();
sdfRobot = avatarSimulation.getHumanoidFloatingRootJointRobot();
private void enablePartialFootholdDetectionAndResponse(DRCSimulationTestHelper drcSimulationTestHelper, double chickenPercentage) HumanoidFloatingRootJointRobot simulatedRobot = drcSimulationTestHelper.getAvatarSimulation().getHumanoidFloatingRootJointRobot();
public LocalObjectCommunicator createSimulatedSensorsPacketCommunicator() { scsSensorOutputPacketCommunicator = new LocalObjectCommunicator(); if (createSCSSimulatedSensors) { DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); DRCRobotJointMap jointMap = robotModel.getJointMap(); TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider(); HumanoidFloatingRootJointRobot robot = avatarSimulation.getHumanoidFloatingRootJointRobot(); Graphics3DAdapter graphics3dAdapter = simulationConstructionSet.getGraphics3dAdapter(); printIfDebug("Streaming SCS Video"); DRCRobotCameraParameters cameraParameters = sensorInformation.getCameraParameters(0); if (cameraParameters != null) { String cameraName = cameraParameters.getSensorNameInSdf(); int width = sdfRobot.getCameraMount(cameraName).getImageWidth(); int height = sdfRobot.getCameraMount(cameraName).getImageHeight(); CameraConfiguration cameraConfiguration = new CameraConfiguration(cameraName); cameraConfiguration.setCameraMount(cameraName); int framesPerSecond = 25; DRCRenderedSceneVideoHandler drcRenderedSceneVideoHandler = new DRCRenderedSceneVideoHandler(scsSensorOutputPacketCommunicator); simulationConstructionSet.startStreamingVideoData(cameraConfiguration, width, height, drcRenderedSceneVideoHandler, timeStampProvider, framesPerSecond); } for (DRCRobotLidarParameters lidarParams : sensorInformation.getLidarParameters()) { DRCLidar.setupDRCRobotLidar(robot, graphics3dAdapter, scsSensorOutputPacketCommunicator, jointMap, lidarParams, timeStampProvider, true); } } return scsSensorOutputPacketCommunicator; }
DRCRobotJointMap jointMap = robotModel.getJointMap(); TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider(); HumanoidFloatingRootJointRobot robot = avatarSimulation.getHumanoidFloatingRootJointRobot(); Graphics3DAdapter graphics3dAdapter = simulationConstructionSet.getGraphics3dAdapter();
sdfRobot = avatarSimulation.getHumanoidFloatingRootJointRobot();
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)); } }
@ContinuousIntegrationTest(estimatedDuration = 53.2) @Test(timeout = 30000) public void testMultiStepBackwardAndContinueWalking() throws SimulationExceededMaximumTimeException, InterruptedException, ControllerFailureException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); setupTest(getRobotModel()); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setBackwardPushParameters(); Vector3D forceDirection = new Vector3D(1.0, 0.0, 0.0); blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0); YoBoolean walk = (YoBoolean) scs.getVariable("ContinuousStepGenerator", "walkCSG"); // disable walking walk.set(false); blockingSimulationRunner.simulateAndBlock(1.0); // push the robot pushRobotController.applyForceDelayed(pushCondition, getPushDelay(), forceDirection, forceMagnitude, forceDuration); // simulate for a little while longer blockingSimulationRunner.simulateAndBlock(forceDuration + 5.0); // re-enable walking walk.set(true); blockingSimulationRunner.simulateAndBlock(6.0); Point3D center = new Point3D(0.0, 0.0, 0.8882009563211146); Vector3D plusMinusVector = new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox, drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot()); createVideo(scs); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
cheatWithGroundHeightAtForFootstep, model); FloatingRootJointRobot robot = track.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); FullHumanoidRobotModel fullRobotModel = model.createFullRobotModel(); PushRobotController pushRobotController = new PushRobotController(robot, fullRobotModel);
@ContinuousIntegrationTest(estimatedDuration = 67.1) @Test(timeout = 30000) @Ignore("Needs to be improved") public void testMultiStepForwardAndContinueWalking() throws SimulationExceededMaximumTimeException, InterruptedException, ControllerFailureException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); setupTest(getRobotModel()); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setForwardPushParameters(); Vector3D forceDirection = new Vector3D(1.0, 0.0, 0.0); blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0); YoBoolean walk = (YoBoolean) scs.getVariable("ContinuousStepGenerator", "walkCSG"); // disable walking walk.set(false); blockingSimulationRunner.simulateAndBlock(4.0); // push the robot pushRobotController.applyForceDelayed(pushCondition, getPushDelay(), forceDirection, forceMagnitude, forceDuration); // simulate for a little while longer blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0); // re-enable walking walk.set(true); blockingSimulationRunner.simulateAndBlock(6.0); Point3D center = new Point3D(0.0, 0.0, 0.8882009563211146); Vector3D plusMinusVector = new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox, drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot()); createVideo(scs); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
HumanoidFloatingRootJointRobot simulatedRobot = drcSimulationTestHelper.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); for (RobotSide robotSide : RobotSide.values)
HumanoidFloatingRootJointRobot simulatedRobot = drcSimulationTestHelper.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); for (RobotSide robotSide : RobotSide.values)