public static FramePoint getCenterOfFootstep(Footstep footstep) { return getCenterOfFootstep(footstep, RobotSide.LEFT, 0.0, 0.0); }
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(); }
protected RobotSide determineClosestFootSideToEnd(SideDependentList<Footstep> lastFootsteps) { FramePose2D endPose = getPath().getPoseAtS(1); Point3D endPoint = new Point3D(endPose.getX(), endPose.getY(), 0.0); RobotSide closestSideToEnd = FootstepUtils.getFrontFootRobotSideFromFootsteps(lastFootsteps, new FramePoint3D(ReferenceFrame.getWorldFrame(), endPoint)); return closestSideToEnd; }
List<FramePoint> expectedContactPoints = FootstepUtils.calculateExpectedContactPoints(footstep, contactablePlaneBody); ArrayList<Point2d> contactPointsInSoleFrame = new ArrayList<Point2d>();
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(); }
protected RobotSide determineClosestFootSideToEnd(SideDependentList<Footstep> lastFootsteps) { FramePose2d endPose = getPath().getPoseAtS(1); Point3d endPoint = new Point3d(endPose.getX(), endPose.getY(), 0.0); RobotSide closestSideToEnd = FootstepUtils.getFrontFootRobotSideFromFootsteps(lastFootsteps, new FramePoint(ReferenceFrame.getWorldFrame(), endPoint)); return closestSideToEnd; }
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(); }
public static FramePoint3D getCenterOfFootstep(Footstep footstep) { return getCenterOfFootstep(footstep, RobotSide.LEFT, 0.0, 0.0); }
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(); }