How to use

getWalkingControllerParameters

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

protected double getForceDelay1()
{
 return 0.5 * robotModel.getWalkingControllerParameters().getDefaultSwingTime();
}

protected double getForceDelay1()
{
 return 0.5 * robotModel.getWalkingControllerParameters().getDefaultSwingTime();
}

protected double getForceDelay2()
{
 return 2.5 * robotModel.getWalkingControllerParameters().getDefaultSwingTime();
}

protected double getForceDelay2()
{
 return 2.5 * robotModel.getWalkingControllerParameters().getDefaultSwingTime();
}

private boolean isUsingPelvisHeightControlOnly()
{
 return getRobotModel().getWalkingControllerParameters().usePelvisHeightControllerOnly();
}

private boolean areFootstepsTooFarApart(FootstepListBehavior footstepListBehavior, ArrayList<Footstep> desiredFootsteps)
{
 ArrayList<Double> footStepLengths = footstepListBehavior.getFootstepLengths(createFootstepDataList(desiredFootsteps),
                                       drcBehaviorTestHelper.getSDFFullRobotModel(),
                                       getRobotModel().getWalkingControllerParameters());
 if (DEBUG)
   for (double footStepLength : footStepLengths)
   {
    PrintTools.debug(this, "foot step length : " + footStepLength);
   }
 boolean footStepsAreTooFarApart = footstepListBehavior.areFootstepsTooFarApart(createFootstepDataList(desiredFootsteps), fullRobotModel,
                                         getRobotModel().getWalkingControllerParameters());
 return footStepsAreTooFarApart;
}

public TestingEnvironment()
{
  SteppingParameters steppingParameters = getRobotModel().getWalkingControllerParameters().getSteppingParameters();
  double flatArea = steppingParameters.getDefaultStepLength() * 0.5;
  double maxElevation = steppingParameters.getMinSwingHeightFromStanceFoot() * 0.25;
  terrain = new CombinedTerrainObject3D(getClass().getSimpleName());
  terrain.addBox(-0.5 - flatArea / 2.0, -1.0, flatArea / 2.0, 1.0, -0.01, 0.0);
  for (int i = 0; i < 50; i++)
  {
   double xStart = flatArea + i * flatArea - flatArea / 2.0;
   double height = maxElevation * 2.0 * (random.nextDouble() - 0.5);
   double length = flatArea;
   terrain.addBox(xStart, -1.0, xStart + length, 1.0, height - 0.01, height);
  }
}

private ArrayList<? extends Point2DBasics> generateContactPointsForAllOfFoot()
{
 WalkingControllerParameters walkingControllerParameters = getRobotModel().getWalkingControllerParameters();
 double footForwardOffset = walkingControllerParameters.getSteppingParameters().getFootForwardOffset();
 double footBackwardOffset = walkingControllerParameters.getSteppingParameters().getFootBackwardOffset();
 double footWidth = walkingControllerParameters.getSteppingParameters().getFootWidth();
 double toeWidth = walkingControllerParameters.getSteppingParameters().getToeWidth();
 ArrayList<Point2D> ret = new ArrayList<Point2D>();
 ret.add(new Point2D(footForwardOffset, toeWidth / 2.0));
 ret.add(new Point2D(footForwardOffset, -toeWidth / 2.0));
 ret.add(new Point2D(-footBackwardOffset, -footWidth / 2.0));
 ret.add(new Point2D(-footBackwardOffset, footWidth / 2.0));
 return ret;
}

private ArrayList<Point2D> generateContactPointsForAllOfFoot()
{
 WalkingControllerParameters walkingControllerParameters = getRobotModel().getWalkingControllerParameters();
 double footForwardOffset = walkingControllerParameters.getSteppingParameters().getFootForwardOffset();
 double footBackwardOffset = walkingControllerParameters.getSteppingParameters().getFootBackwardOffset();
 double footWidth = walkingControllerParameters.getSteppingParameters().getFootWidth();
 double toeWidth = walkingControllerParameters.getSteppingParameters().getToeWidth();
 ArrayList<Point2D> ret = new ArrayList<Point2D>();
 ret.add(new Point2D(footForwardOffset, toeWidth / 2.0));
 ret.add(new Point2D(footForwardOffset, -toeWidth / 2.0));
 ret.add(new Point2D(-footBackwardOffset, -footWidth / 2.0));
 ret.add(new Point2D(-footBackwardOffset, footWidth / 2.0));
 return ret;
}

private WalkToLocationBehavior createNewWalkToLocationBehavior()
{
 Ros2Node ros2Node = drcBehaviorTestHelper.getRos2Node();
 FullHumanoidRobotModel fullRobotModel = drcBehaviorTestHelper.getSDFFullRobotModel();
 HumanoidReferenceFrames referenceFrames = drcBehaviorTestHelper.getReferenceFrames();
 WalkingControllerParameters walkingControllerParams = getRobotModel().getWalkingControllerParameters();
 final WalkToLocationBehavior walkToLocationBehavior = new WalkToLocationBehavior(drcBehaviorTestHelper.getRobotName(), ros2Node, fullRobotModel, referenceFrames, walkingControllerParams);
 return walkToLocationBehavior;
}

