private void initializeStanceFeet(ArrayList<Footstep> previousFootsteps, AbstractFootstepGenerator generator) { if (previousFootsteps != null) { int numFootsteps = previousFootsteps.size(); RobotSide lastStepSide = RobotSide.RIGHT; if (numFootsteps >= 1) { Footstep lastFootstep = previousFootsteps.get(numFootsteps - 1); lastStepSide = lastFootstep.getRobotSide(); setTransitionStep(lastFootstep, transitionStanceFeet); } else { System.err.println("Why are there no feet generated for the first segment of TurnTranslateTurnFootstepGenerator?"); setTransitionStep(FootstepUtils.generateStandingFootstep(lastStepSide, feet.get(lastStepSide), soleFrames.get(lastStepSide)), transitionStanceFeet); } if (numFootsteps >= 2) { setTransitionStep(previousFootsteps.get(numFootsteps - 2), transitionStanceFeet); } else { RobotSide lastStepOppositeSide = lastStepSide.getOppositeSide(); setTransitionStep(FootstepUtils.generateStandingFootstep(lastStepOppositeSide, feet.get(lastStepOppositeSide), soleFrames.get(lastStepOppositeSide)), transitionStanceFeet); } generator.setPriorFootposes(transitionStanceFeet); } generator.initialize(); }
private void initializeStanceFeet(ArrayList<Footstep> previousFootsteps, AbstractFootstepGenerator generator) { if (previousFootsteps != null) { int numFootsteps = previousFootsteps.size(); RobotSide lastStepSide = RobotSide.RIGHT; if (numFootsteps >= 1) { Footstep lastFootstep = previousFootsteps.get(numFootsteps - 1); lastStepSide = lastFootstep.getRobotSide(); setTransitionStep(lastFootstep, transitionStanceFeet); } else { System.err.println("Why are there no feet generated for the first segment of TurnTranslateTurnFootstepGenerator?"); setTransitionStep(FootstepUtils.generateStandingFootstep(lastStepSide, feet.get(lastStepSide), soleFrames.get(lastStepSide)), transitionStanceFeet); } if (numFootsteps >= 2) { setTransitionStep(previousFootsteps.get(numFootsteps - 2), transitionStanceFeet); } else { RobotSide lastStepOppositeSide = lastStepSide.getOppositeSide(); setTransitionStep(FootstepUtils.generateStandingFootstep(lastStepOppositeSide, feet.get(lastStepOppositeSide), soleFrames.get(lastStepOppositeSide)), transitionStanceFeet); } generator.setPriorFootposes(transitionStanceFeet); } generator.initialize(); }
private void initializeStanceFeetOfSecondSegment(ArrayList<Footstep> footstepsToControlRing) { int numFootsteps = footstepsToControlRing.size(); RobotSide lastStepSide = RobotSide.RIGHT; if (numFootsteps >= 1) { Footstep lastFootstep = footstepsToControlRing.get(numFootsteps - 1); lastStepSide = lastFootstep.getRobotSide(); setMidstep(lastFootstep); } else { System.err.println("Why are there no feet generated for the first segment of TwoSegmentFootstepGenerator?"); setMidstep(FootstepUtils.generateStandingFootstep(lastStepSide, feet.get(lastStepSide), soleFrames.get(lastStepSide))); } if (numFootsteps >= 2) { setMidstep(footstepsToControlRing.get(numFootsteps - 2)); } else { RobotSide lastStepOppositeSide = lastStepSide.getOppositeSide(); setMidstep(FootstepUtils.generateStandingFootstep(lastStepOppositeSide, feet.get(lastStepOppositeSide), soleFrames.get(lastStepOppositeSide))); } if (!secondSegmentManuallySpecifiesStart) footstepGenerator2.setStanceStartPreference(lastStepSide); footstepGenerator2.setPriorFootposes(midStanceFeet); footstepGenerator2.initialize(); }
private void initializeStanceFeetOfSecondSegment(ArrayList<Footstep> footstepsToControlRing) { int numFootsteps = footstepsToControlRing.size(); RobotSide lastStepSide = RobotSide.RIGHT; if (numFootsteps >= 1) { Footstep lastFootstep = footstepsToControlRing.get(numFootsteps - 1); lastStepSide = lastFootstep.getRobotSide(); setMidstep(lastFootstep); } else { System.err.println("Why are there no feet generated for the first segment of TwoSegmentFootstepGenerator?"); setMidstep(FootstepUtils.generateStandingFootstep(lastStepSide, feet.get(lastStepSide), soleFrames.get(lastStepSide))); } if (numFootsteps >= 2) { setMidstep(footstepsToControlRing.get(numFootsteps - 2)); } else { RobotSide lastStepOppositeSide = lastStepSide.getOppositeSide(); setMidstep(FootstepUtils.generateStandingFootstep(lastStepOppositeSide, feet.get(lastStepOppositeSide), soleFrames.get(lastStepOppositeSide))); } if (!secondSegmentManuallySpecifiesStart) footstepGenerator2.setStanceStartPreference(lastStepSide); footstepGenerator2.setPriorFootposes(midStanceFeet); footstepGenerator2.initialize(); }