Draw log data to SVG for papers

ihmc-avatar-interfaces/build.gradle.kts

@@ -17,6 +17,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.27.3")

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

@@ -0,0 +1,45 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.yoVariables.variable.YoEnum;
+
+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 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,77 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType;
+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;
+
+public class SCS2LogFootState
+{
+   private final RobotSide side;
+   private final SCS2LogEnum<ConstraintType> yoFootState;
+   private boolean newStep = false;
+   private double fullSupportTime = Double.NaN;
+   private YoVariable footPolygon_0_x ;
+   private YoVariable footPolygon_0_y ;
+   private YoVariable footPolygon_1_x ;
+   private YoVariable footPolygon_1_y ;
+   private YoVariable footPolygon_2_x ;
+   private YoVariable footPolygon_2_y ;
+   private YoVariable footPolygon_3_x ;
+   private YoVariable footPolygon_3_y ;
+   private final ArrayList<SCS2LogFootstep> footsteps = new ArrayList<>();
+   private final double comPlotProximityToFootsteps = 5.0;
+
+   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 boolean afterRead(double currentTime)
+   {
+      if (yoFootState.changedTo(ConstraintType.FULL))
+      {
+         newStep = true;
+         fullSupportTime = currentTime;
+      }
+      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;
+      }
+
+      // recent footstep
+      return !Double.isNaN(fullSupportTime) && currentTime - fullSupportTime < comPlotProximityToFootsteps;
+   }
+
+   public ArrayList<SCS2LogFootstep> getFootsteps()
+   {
+      return footsteps;
+   }
+}

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

@@ -0,0 +1,32 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.robotics.robotSide.RobotSide;
+
+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,18 @@
+package us.ihmc.avatar.logProcessor;
+
+import us.ihmc.yoVariables.variable.YoDouble;
+
+public class SCS2LogJointTracker
+{
+   private final YoDouble jointPosition;
+
+   public SCS2LogJointTracker(YoDouble jointPosition)
+   {
+      this.jointPosition = jointPosition;
+   }
+
+   public void update()
+   {
+
+   }
+}