diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogDataProcessor.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogDataProcessor.java index 5b7336a4e012..232da5b1f908 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogDataProcessor.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogDataProcessor.java @@ -2,25 +2,25 @@ import org.jfree.svg.SVGGraphics2D; import org.jfree.svg.SVGHints; -import org.jfree.svg.SVGHints.Key; import org.jfree.svg.SVGUnits; -import org.jfree.svg.SVGUtils; +import us.ihmc.commonWalkingControlModules.desiredFootStep.FootstepListVisualizer; import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.commons.thread.ThreadTools; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.log.LogTools; -import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.scs2.session.log.LogSession; import us.ihmc.tools.io.JSONFileTools; import us.ihmc.tools.thread.MissingThreadTools; -import java.awt.*; +import java.awt.Color; +import java.awt.BasicStroke; +import java.awt.Font; import java.io.FileWriter; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; import java.nio.file.Paths; -import java.util.UUID; +import java.util.List; import java.util.function.Consumer; public class SCS2LogDataProcessor @@ -183,7 +183,7 @@ private void drawSVG() svgGraphics2D = new SVGGraphics2D(documentSizeMillimeters, documentSizeMillimeters, SVGUnits.MM); svgGraphics2D.setColor(Color.BLACK); - svgGraphics2D.setStroke(new BasicStroke(7)); + svgGraphics2D.setStroke(new BasicStroke(5)); svgGraphics2D.setFont(new Font("Arial", Font.PLAIN, 20)); svgGraphics2D.setFontSizeUnits(SVGUnits.MM); @@ -199,7 +199,8 @@ private void drawSVG() for (SCS2LogDataFootstep footstep : logWalk.getFootsteps()) { - svgGraphics2D.setColor(footstep.getSide() == RobotSide.LEFT ? Color.RED : Color.GREEN); + Color color = FootstepListVisualizer.defaultFeetColors.get(footstep.getSide()); + svgGraphics2D.setColor(color); double[] polygon = footstep.getPolygon(); LogTools.info("Drawing step at {} {}", new Point2D(polygon[0], polygon[4]), new Point2D(metersToMMX(polygon[0]), metersToMMY(polygon[4]))); @@ -214,10 +215,17 @@ private void drawSVG() 4); } - svgGraphics2D.setColor(Color.BLACK); - svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, groupName); + svgGraphics2D.setStroke(new BasicStroke(2.5f)); + svgGraphics2D.setColor(Color.BLACK); + plot(logWalk.getComs(), "Coms"); + svgGraphics2D.setColor(Color.BLUE); + + svgGraphics2D.setStroke(new BasicStroke(2.5f, BasicStroke.CAP_BUTT, BasicStroke.JOIN_MITER, 10.0f, new float[] {10.0f}, 0.0f)); + plot(logWalk.getIcps(), "ICPs"); + + svgGraphics2D.setColor(Color.BLACK); if (logWalk.isEndedWithFall()) { Point2D fallLocation; @@ -234,27 +242,11 @@ private void drawSVG() svgGraphics2D.drawString("Fall %d".formatted(walk), metersToMMX(fallLocation.getX()), metersToMMY(fallLocation.getY() + 0.3)); } - svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, walkName); ++walk; } - RecyclingArrayList coms = locomotionData.getComs(); - if (!coms.isEmpty()) - { - int[] comXs = new int[coms.size()]; - int[] comYs = new int[coms.size()]; - for (int i = 0; i < coms.size(); i++) - { - comXs[i] = metersToMMX(coms.get(i).getX()); - comYs[i] = metersToMMY(coms.get(i).getY()); - } - svgGraphics2D.setColor(Color.BLUE); - svgGraphics2D.setRenderingHint(SVGHints.KEY_BEGIN_GROUP, "Coms"); - svgGraphics2D.drawPolyline(comXs, comYs, comXs.length); - svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, "Coms"); - } LogTools.info("Saving SVG to {}", svgPath); @@ -271,6 +263,23 @@ private void drawSVG() } } + private void plot(List points, String name) + { + if (!points.isEmpty()) + { + int[] xs = new int[points.size()]; + int[] ys = new int[points.size()]; + for (int i = 0; i < points.size(); i++) + { + xs[i] = metersToMMX(points.get(i).getX()); + ys[i] = metersToMMY(points.get(i).getY()); + } + svgGraphics2D.setRenderingHint(SVGHints.KEY_BEGIN_GROUP, name); + svgGraphics2D.drawPolyline(xs, ys, xs.length); + svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, name); + } + } + private int metersToMMX(double x) { double fromStart = x - locomotionData.getRobotStartLocation().getX(); @@ -337,7 +346,7 @@ public int getNumberOfFootstepsStat() public int getNumberOfComsStat() { - return locomotionData == null ? numberOfComsStat : locomotionData.getComs().size(); + return locomotionData == null ? numberOfComsStat : locomotionData.getNumberOfComs(); } public int getWorkingCounterMismatchStat() diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java index 8d760de6be61..13395c8587cf 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java @@ -2,13 +2,13 @@ import com.fasterxml.jackson.databind.node.ObjectNode; import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType; -import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.scs2.session.log.LogSession; +import us.ihmc.yoVariables.euclid.YoPoint2D; import us.ihmc.yoVariables.euclid.YoPoint3D; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; @@ -26,14 +26,14 @@ public class SCS2LogLocomotionData private YoInteger workingCounterMismatch; private YoBoolean isRobotFalling; private SCS2LogDataEnum controllerState; - private final Point2D robotStartLocation = new Point2D(); + private final Point2D robotStartLocation = new Point2D(Double.NaN, Double.NaN); private final SideDependentList footStates = new SideDependentList<>(); private final ArrayList logWalks = new ArrayList<>(); private final Point2D lastCenterOfMass = new Point2D(Double.NaN, Double.NaN); private YoPoint3D centerOfMass; - private final double comPlotResolution = 0.1; + private YoPoint2D capturePoint; + private final double plotTimeResolution = 0.1; private double lastCoMPlotTime = Double.NaN; - private final RecyclingArrayList coms = new RecyclingArrayList<>(Point2D::new); private boolean requestStopProcessing = false; public void setup(LogSession logSession) @@ -58,6 +58,10 @@ public void setup(LogSession logSession) && rootRegistry.findVariable(momentumRateControl + "centerOfMassY") instanceof YoDouble yVariable && rootRegistry.findVariable(momentumRateControl + "centerOfMassX") instanceof YoDouble zVariable) centerOfMass = new YoPoint3D(xVariable, yVariable, zVariable); + + if (rootRegistry.findVariable(momentumRateControl + "capturePointX") instanceof YoDouble xVariable + && rootRegistry.findVariable(momentumRateControl + "capturePointY") instanceof YoDouble yVariable) + capturePoint = new YoPoint2D(xVariable, yVariable); String feetManager = highLevelController + "HighLevelHumanoidControllerFactory.HighLevelControlManagerFactory.FeetManager."; for (RobotSide side : RobotSide.values) @@ -106,36 +110,31 @@ private void afterRead(double currentTime) footStates.get(side).getFootsteps().clear(); } - if (recentSteps) + if (robotStartLocation.containsNaN()) { - if (Double.isNaN(lastCoMPlotTime) || currentTime - lastCoMPlotTime > comPlotResolution) - { - if (coms.isEmpty()) - { - robotStartLocation.set(centerOfMass.getX(), centerOfMass.getY()); - LogTools.info("Robot start location: {}", robotStartLocation); - } - - coms.add().set(centerOfMass); - - lastCenterOfMass.set(centerOfMass); - lastCoMPlotTime = currentTime; - } + robotStartLocation.set(centerOfMass.getX(), centerOfMass.getY()); + LogTools.info("Robot start location: {}", robotStartLocation); } - + if (lastCenterOfMass.containsNaN()) + { + logWalk.getComs().add().set(centerOfMass); + lastCenterOfMass.set(centerOfMass); + lastCoMPlotTime = currentTime; + } + else if (centerOfMass.distanceXY(lastCenterOfMass) > 0.001 && currentTime - lastCoMPlotTime > plotTimeResolution) + { + logWalk.getComs().add().set(centerOfMass); + logWalk.getIcps().add().set(capturePoint); + lastCenterOfMass.set(centerOfMass); + lastCoMPlotTime = currentTime; + } } - // TODO: - // # Falls // # Runs of action (split by 30 seconds of inactivity) // Timestamps where runs start // Arm motions - - - - } public void writeJSON(ObjectNode rootNode) @@ -143,7 +142,7 @@ public void writeJSON(ObjectNode rootNode) rootNode.put("numberOfWalks", logWalks.size()); rootNode.put("numberOfFalls", getFalls()); rootNode.put("numberOfFootsteps", getNumberOfFootsteps()); - rootNode.put("numberOfComs", coms.size()); + rootNode.put("numberOfComs", getNumberOfComs()); rootNode.put("workingCounterMismatch", getWorkingCounterMismatch()); } @@ -172,9 +171,14 @@ public int getNumberOfFootsteps() return numberOfFootsteps; } - public RecyclingArrayList getComs() + public int getNumberOfComs() { - return coms; + int numberOfComs = 0; + for (SCS2LogWalk logWalk : logWalks) + { + numberOfComs += logWalk.getComs().size(); + } + return numberOfComs; } public ArrayList getLogWalks() diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogWalk.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogWalk.java index 888505f8bff5..719e43ec6347 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogWalk.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogWalk.java @@ -12,6 +12,7 @@ public class SCS2LogWalk private final Point2D walkStart = new Point2D(); private final ArrayList footsteps = new ArrayList<>(); private final RecyclingArrayList coms = new RecyclingArrayList<>(Point2D::new); + private final RecyclingArrayList icps = new RecyclingArrayList<>(Point2D::new); private boolean endedWithFall = false; private int initialWorkingCounterMismatch = -1; @@ -60,6 +61,11 @@ public RecyclingArrayList getComs() return coms; } + public RecyclingArrayList getIcps() + { + return icps; + } + public int getInitialWorkingCounterMismatch() { return initialWorkingCounterMismatch;