private void assertMaxDisplacementStanceToSwing(String message, ArrayList<Footstep> footsteps, double distTest) { if (distTest == Double.POSITIVE_INFINITY) return; Footstep footstep1; Footstep footstep2; for (int i = 0; i < footsteps.size() - 1; i++) { footstep1 = footsteps.get(i); footstep2 = footsteps.get(i + 1); FramePoint3D position2 = new FramePoint3D(); footstep2.getPosition(position2); FramePoint3D position1 = new FramePoint3D(); footstep1.getPosition(position1); double dis = position2.distance(position1); assertTrue(String.format(message + " %.2f<=%.2f? step %d to %d", dis, distTest, i, i + 1), dis < distTest + 1e-14); } }
private void assertMaxDisplacementSwingToSwing(String message, ArrayList<Footstep> footsteps, double distTest) { if (distTest == Double.POSITIVE_INFINITY) return; Footstep footstep1; Footstep footstep2; for (int i = 0; i < footsteps.size() - 2; i++) { footstep1 = footsteps.get(i); footstep2 = footsteps.get(i + 2); FramePoint3D position2 = new FramePoint3D(); footstep2.getPosition(position2); FramePoint3D position1 = new FramePoint3D(); footstep1.getPosition(position1); double dis = position2.distance(position1); // assertTrue(true); assertTrue(String.format(message + " %.2f<=%.2f? step %d to %d", dis, distTest, i, i + 2), dis < distTest + 1e-14); } }
public double getSwingHeight(Footstep initialFootstep, Footstep stanceFootstep, Footstep endFootstep, HeightMapWithPoints heightMap) { FramePose startPose = new FramePose(); initialFootstep.getSolePose(startPose); FramePose endPose = new FramePose(); endFootstep.getSolePose(endPose); Point3d position = new Point3d(); stanceFootstep.getPosition(position); return getSwingHeight(startPose, endPose, position.getZ(), heightMap); }
public static RobotSide getFrontFootRobotSideFromFootsteps(SideDependentList<Footstep> footSteps, FramePoint3D destination) { FramePoint3D destinationInWorld = new FramePoint3D(destination); destinationInWorld.changeFrame(worldFrame); SideDependentList<Double> distances = new SideDependentList<Double>(); for (RobotSide side : RobotSide.values) { FramePoint3D footstepPosition = new FramePoint3D(); footSteps.get(side).getPosition(footstepPosition); footstepPosition.changeFrame(worldFrame); distances.set(side, new Double(footstepPosition.distanceSquared(destinationInWorld))); } RobotSide frontSide = (distances.get(RobotSide.LEFT) <= distances.get(RobotSide.RIGHT)) ? RobotSide.LEFT : RobotSide.RIGHT; return frontSide; }
transferFromFootstep.getPosition(transferFromFootFramePoint); FrameQuaternion transferFromFootOrientation = new FrameQuaternion(); transferFromFootstep.getOrientation(transferFromFootOrientation); transferToFootstep.getPosition(transferToFootFramePoint); FrameQuaternion transferToFootOrientation = new FrameQuaternion(); transferToFootstep.getOrientation(transferToFootOrientation);