private void addCorruptionToFootstepTimings(FootstepDataListMessage message)
{
 for (int i = 0; i < message.getFootstepDataList().size(); i++)
 {
   double transferCorruption = RandomNumbers.nextDouble(random, 0.02);
   double swingCorruption = RandomNumbers.nextDouble(random, 0.02);
   double transferDuration = getRobotModel().getWalkingControllerParameters().getDefaultTransferTime();
   double swingDuration = getRobotModel().getWalkingControllerParameters().getDefaultSwingTime();
   transferDuration += transferCorruption;
   swingDuration += swingCorruption;
   message.getFootstepDataList().get(i).setTransferDuration(transferDuration);
   message.getFootstepDataList().get(i).setSwingDuration(swingDuration);
 }
}

private ArrayList<Point2D> generateContactPointsForRandomRotatedLineOfContact(Random random)
{
 double angle = RandomNumbers.nextDouble(random, Math.PI);
 SteppingParameters steppingParameters = getRobotModel().getWalkingControllerParameters().getSteppingParameters();
 double footLengthForLineOrigin = 0.5 * steppingParameters.getFootLength();
 double footWidthForLineOrigin = 0.5 * steppingParameters.getFootWidth();
 double x = RandomNumbers.nextDouble(random, footLengthForLineOrigin) - footLengthForLineOrigin/2.0;
 double y = RandomNumbers.nextDouble(random, footWidthForLineOrigin) - footWidthForLineOrigin/2.0;
 return generateContactPointsForRotatedLineOfContact(angle, x, y);
}

private double computeWalkingDuration(FootstepDataListMessage footstepDataList)
{
 WalkingControllerParameters walkingControllerParameters = getRobotModel().getWalkingControllerParameters();
 double stepDuration = walkingControllerParameters.getDefaultTransferTime() + walkingControllerParameters.getDefaultSwingTime();
 double totalDuration = footstepDataList.getFootstepDataList().size() * stepDuration;
 totalDuration += walkingControllerParameters.getDefaultFinalTransferTime() - walkingControllerParameters.getDefaultTransferTime()
    + walkingControllerParameters.getDefaultInitialTransferTime();
 totalDuration += 0.5;
 return totalDuration;
}

private double sendWalkingPacket(DRCRobotModel robotModel, FullHumanoidRobotModel fullRobotModel, HumanoidReferenceFrames referenceFrames,
                YoVariableRegistry registry)
{
 referenceFrames.updateFrames();
 WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
 double swingTime = walkingControllerParameters.getDefaultSwingTime();
 double transferTime = walkingControllerParameters.getDefaultTransferTime();
 double stepTime = swingTime + transferTime;
 Vector2D desiredVelocity = new Vector2D(0.15, 0.0);
 int numberOfSteps = 10;
 FootstepDataListMessage footsteps = computeNextFootsteps(numberOfSteps, RobotSide.LEFT, referenceFrames.getSoleFrames(), walkingControllerParameters,
                              desiredVelocity);
 footsteps.setDefaultSwingDuration(swingTime);
 footsteps.setDefaultTransferDuration(transferTime);
 drcSimulationTestHelper.publishToController(footsteps);
 int timeWalking = numberOfSteps;
 double timeToCompleteWalking = stepTime * timeWalking;
 return timeToCompleteWalking;
}

private DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment, GroundProfile3D groundProfile3D)
{
 this.robotModel = robotModel;
 this.environment = environment;
 this.guiInitialSetup = new DRCGuiInitialSetup(false, false);
 this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0);
 this.createSCSSimulatedSensors = true;
 this.scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT());
 this.scsInitialSetup.setDrawGroundProfile(environment == null);
 this.scsInitialSetup.setInitializeEstimatorToActual(false);
 this.scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT());
 this.scsInitialSetup.setRunMultiThreaded(true);
 this.highLevelControllerParameters = robotModel.getHighLevelControllerParameters();
 this.walkingControllerParameters = robotModel.getWalkingControllerParameters();
 this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters();
 this.contactPointParameters = robotModel.getContactPointParameters();
}

public DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment)
{
 this.robotModel = robotModel;
 this.environment = environment;
 this.guiInitialSetup = new DRCGuiInitialSetup(false, false);
 this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0);
 this.createSCSSimulatedSensors = true;
 scsInitialSetup = new DRCSCSInitialSetup(environment, robotModel.getSimulateDT());
 scsInitialSetup.setInitializeEstimatorToActual(false);
 scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT());
 scsInitialSetup.setRunMultiThreaded(true);
 this.walkingControllerParameters = robotModel.getWalkingControllerParameters();
 this.armControllerParameters = robotModel.getArmControllerParameters();
 this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters();
 this.icpOptimizationParameters = robotModel.getICPOptimizationParameters();
 this.contactPointParameters = robotModel.getContactPointParameters();
}

