Skip to content

Commit

Permalink
Plot double support durations.
Browse files Browse the repository at this point in the history
  • Loading branch information
@calvertdw
calvertdw committed Jan 23, 2025
1 parent 99f69cc commit 39c83a2
Show file tree
Hide file tree
Showing 4 changed files with 90 additions and 7 deletions.
20 changes: 19 additions & 1 deletion ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -50,6 +51,8 @@ public class SCS2LogLocomotionData
private final SideDependentList<ReferenceFrame> footFrames = new SideDependentList<>();
private boolean requestStopProcessing = false;
private Robot robot;
private final SideDependentList<Boolean> isInSupport = new SideDependentList<>(true, true);
private double timeInDoubleSupport = Double.NaN;

public void setup(LogSession logSession)
{
Expand Down Expand Up @@ -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();

Expand All @@ -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())
Expand Down
13 changes: 13 additions & 0 deletions ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogProcessor.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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())
{
Expand Down
7 changes: 7 additions & 0 deletions ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogWalk.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ public record FootStateChange(double time, ConstraintType state) { }
private final SideDependentList<ArrayList<FootStateChange>> footStateChanges = new SideDependentList<>(new ArrayList<>(), new ArrayList<>());
public record FootSwing(double completeTime, double swingDuration) { }
private final SideDependentList<ArrayList<FootSwing>> footSwings = new SideDependentList<>(new ArrayList<>(), new ArrayList<>());
public record DoubleSupportDuration(double completeTime, double supportDuration) { }
private final ArrayList<DoubleSupportDuration> doubleSupportDurations = new ArrayList<>();
private final TDoubleArrayList times = new TDoubleArrayList();
private final RecyclingArrayList<Pose3D> pelvisPoses = new RecyclingArrayList<>(Pose3D::new);
private final SideDependentList<RecyclingArrayList<Pose3D>> handPoses = new SideDependentList<>(new RecyclingArrayList<>(Pose3D::new),
Expand Down Expand Up @@ -68,6 +70,11 @@ public SideDependentList<ArrayList<FootSwing>> getFootSwings()
return footSwings;
}

public ArrayList<DoubleSupportDuration> getDoubleSupportDurations()
{
return doubleSupportDurations;
}

public boolean isEndedWithFall()
{
return endedWithFall;
Expand Down
57 changes: 51 additions & 6 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXSCS2LogProcessor.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<Double> times = new ArrayList<>();
ArrayList<Double> 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");
}
}

Expand Down

0 comments on commit 39c83a2

Please sign in to comment.