public abstract class DRCPushRecoveryMultiStepTest implements MultiRobotTestInterface {
private static final SimulationTestingParameters simulationTestingParameters =
SimulationTestingParameters.createFromEnvironmentVariables();
private DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack;
private static final boolean VISUALIZE_FORCE = false;
private static final double PUSH_DELAY = 0.5;
protected PushRobotController pushRobotController;
protected BlockingSimulationRunner blockingSimulationRunner;
protected double forceMagnitude;
protected double forceDuration;
protected SideDependentList<StateTransitionCondition> doubleSupportStartConditions =
new SideDependentList<>();
StateTransitionCondition pushCondition = doubleSupportStartConditions.get(RobotSide.LEFT);
@Before
public void showMemoryUsageBeforeTest() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
}
@After
public void destroySimulationAndRecycleMemory() {
if (simulationTestingParameters.getKeepSCSUp()) {
ThreadTools.sleepForever();
}
// Do this here in case a test fails. That way the memory will be recycled.
if (drcFlatGroundWalkingTrack != null) {
drcFlatGroundWalkingTrack.destroySimulation();
drcFlatGroundWalkingTrack = null;
}
if (blockingSimulationRunner != null) {
blockingSimulationRunner.destroySimulation();
blockingSimulationRunner = null;
}
pushRobotController = null;
doubleSupportStartConditions = null;
pushCondition = null;
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " after test.");
}
@Ignore("Needs to be improved")
@ContinuousIntegrationTest(estimatedDuration = 67.1)
@Test(timeout = 340000)
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);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(4.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, 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.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
@ContinuousIntegrationTest(estimatedDuration = 53.2)
@Test(timeout = 270000)
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);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(1.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, 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.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
protected void setupTest(DRCRobotModel robotModel)
throws SimulationExceededMaximumTimeException, InterruptedException {
boolean runMultiThreaded = false;
setupTrack(runMultiThreaded, robotModel);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
pushRobotController =
new PushRobotController(
drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);
if (VISUALIZE_FORCE) {
drcFlatGroundWalkingTrack
.getSimulationConstructionSet()
.addYoGraphic(pushRobotController.getForceVisualizer());
}
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
BooleanYoVariable enable =
(BooleanYoVariable) 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 EnumYoVariable<WalkingStateEnum> walkingState =
(EnumYoVariable<WalkingStateEnum>)
scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
doubleSupportStartConditions.put(
robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
}
}
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) {
DRCGuiInitialSetup guiInitialSetup =
new DRCGuiInitialSetup(true, false, simulationTestingParameters);
GroundProfile3D groundProfile = new FlatGroundProfile();
DRCSCSInitialSetup scsInitialSetup =
new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
// scsInitialSetup.setInitializeEstimatorToActual(true);
scsInitialSetup.setDrawGroundProfile(true);
scsInitialSetup.setRunMultiThreaded(runMultiThreaded);
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup =
robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
drcFlatGroundWalkingTrack =
new DRCFlatGroundWalkingTrack(
robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel);
drcFlatGroundWalkingTrack.getDrcSimulation();
}
private class DoubleSupportStartCondition implements StateTransitionCondition {
private final EnumYoVariable<WalkingStateEnum> walkingState;
private final RobotSide side;
public DoubleSupportStartCondition(
EnumYoVariable<WalkingStateEnum> walkingState, RobotSide side) {
this.walkingState = walkingState;
this.side = side;
}
@Override
public boolean checkCondition() {
if (side == RobotSide.LEFT) {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_LEFT_SUPPORT);
} else {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_RIGHT_SUPPORT);
}
}
}
private void createVideo(SimulationConstructionSet scs) {
if (simulationTestingParameters.getCreateSCSVideos()) {
BambooTools.createVideoAndDataWithDateTimeClassMethodAndShareOnSharedDriveIfAvailable(
getSimpleRobotName(), scs, 1);
}
}
protected abstract void setForwardPushParameters();
protected abstract void setBackwardPushParameters();
}
public abstract class DRCWalkToGoalBehaviorTest implements MultiRobotTestInterface {
private static final SimulationTestingParameters simulationTestingParameters =
SimulationTestingParameters.createFromEnvironmentVariables();
private DRCSimulationTestHelper drcSimulationTestHelper;
@Before
public void showMemoryUsageBeforeTest() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
}
@After
public void destroySimulationAndRecycleMemory() {
if (simulationTestingParameters.getKeepSCSUp()) {
ThreadTools.sleepForever();
}
// Do this here in case a test fails. That way the memory will be recycled.
if (drcSimulationTestHelper != null) {
drcSimulationTestHelper.destroySimulation();
drcSimulationTestHelper = null;
}
if (behaviorCommunicatorClient != null) {
behaviorCommunicatorClient.close();
}
if (behaviorCommunicatorServer != null) {
behaviorCommunicatorServer.close();
}
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " after test.");
}
private static final boolean DEBUG = true;
private final double ASSUMED_WALKING_SPEED_mPerSec = 0.2;
private final double POSITION_THRESHOLD = 0.1;
private final double ORIENTATION_THRESHOLD = 0.05;
private final double EXTRA_SIM_TIME_FOR_SETTLING = 1.0;
private final DRCDemo01NavigationEnvironment testEnvironment =
new DRCDemo01NavigationEnvironment();
private DoubleYoVariable yoTime;
private ForceSensorDataHolder forceSensorDataHolder;
private HumanoidRobotDataReceiver robotDataReceiver;
private BehaviorCommunicationBridge communicationBridge;
private HumanoidFloatingRootJointRobot robot;
private FullHumanoidRobotModel fullRobotModel;
private PacketCommunicator behaviorCommunicatorServer;
private PacketCommunicator behaviorCommunicatorClient;
@Before
public void setUp() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
behaviorCommunicatorServer =
PacketCommunicator.createIntraprocessPacketCommunicator(
NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
behaviorCommunicatorClient =
PacketCommunicator.createIntraprocessPacketCommunicator(
NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
try {
behaviorCommunicatorClient.connect();
behaviorCommunicatorServer.connect();
} catch (IOException e) {
throw new RuntimeException(e);
}
drcSimulationTestHelper =
new DRCSimulationTestHelper(
testEnvironment,
getSimpleRobotName(),
DRCObstacleCourseStartingLocation.DEFAULT,
simulationTestingParameters,
getRobotModel());
Robot robotToTest = drcSimulationTestHelper.getRobot();
yoTime = robotToTest.getYoTime();
robot = drcSimulationTestHelper.getRobot();
fullRobotModel = getRobotModel().createFullRobotModel();
forceSensorDataHolder =
new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions()));
robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
drcSimulationTestHelper.attachListener(RobotConfigurationData.class, robotDataReceiver);
PacketRouter<PacketDestination> networkProcessor = new PacketRouter<>(PacketDestination.class);
networkProcessor.attachPacketCommunicator(
PacketDestination.CONTROLLER, drcSimulationTestHelper.getControllerCommunicator());
networkProcessor.attachPacketCommunicator(
PacketDestination.BEHAVIOR_MODULE, behaviorCommunicatorClient);
communicationBridge =
new BehaviorCommunicationBridge(
behaviorCommunicatorServer, robotToTest.getRobotsYoVariableRegistry());
}
@ContinuousIntegrationTest(
estimatedDuration = 50.0,
categoriesOverride = IntegrationCategory.EXCLUDE)
@Test(timeout = 300000)
public void testWalkForwardsX() throws SimulationExceededMaximumTimeException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0);
assertTrue(success);
double walkDistance = RandomTools.generateRandomDouble(new Random(), 1.0, 2.0);
Vector2d walkDirection = new Vector2d(1, 0);
double trajectoryTime = walkDistance / ASSUMED_WALKING_SPEED_mPerSec;
WalkToGoalBehavior walkToGoalBehavior =
testWalkToGoalBehavior(walkDistance, walkDirection, trajectoryTime);
assertTrue(walkToGoalBehavior.isDone());
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
private WalkToGoalBehavior testWalkToGoalBehavior(
double walkDistance, Vector2d walkDirection, double trajectoryTime)
throws SimulationExceededMaximumTimeException {
FramePose2d initialMidFeetPose =
getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
FramePose2d desiredMidFeetPose =
offsetMidFeetPose2d(initialMidFeetPose, walkDistance, walkDirection);
final WalkToGoalBehavior walkToGoalBehavior =
new WalkToGoalBehavior(
communicationBridge,
fullRobotModel,
yoTime,
getRobotModel().getPhysicalProperties().getAnkleHeight());
communicationBridge.attachGlobalListener(
walkToGoalBehavior.getControllerGlobalPacketConsumer());
walkToGoalBehavior.initialize();
WalkToGoalBehaviorPacket requestedGoal =
new WalkToGoalBehaviorPacket(
desiredMidFeetPose.getX(),
desiredMidFeetPose.getY(),
desiredMidFeetPose.getYaw(),
RobotSide.RIGHT);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(requestedGoal);
WalkToGoalBehaviorPacket walkToGoalFindPathPacket =
new WalkToGoalBehaviorPacket(WalkToGoalAction.FIND_PATH);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalFindPathPacket);
WalkToGoalBehaviorPacket walkToGoalExecutePacket =
new WalkToGoalBehaviorPacket(WalkToGoalAction.EXECUTE);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalExecutePacket);
walkToGoalBehavior.doControl();
assertTrue(walkToGoalBehavior.hasInputBeenSet());
boolean success = executeBehavior(walkToGoalBehavior, trajectoryTime);
assertTrue(success);
FramePose2d finalMidFeetPose =
getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
// FramePose2d finalMidFeetPose = getCurrentMidFeetPose2d(referenceFrames);
PrintTools.debug(this, " initial Midfeet Pose :\n" + initialMidFeetPose + "\n");
assertPosesAreWithinThresholds(desiredMidFeetPose, finalMidFeetPose);
return walkToGoalBehavior;
}
private FramePose2d offsetMidFeetPose2d(
FramePose2d initialPose, double walkDistance, Vector2d walkDirection) {
FramePose2d ret = new FramePose2d(initialPose);
walkDirection.normalize();
ret.setX(initialPose.getX() + walkDistance * walkDirection.getX());
ret.setY(initialPose.getY() + walkDistance * walkDirection.getY());
return ret;
}
private boolean executeBehavior(final AbstractBehavior behavior, double trajectoryTime)
throws SimulationExceededMaximumTimeException {
final double simulationRunTime = trajectoryTime + EXTRA_SIM_TIME_FOR_SETTLING;
if (DEBUG) {
System.out.println("\n");
PrintTools.debug(
this, "starting behavior: " + behavior.getName() + " t = " + yoTime.getDoubleValue());
}
Thread behaviorThread =
new Thread() {
public void run() {
{
double startTime = Double.NaN;
boolean simStillRunning = true;
boolean initalized = false;
while (simStillRunning) {
if (!initalized) {
startTime = yoTime.getDoubleValue();
initalized = true;
}
double timeSpentSimulating = yoTime.getDoubleValue() - startTime;
simStillRunning = timeSpentSimulating < simulationRunTime;
robotDataReceiver.updateRobotModel();
behavior.doControl();
}
}
}
};
behaviorThread.start();
boolean ret = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(simulationRunTime);
PrintTools.debug(
this, "done with behavior: " + behavior.getName() + " t = " + yoTime.getDoubleValue());
return ret;
}
private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
robotDataReceiver.updateRobotModel();
referenceFrames.updateFrames();
ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();
FramePose midFeetPose = new FramePose();
midFeetPose.setToZero(midFeetFrame);
midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
midFeetPose.getReferenceFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
private FramePose2d getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(
HumanoidFloatingRootJointRobot robot) {
FramePose midFeetPose = getRobotMidFeetPose(robot);
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
ReferenceFrame.getWorldFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
private FramePose getRobotMidFeetPose(HumanoidFloatingRootJointRobot robot) {
FramePose leftFootPose = getRobotFootPose(robot, RobotSide.LEFT);
FramePose rightFootPose = getRobotFootPose(robot, RobotSide.RIGHT);
FramePose ret = new FramePose();
ret.interpolate(leftFootPose, rightFootPose, 0.5);
return ret;
}
private FramePose getRobotFootPose(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) {
List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide);
Joint ankleJoint = gcPoints.get(0).getParentJoint();
RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform();
ankleJoint.getTransformToWorld(ankleTransformToWorld);
FramePose ret = new FramePose();
ret.setPose(ankleTransformToWorld);
return ret;
}
private void assertPosesAreWithinThresholds(FramePose2d framePose1, FramePose2d framePose2) {
double positionDistance = framePose1.getPositionDistance(framePose2);
double orientationDistance = framePose1.getOrientationDistance(framePose2);
if (DEBUG) {
PrintTools.debug(this, " desired Midfeet Pose :\n" + framePose1 + "\n");
PrintTools.debug(this, " actual Midfeet Pose :\n" + framePose2 + "\n");
PrintTools.debug(this, " positionDistance = " + positionDistance);
PrintTools.debug(this, " orientationDistance = " + orientationDistance);
}
assertEquals(0.0, positionDistance, POSITION_THRESHOLD);
assertEquals(0.0, orientationDistance, ORIENTATION_THRESHOLD);
}
}