Draw log data to SVG for papers

ihmc-avatar-interfaces/build.gradle.kts

@@ -14,6 +14,8 @@ mainDependencies {
    api("com.martiansoftware:jsap:2.1")
    api("org.apache.poi:poi:3.15") // I/O library for xls files.
    api("com.hierynomus:sshj:0.32.0")
+   api("org.jfree:org.jfree.svg:5.0.6")
+   api("com.github.sh0nk:matplotlib4j:0.5.0")
 
    api("us.ihmc:mecano-graphviz:17-0.18.1")
    api("us.ihmc:scs2-bullet-simulation:17-0.28.3")

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/LogCSVProcessor.java

@@ -0,0 +1,66 @@
+package us.ihmc.avatar.logProcessor;
+
+import gnu.trove.list.array.TDoubleArrayList;
+import us.ihmc.log.LogTools;
+
+import java.io.IOException;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.List;
+
+/**
+ * Used as a scratchpad to calculate RMS error data for papers.
+ */
+public class LogCSVProcessor
+{
+   private final String LOG_PATH = Paths.get(System.getProperty("log.path")).toString();
+
+   public LogCSVProcessor()
+   {
+      Path filePath = Paths.get(LOG_PATH);
+
+      List<String> fileContents;
+      try
+      {
+         fileContents = Files.readAllLines(filePath, StandardCharsets.UTF_8);
+      }
+      catch (IOException e)
+      {
+         throw new RuntimeException();
+      }
+
+      fileContents.remove(0); // Remove header
+
+      TDoubleArrayList controllerICPErrorXs = new TDoubleArrayList();
+      TDoubleArrayList controllerICPErrorYs = new TDoubleArrayList();
+
+      for (String line : fileContents)
+      {
+         String[] splitLine = line.split(",");
+         controllerICPErrorXs.add(Double.parseDouble(splitLine[0]));
+         controllerICPErrorYs.add(Double.parseDouble(splitLine[1]));
+      }
+
+      LogTools.info("RMS controllerICPErrorXs: {}", calculateRMS(controllerICPErrorXs));
+      LogTools.info("RMS controllerICPErrorYs: {}", calculateRMS(controllerICPErrorYs));
+   }
+
+   public double calculateRMS(TDoubleArrayList values)
+   {
+      double sum = 0.0;
+      for (int i = 0; i < values.size(); i++)
+      {
+         double value = values.get(i);
+         sum += value * value;
+      }
+      double averageSquare = sum / values.size();
+      return Math.sqrt(averageSquare);
+   }
+
+   public static void main(String[] args)
+   {
+      new LogCSVProcessor();
+   }
+}

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogEnum.java

@@ -0,0 +1,51 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.yoVariables.variable.YoEnum;
+
+/** Used to process YoEnum changes for data post-processing. */
+public class SCS2LogEnum<E extends Enum<E>>
+{
+   private final YoEnum<?> yoEnum;
+   private final Class<E> enumType;
+   private E lastValue = null;
+
+   public SCS2LogEnum(YoEnum<?> yoEnum, Class<E> enumType)
+   {
+      this.yoEnum = yoEnum;
+      this.enumType = enumType;
+   }
+
+   public E getValue()
+   {
+      for (E enumConstant : enumType.getEnumConstants())
+      {
+         if (yoEnum.getStringValue().equals(enumConstant.name()))
+         {
+            return enumConstant;
+         }
+      }
+
+      return null;
+   }
+
+   public boolean changed()
+   {
+      return lastValue != getValue();
+   }
+
+   public boolean changedFrom(E fromValue)
+   {
+      return lastValue == fromValue && getValue() != fromValue;
+   }
+
+   public boolean changedTo(E toValue)
+   {
+      return getValue() == toValue && lastValue != toValue;
+   }
+
+   public void postUpdate()
+   {
+      E currentValue = getValue();
+      lastValue = currentValue;
+   }
+}

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogFootState.java

@@ -0,0 +1,107 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType;
+import us.ihmc.commons.thread.TypedNotification;
+import us.ihmc.euclid.tuple2D.Point2D;
+import us.ihmc.log.LogTools;
+import us.ihmc.robotics.robotSide.RobotSide;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoVariable;
+
+import java.util.ArrayList;
+
+/** Data holder for YoVariable information relating to foot state for data post-processing. */
+public class SCS2LogFootState
+{
+   private final RobotSide side;
+   private final SCS2LogEnum<ConstraintType> yoFootState;
+   private boolean newStep = false;
+   private double fullSupportTime = Double.NaN;
+   private final YoVariable footPolygon_0_x;
+   private final YoVariable footPolygon_0_y;
+   private final YoVariable footPolygon_1_x;
+   private final YoVariable footPolygon_1_y;
+   private final YoVariable footPolygon_2_x;
+   private final YoVariable footPolygon_2_y;
+   private final YoVariable footPolygon_3_x;
+   private final YoVariable footPolygon_3_y;
+   private final ArrayList<SCS2LogFootstep> footsteps = new ArrayList<>();
+   private final TypedNotification<ConstraintType> stateChanged = new TypedNotification<>();
+   private final double comPlotProximityToFootsteps = 5.0;
+   private double timeStartedSwing = Double.NaN;
+   private final TypedNotification<Double> swingCompleted = new TypedNotification<>();
+
+   public SCS2LogFootState(RobotSide side, SCS2LogEnum<ConstraintType> yoFootState, YoRegistry rootRegistry)
+   {
+      this.side = side;
+      this.yoFootState = yoFootState;
+
+      String highLevelController = "root.main.DRCControllerThread.DRCMomentumBasedController.HumanoidHighLevelControllerManager.";
+      String footPolygonPrefix = highLevelController + "HighLevelHumanoidControllerToolbox.BipedSupportPolygons.";
+      footPolygon_0_x  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_0_x".formatted(side.getLowerCaseName()));
+      footPolygon_0_y  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_0_y".formatted(side.getLowerCaseName()));
+      footPolygon_1_x  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_1_x".formatted(side.getLowerCaseName()));
+      footPolygon_1_y  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_1_y".formatted(side.getLowerCaseName()));
+      footPolygon_2_x  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_2_x".formatted(side.getLowerCaseName()));
+      footPolygon_2_y  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_2_y".formatted(side.getLowerCaseName()));
+      footPolygon_3_x  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_3_x".formatted(side.getLowerCaseName()));
+      footPolygon_3_y  = rootRegistry.findVariable(footPolygonPrefix + "%sFootPolygon_3_y".formatted(side.getLowerCaseName()));
+   }
+
+   public void afterRead(double currentTime)
+   {
+      if (yoFootState.changed())
+      {
+         stateChanged.set(yoFootState.getValue());
+      }
+      if (yoFootState.changedTo(ConstraintType.FULL))
+      {
+         newStep = true;
+         fullSupportTime = currentTime;
+      }
+      if (yoFootState.changedTo(ConstraintType.SWING))
+      {
+         timeStartedSwing = currentTime;
+      }
+      if (yoFootState.changedFrom(ConstraintType.SWING))
+      {
+         double swingDuration = currentTime - timeStartedSwing;
+         swingCompleted.set(swingDuration);
+      }
+      yoFootState.postUpdate();
+
+      if (newStep && currentTime - fullSupportTime > 0.1)
+      {
+         LogTools.info("%s step at %s".formatted(side.getPascalCaseName(), new Point2D(footPolygon_0_x.getValueAsDouble(), footPolygon_0_y.getValueAsDouble())));
+         footsteps.add(new SCS2LogFootstep(currentTime, side, new double[] {footPolygon_0_x.getValueAsDouble(),
+                                                                            footPolygon_1_x.getValueAsDouble(),
+                                                                            footPolygon_2_x.getValueAsDouble(),
+                                                                            footPolygon_3_x.getValueAsDouble(),
+                                                                            footPolygon_0_y.getValueAsDouble(),
+                                                                            footPolygon_1_y.getValueAsDouble(),
+                                                                            footPolygon_2_y.getValueAsDouble(),
+                                                                            footPolygon_3_y.getValueAsDouble()}));
+         newStep = false;
+      }
+   }
+
+   public RobotSide getSide()
+   {
+      return side;
+   }
+
+   public ArrayList<SCS2LogFootstep> getFootsteps()
+   {
+      return footsteps;
+   }
+
+   public TypedNotification<ConstraintType> getStateChanged()
+   {
+      return stateChanged;
+   }
+
+   public TypedNotification<Double> getSwingCompleted()
+   {
+      return swingCompleted;
+   }
+}

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogFootstep.java

@@ -0,0 +1,33 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.robotics.robotSide.RobotSide;
+
+/** Holder for foostep data for post-processing. */
+public class SCS2LogFootstep
+{
+   private final double time;
+   private final RobotSide side;
+   private final double[] polygon;
+
+   public SCS2LogFootstep(double time, RobotSide side, double[] polygon)
+   {
+      this.time = time;
+      this.side = side;
+      this.polygon = polygon.clone();
+   }
+
+   public double getTime()
+   {
+      return time;
+   }
+
+   public RobotSide getSide()
+   {
+      return side;
+   }
+
+   public double[] getPolygon()
+   {
+      return polygon;
+   }
+}

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogJointTracker.java

@@ -0,0 +1,19 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.yoVariables.variable.YoDouble;
+
+/** Work in progress, will likely be needed for future manipulation publications. For log data post-processing. */
+public class SCS2LogJointTracker
+{
+   private final YoDouble jointPosition;
+
+   public SCS2LogJointTracker(YoDouble jointPosition)
+   {
+      this.jointPosition = jointPosition;
+   }
+
+   public void update()
+   {
+
+   }
+}