From 39c83a276439628bd212265eaa1ba49dc0d1f113 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 22 Jan 2025 22:23:51 -0600 Subject: [PATCH] Plot double support durations. --- .../logProcessor/SCS2LogLocomotionData.java | 20 ++++++- .../avatar/logProcessor/SCS2LogProcessor.java | 13 +++++ .../ihmc/avatar/logProcessor/SCS2LogWalk.java | 7 +++ .../ihmc/rdx/logging/RDXSCS2LogProcessor.java | 57 +++++++++++++++++-- 4 files changed, 90 insertions(+), 7 deletions(-) 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 34f11b8fefe..a5deb66c401 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 @@ -1,6 +1,7 @@ package us.ihmc.avatar.logProcessor; import com.fasterxml.jackson.databind.node.ObjectNode; +import us.ihmc.avatar.logProcessor.SCS2LogWalk.DoubleSupportDuration; import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootStateChange; import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootSwing; import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType; @@ -50,6 +51,8 @@ public class SCS2LogLocomotionData private final SideDependentList footFrames = new SideDependentList<>(); private boolean requestStopProcessing = false; private Robot robot; + private final SideDependentList isInSupport = new SideDependentList<>(true, true); + private double timeInDoubleSupport = Double.NaN; public void setup(LogSession logSession) { @@ -163,6 +166,7 @@ private void afterRead(double currentTime) for (SCS2LogFootState footState : footStates.values()) { footState.afterRead(currentTime); + ConstraintType changedTo = footState.getStateChanged().peek(); logWalk.getFootsteps().addAll(footState.getFootsteps()); footState.getFootsteps().clear(); @@ -172,7 +176,21 @@ private void afterRead(double currentTime) if (footState.getSwingCompleted().poll()) logWalk.getFootSwings().get(footState.getSide()).add(new FootSwing(currentTime, footState.getSwingCompleted().read())); - // TODO: transfer duration -> BOTH (FULL | TOES) -> + if (changedTo != null) + { + isInSupport.put(footState.getSide(), changedTo == ConstraintType.FULL || changedTo == ConstraintType.TOES); + + if (isInSupport.get(RobotSide.LEFT) && isInSupport.get(RobotSide.RIGHT)) + { + timeInDoubleSupport = currentTime; + } + else + { + double duration = currentTime - timeInDoubleSupport; + logWalk.getDoubleSupportDurations().add(new DoubleSupportDuration(currentTime, duration)); + timeInDoubleSupport = Double.NaN; + } + } } if (robotStartLocation.containsNaN()) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogProcessor.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogProcessor.java index c32ba5fe757..b3580bf3602 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogProcessor.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogProcessor.java @@ -1,5 +1,6 @@ package us.ihmc.avatar.logProcessor; +import us.ihmc.avatar.logProcessor.SCS2LogWalk.DoubleSupportDuration; import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootStateChange; import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootSwing; import us.ihmc.commons.thread.ThreadTools; @@ -267,6 +268,18 @@ else if (leftPushBarDisturbed) } } }); + + writeCSV(logFolderName, "DoubleSupportDurations", "Time,Duration", writer -> + { + for (SCS2LogWalk logWalk : locomotionData.getLogWalks()) + { + for (DoubleSupportDuration doubleSupportDuration : logWalk.getDoubleSupportDurations()) + { + writer.write("%.2f,%.2f".formatted(doubleSupportDuration.completeTime(), doubleSupportDuration.supportDuration())); + writer.newLine(); + } + } + }); for (RobotSide side : locomotionData.getHandFrames().sides()) { 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 cb792a93a6d..ea6e0bb4e8b 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 @@ -19,6 +19,8 @@ public record FootStateChange(double time, ConstraintType state) { } private final SideDependentList> footStateChanges = new SideDependentList<>(new ArrayList<>(), new ArrayList<>()); public record FootSwing(double completeTime, double swingDuration) { } private final SideDependentList> footSwings = new SideDependentList<>(new ArrayList<>(), new ArrayList<>()); + public record DoubleSupportDuration(double completeTime, double supportDuration) { } + private final ArrayList doubleSupportDurations = new ArrayList<>(); private final TDoubleArrayList times = new TDoubleArrayList(); private final RecyclingArrayList pelvisPoses = new RecyclingArrayList<>(Pose3D::new); private final SideDependentList> handPoses = new SideDependentList<>(new RecyclingArrayList<>(Pose3D::new), @@ -68,6 +70,11 @@ public SideDependentList> getFootSwings() return footSwings; } + public ArrayList getDoubleSupportDurations() + { + return doubleSupportDurations; + } + public boolean isEndedWithFall() { return endedWithFall; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXSCS2LogProcessor.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXSCS2LogProcessor.java index 9837633e081..d212caeeee8 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXSCS2LogProcessor.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXSCS2LogProcessor.java @@ -182,7 +182,7 @@ public void create() tableFlags += ImGuiTableFlags.BordersV; tableFlags += ImGuiTableFlags.NoBordersInBody; - if (ImGui.beginTable(labels.get("Logs"), 12, tableFlags)) + if (ImGui.beginTable(labels.get("Logs"), 11, tableFlags)) { float charWidth = ImGuiTools.calcTextSizeX("A"); ImGui.tableSetupColumn(labels.get("Name"), ImGuiTableColumnFlags.WidthFixed, 50 * charWidth); @@ -196,7 +196,6 @@ public void create() ImGui.tableSetupColumn(labels.get("Plot hand poses"), ImGuiTableColumnFlags.WidthFixed, 12 * charWidth); ImGui.tableSetupColumn(labels.get("Plot ICP error"), ImGuiTableColumnFlags.WidthFixed, 12 * charWidth); ImGui.tableSetupColumn(labels.get("Plot step timings"), ImGuiTableColumnFlags.WidthFixed, 12 * charWidth); - ImGui.tableSetupColumn(labels.get("Plot swing durations"), ImGuiTableColumnFlags.WidthFixed, 12 * charWidth); ImGui.tableSetupScrollFreeze(0, 1); ImGui.tableHeadersRow(); @@ -437,10 +436,6 @@ public void create() } } }, "Plot Step Timings"); - } - ImGui.tableNextColumn(); - if (ImGuiTools.textWithUnderlineOnHover("Plot swing durations") && ImGui.isMouseClicked(ImGuiMouseButton.Left)) - { ThreadTools.startAsDaemon(() -> { String logFolderName = logDirectory.getFileName().toString(); @@ -491,6 +486,56 @@ public void create() } } }, "Plot Swing Durations"); + ThreadTools.startAsDaemon(() -> + { + String logFolderName = logDirectory.getFileName().toString(); + Path csvFile = logDirectory.resolve(logFolderName + "_DoubleSupportDurations.csv"); + + if (Files.exists(csvFile)) + { + try + { + Plot pyplot = Plot.create(); + + ArrayList times = new ArrayList<>(); + ArrayList durations = new ArrayList<>(); + try (BufferedReader reader = Files.newBufferedReader(csvFile)) + { + reader.readLine(); // skip header + + String line; + boolean processingLeft = true; + while ((line = reader.readLine()) != null) + { + String[] values = line.split(","); + + times.add(Double.parseDouble(values[0])); + durations.add(Double.parseDouble(values[1])); + } + + } + catch (IOException e) + { + throw new RuntimeException("Error reading CSV file.", e); + } + + // Convert Pascal case to title case + String afterUnderscore = logFolderName.substring(logFolderName.lastIndexOf("_") + 1); + String titleCaseString = WordUtils.capitalizeFully(afterUnderscore.replaceAll("([a-z])([A-Z])", "$1 $2")); + + pyplot.plot().add(times, durations).label("double support durations"); + pyplot.ylim(0.0, durations.stream().max(Double::compare).orElse(0.0) + 0.2); + pyplot.xlabel("Time (s)"); + pyplot.title("%s Double Support Durations".formatted(titleCaseString)); + pyplot.legend(); + pyplot.show(); + } + catch (IOException | PythonExecutionException e) + { + throw new RuntimeException(e); + } + } + }, "Plot Double Support Durations"); } }