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); }