public static FootstepTiming[] createTimings(int numberOfTimings) { FootstepTiming[] timings = new FootstepTiming[numberOfTimings]; for (int i = 0; i < numberOfTimings; i++) { timings[i] = new FootstepTiming(); } return timings; } }
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)); }
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()); }
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()); }
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; }
@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()); } }
@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); }