private FootstepTiming createFootstepTiming(FootstepDataCommand footstep) { FootstepTiming timing = new FootstepTiming(); if (footstep.hasTimings()) timing.setTimings(footstep.getSwingTime(), footstep.getTransferTime()); else { if (upcomingFootstepTimings.isEmpty()) timing.setTimings(defaultSwingTime.getDoubleValue(), defaultInitialTransferTime.getDoubleValue()); else timing.setTimings(defaultSwingTime.getDoubleValue(), defaultTransferTime.getDoubleValue()); } if (footstep.hasAbsoluteTime()) timing.setAbsoluteTime(timing.getSwingStartTime()); return timing; }
public void addFootstepToPlan(Footstep footstep, FootstepTiming timing) { if (footstep == null) return; referenceCMPsCalculator.addUpcomingFootstep(footstep); int footstepIndex = referenceCMPsCalculator.getNumberOfFootstepRegistered() - 1; swingTimes.get(footstepIndex).set(timing.getSwingTime()); transferTimes.get(footstepIndex).set(timing.getTransferTime()); }
public double getNextStepTime() { if (upcomingFootstepTimings.isEmpty()) return getDefaultStepTime(); return upcomingFootstepTimings.get(0).getStepTime(); }
private void checkTimings(List<FootstepTiming> upcomingFootstepTimings) { if (upcomingFootstepTimings.isEmpty()) return; boolean timingsValid = upcomingFootstepTimings.get(0).hasAbsoluteTime(); boolean atLeastOneFootstepHadTiming = upcomingFootstepTimings.get(0).hasAbsoluteTime(); double lastTime = upcomingFootstepTimings.get(0).getSwingStartTime(); timingsValid = timingsValid && lastTime > 0.0; for (int footstepIdx = 1; footstepIdx < upcomingFootstepTimings.size(); footstepIdx++) { FootstepTiming footstep = upcomingFootstepTimings.get(footstepIdx); boolean timeIncreasing = footstep.getSwingStartTime() > lastTime; timingsValid = timingsValid && footstep.hasAbsoluteTime() && timeIncreasing; atLeastOneFootstepHadTiming = atLeastOneFootstepHadTiming || footstep.hasAbsoluteTime(); lastTime = footstep.getSwingStartTime(); if (!timingsValid) break; } if (atLeastOneFootstepHadTiming && !timingsValid) { PrintTools.warn("Recieved footstep data with invalid timings. Using swing and transfer times instead."); for (int footstepIdx = 1; footstepIdx < upcomingFootstepTimings.size(); footstepIdx++) upcomingFootstepTimings.get(footstepIdx).removeAbsoluteTime(); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { FootstepData testObject = new FootstepData(); assertTrue(testObject != null); Footstep testFootstep = new Footstep(RobotSide.LEFT); FootstepTiming testFootstepTiming = new FootstepTiming(0.2, 0.1); testFootstepTiming.setAbsoluteTime(0.1, 0.5); testObject.set(testFootstep, testFootstepTiming); assertTrue(testObject.getFootstep() == testFootstep); assertTrue(testObject.getStepTime() == testFootstepTiming.getStepTime()); assertTrue(testObject.getSwingSide() == testFootstep.getRobotSide()); assertTrue(testObject.getSupportSide() == testFootstep.getRobotSide().getOppositeSide()); assertTrue(testObject.getSwingTime() == testFootstepTiming.getSwingTime()); assertTrue(testObject.getTransferTime() == testFootstepTiming.getTransferTime()); assertTrue(MathTools.epsilonEquals(testObject.getTransferStartTime(), testFootstepTiming.getExecutionStartTime(), Epsilons.ONE_BILLIONTH)); assertTrue(testObject.getSwingStartTime() == testFootstepTiming.getSwingStartTime()); assertTrue(testObject.getFootstepPoseReferenceFrame() == testFootstep.getFootstepPose().getReferenceFrame()); assertTrue(testObject.getFramePose() == testFootstep.getFootstepPose()); testObject.setSwingTime(0.8); assertTrue(testObject.getSwingTime() == 0.8); Footstep newFootstep = new Footstep(RobotSide.LEFT); testObject.setFootstep(newFootstep); assertTrue(testObject.getFootstep() == newFootstep); }
public FootstepTiming(double swingTime, double transferTime) { setTimings(swingTime, transferTime); }
public static FootstepTiming[] createTimings(int numberOfTimings) { FootstepTiming[] timings = new FootstepTiming[numberOfTimings]; for (int i = 0; i < numberOfTimings; i++) { timings[i] = new FootstepTiming(); } return timings; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCopy() { Footstep footstep = new Footstep(RobotSide.RIGHT); FootstepTiming timing = new FootstepTiming(0.1, 1.2); FootstepData obj1 = new FootstepData(footstep, timing); FootstepData obj2 = new FootstepData(); assertTrue(obj2.getFootstep() == null); obj2.set(obj1); assertTrue(obj2.getFootstep() == footstep); assertTrue(obj2.getStepTime() == timing.getStepTime()); } }
public double getNextTransferTime() { if (upcomingFootstepTimings.isEmpty()) return getDefaultTransferTime(); return upcomingFootstepTimings.get(0).getTransferTime(); }
public double getNextSwingTime() { if (upcomingFootstepTimings.isEmpty()) return getDefaultSwingTime(); return upcomingFootstepTimings.get(0).getSwingTime(); }
footstepTiming.setTimings(swingTime, defaultTransferTime); nextFootstep = balanceManager.createFootstepForRecoveringFromDisturbance(swingSide, defaultSwingTime); nextFootstep.setTrajectoryType(TrajectoryType.DEFAULT); footstepTiming.set(walkingMessageHandler.peekTiming(0)); nextFootstep = walkingMessageHandler.poll();
public FootstepTiming(double swingTime, double transferTime) { setTimings(swingTime, transferTime); }
private void planFootsteps() { FootstepTestHelper footstepHelper = new FootstepTestHelper(feet); footstepList = footstepHelper.createFootsteps(stepWidth, stepLength, numberOfFootstepsForTest); timingList = new ArrayList<>(footstepList.size()); timingList.add(new FootstepTiming(defaultInitialSwingTime, defaultInitialTransferTime)); for (int i = 0; i < footstepList.size() - 1; i++) timingList.add(new FootstepTiming(defaultSwingTime, defaultTransferTime)); }
swingTime = timing.getSwingTime(); transferTime = timing.getTransferTime();
public FootstepTiming(double swingTime, double touchdownTime, double transferTime) { setTimings(swingTime, touchdownTime, transferTime); }
public void sendFootStepMessages(int numberOfFootstepsToPlan) { RobotSide robotSide = RobotSide.LEFT; FramePoint3D footstepLocation = new FramePoint3D(); FrameQuaternion footstepOrientation = new FrameQuaternion(); upcomingFootstepsData.clear(); for (int i = 1; i < numberOfFootstepsToPlan + 1; i++) { Footstep footstep = new Footstep(robotSide); footstepLocation.set(i * stepLength, robotSide.negateIfRightSide(stepWidth), 0.0); footstep.setPose(footstepLocation, footstepOrientation); FootstepTiming timing = new FootstepTiming(swingTime, transferTime); upcomingFootstepsData.add(new FootstepData(footstep, timing)); robotSide = robotSide.getOppositeSide(); } numberOfUpcomingFootsteps.set(upcomingFootstepsData.size()); }
(inDoubleSupport.getBooleanValue() ? timingList.get(currentStepCount).getTransferTime() : timingList.get(currentStepCount).getSwingTime()), currentStepCount, planner1, planner2); currentStepCount = updateStateMachine(currentStepCount);
private void initiateFootLoadingProcedure(RobotSide swingSide) { loadFoot.set(true); loadFootStartTime.set(getTimeInCurrentState()); balanceManager.clearICPPlan(); footstepTiming.setTimings(loadFootDuration.getDoubleValue(), loadFootTransferDuration.getDoubleValue()); balanceManager.addFootstepToPlan(walkingMessageHandler.getFootstepAtCurrentLocation(swingSide), footstepTiming); balanceManager.setICPPlanSupportSide(supportSide); double defaultSwingTime = loadFootDuration.getDoubleValue(); double defaultTransferTime = loadFootTransferDuration.getDoubleValue(); double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); balanceManager.initializeICPPlanForSingleSupport(defaultSwingTime, defaultTransferTime, finalTransferTime); balanceManager.freezePelvisXYControl(); } }
private void setupInputs() { clear(); RobotSide side = RobotSide.LEFT; for (int i = 0; i < testParameters.getNumberOfFootstepsToConsider(); i++) { // Update the current location based on walking direction tempFrameVector.setIncludingFrame(walkingDirectionUnitVector); tempFrameVector.scale(stepLength); currentLocation.add(tempFrameVector); // Create new footstep based on updated CoM location Footstep newFootstep = new Footstep(); newFootstep.setRobotSide(side); getFootLocationFromCoMLocation(tempFramePoint1, side, currentLocation, walkingDirectionUnitVector, stepLength, stepWidth); // Set calculated pose to footstep newFootstep.setPose(tempFramePoint1, robotOrientation); upcomingFootstepsData.add(new FootstepData(newFootstep, new FootstepTiming(swingTime, transferTime))); side = side.getOppositeSide(); } numberOfUpcomingFootsteps.set(upcomingFootstepsData.size()); }
testForPlanningConsistency(inDoubleSupport.getBooleanValue(), currentStepCount); simulateTicks(checkForDiscontinuities, checkIfDynamicsAreSatisfied, (inDoubleSupport.getBooleanValue() ? timingList.get(currentStepCount).getTransferTime() : timingList.get(currentStepCount).getSwingTime()), currentStepCount); currentStepCount = updateStateMachine(currentStepCount);