How to use

getHumanoidFloatingRootJointRobot

method

in

us.ihmc.avatar.factory.AvatarSimulation

  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)