private boolean pickupFoot(RobotSide robotSide, RigidBodyBasics foot) throws SimulationExceededMaximumTimeException
{
 Point3D desiredPosition = new Point3D();
 Quaternion desiredOrientation = new Quaternion();
 double timeToPickupFoot = 1.0;
 FramePose3D footPoseCloseToActual = new FramePose3D(foot.getBodyFixedFrame());
 footPoseCloseToActual.setPosition(0.0, 0.0, getLiftOffHeight());
 footPoseCloseToActual.changeFrame(ReferenceFrame.getWorldFrame());
 footPoseCloseToActual.get(desiredPosition, desiredOrientation);
 FootTrajectoryMessage footTrajectoryMessage = HumanoidMessageTools.createFootTrajectoryMessage(robotSide, timeToPickupFoot, desiredPosition, desiredOrientation);
 drcSimulationTestHelper.publishToController(footTrajectoryMessage);
 return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(timeToPickupFoot + getRobotModel().getWalkingControllerParameters().getDefaultInitialTransferTime());
}

private void createControllerCore(YoVariableRegistry walkingControllerRegistry)
{
 JointBasics[] jointsToIgnore = DRCControllerThread.createListOfJointsToIgnore(fullRobotModel, robotModel, robotModel.getSensorInformation());
 JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullRobotModel, jointsToIgnore);
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame();
 WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
 MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings();
 WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame,
                                    momentumOptimizationSettings, yoGraphicsListRegistry, walkingControllerRegistry);
 toolbox.setupForInverseDynamicsSolver(contactableBodies);
 JointPrivilegedConfigurationParameters jointPrivilegedConfigurationParameters = walkingControllerParameters.getJointPrivilegedConfigurationParameters();
 toolbox.setJointPrivilegedConfigurationParameters(jointPrivilegedConfigurationParameters);
 FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate();
 controllerOutput = new JointDesiredOutputList(fullRobotModel.getControllableOneDoFJoints());
 controllerCore = new WholeBodyControllerCore(toolbox, template, controllerOutput, walkingControllerRegistry);
}

private boolean putFootOnGround(RobotSide robotSide, RigidBodyBasics foot, FramePose3D desiredPose) throws SimulationExceededMaximumTimeException
{
 Point3D desiredPosition = new Point3D();
 Quaternion desiredOrientation = new Quaternion();
 double trajectoryTime = 1.0;
 desiredPose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 desiredPose.get(desiredPosition, desiredOrientation);
 FootTrajectoryMessage footTrajectoryMessage = HumanoidMessageTools.createFootTrajectoryMessage(robotSide, trajectoryTime, desiredPosition, desiredOrientation);
 drcSimulationTestHelper.publishToController(footTrajectoryMessage);
 FootLoadBearingMessage loadBearingMessage = new FootLoadBearingMessage();
 loadBearingMessage.setRobotSide(robotSide.toByte());
 loadBearingMessage.setLoadBearingRequest(LoadBearingRequest.LOAD.toByte());
 drcSimulationTestHelper.publishToController(loadBearingMessage);
 return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.2 + trajectoryTime + getRobotModel().getWalkingControllerParameters().getDefaultInitialTransferTime());
}

private void moveFootToRandomPosition(Random random, RobotSide robotSide, RigidBodyBasics foot, SimulationConstructionSet scs)
   throws SimulationExceededMaximumTimeException
{
 Point3D desiredPosition = new Point3D();
 Quaternion desiredOrientation = new Quaternion();
 double trajectoryTime = 1.0;
 String bodyName = foot.getName();
 FramePose3D desiredRandomFootPose = new FramePose3D(foot.getBodyFixedFrame());
 desiredRandomFootPose.setOrientation(RandomGeometry.nextQuaternion(random, 1.0));
 desiredRandomFootPose.setPosition(getRandomPositionInSphere(random, robotSide));
 desiredRandomFootPose.changeFrame(ReferenceFrame.getWorldFrame());
 desiredRandomFootPose.get(desiredPosition, desiredOrientation);
 FootTrajectoryMessage footTrajectoryMessage = HumanoidMessageTools.createFootTrajectoryMessage(robotSide, trajectoryTime, desiredPosition, desiredOrientation);
 drcSimulationTestHelper.publishToController(footTrajectoryMessage);
 boolean result = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + getRobotModel().getWalkingControllerParameters().getDefaultInitialTransferTime());
 assertTrue(result);
 EndToEndHandTrajectoryMessageTest.assertSingleWaypointExecuted(bodyName, desiredPosition, desiredOrientation, scs);
}