Skip to content

Commit

Permalink
Plot ICP error.
Browse files Browse the repository at this point in the history
  • Loading branch information
@calvertdw
calvertdw committed Jan 24, 2025
1 parent d357755 commit 04e7308
Show file tree
Hide file tree
Showing 4 changed files with 81 additions and 5 deletions.
4 changes: 4 additions & 0 deletions ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogLocomotionData.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,11 @@
import us.ihmc.avatar.logProcessor.SCS2LogWalk.DoubleSupportDuration;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootStateChange;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootSwing;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.ICPErrorEntry;
import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.log.LogTools;
import us.ihmc.mecano.frames.MovingReferenceFrame;
Expand Down Expand Up @@ -177,6 +179,8 @@ private void afterRead(double currentTime)
SCS2LogWalk logWalk = getCurrentLogWalk();
logWalk.update(currentTime, tick, workingCounterMismatch);

logWalk.getICPErrors().add(new ICPErrorEntry(currentTime, new Vector2D(capturePointError)));

for (SCS2LogFootState footState : footStates.values())
{
footState.afterRead(currentTime);
Expand Down
23 changes: 19 additions & 4 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
Expand Up @@ -3,6 +3,7 @@
import us.ihmc.avatar.logProcessor.SCS2LogWalk.DoubleSupportDuration;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootStateChange;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.FootSwing;
import us.ihmc.avatar.logProcessor.SCS2LogWalk.ICPErrorEntry;
import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.euclid.Axis3D;
import us.ihmc.euclid.geometry.tools.EuclidGeometryTools;
Expand Down Expand Up @@ -239,7 +240,7 @@ else if (leftPushBarDisturbed)

for (FootStateChange footStateChange : footStateChanges)
{
writer.write("%s,%.2f,%d,%s".formatted(side.getLowerCaseName(),
writer.write("%s,%.5f,%d,%s".formatted(side.getLowerCaseName(),
footStateChange.time(),
footStateChange.state().ordinal(),
footStateChange.state().name()));
Expand All @@ -263,7 +264,7 @@ else if (leftPushBarDisturbed)

for (FootSwing footSwing : timeSorted)
{
writer.write("%.2f,%.2f,%.2f".formatted(footSwing.completeTime(), footSwing.swingDuration(), footSwing.desiredSwingDuration()));
writer.write("%.5f,%.5f,%.5f".formatted(footSwing.completeTime(), footSwing.swingDuration(), footSwing.desiredSwingDuration()));
writer.newLine();
}
}
Expand All @@ -275,14 +276,28 @@ else if (leftPushBarDisturbed)
{
for (DoubleSupportDuration doubleSupportDuration : logWalk.getDoubleSupportDurations())
{
writer.write("%.2f,%.2f,%.2f".formatted(doubleSupportDuration.completeTime(),
writer.write("%.5f,%.5f,%.5f".formatted(doubleSupportDuration.completeTime(),
doubleSupportDuration.supportDuration(),
doubleSupportDuration.desiredTransferDuration()));
writer.newLine();
}
}
});


writeCSV(logFolderName, "ICPError", "Time,ErrorX,ErrorY", writer ->
{
for (SCS2LogWalk logWalk : locomotionData.getLogWalks())
{
for (ICPErrorEntry icpError : logWalk.getICPErrors())
{
writer.write("%.5f,%.5f,%.5f".formatted(icpError.time(),
icpError.icpError().getX(),
icpError.icpError().getY()));
writer.newLine();
}
}
});

for (RobotSide side : locomotionData.getHandFrames().sides())
{
try (BufferedWriter writer = Files.newBufferedWriter(logPath.resolve("%s_%sArmPoses.csv".formatted(logFolderName, side.getPascalCaseName()))))
Expand Down
8 changes: 8 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 @@ -5,6 +5,7 @@
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.yoVariables.variable.YoInteger;

Expand All @@ -14,6 +15,8 @@ public class SCS2LogWalk
{
private long walkStartTick = -1;
private final Point2D walkStart = new Point2D();
public record ICPErrorEntry(double time, Vector2D icpError) { }
private final ArrayList<ICPErrorEntry> icpErrors = new ArrayList<>();
private final ArrayList<SCS2LogFootstep> footsteps = new ArrayList<>();
public record FootStateChange(double time, ConstraintType state) { }
private final SideDependentList<ArrayList<FootStateChange>> footStateChanges = new SideDependentList<>(new ArrayList<>(), new ArrayList<>());
Expand Down Expand Up @@ -55,6 +58,11 @@ public Point2D getWalkStart()
return walkStart;
}

public ArrayList<ICPErrorEntry> getICPErrors()
{
return icpErrors;
}

public ArrayList<SCS2LogFootstep> getFootsteps()
{
return footsteps;
Expand Down
51 changes: 50 additions & 1 deletion 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 @@ -495,7 +495,6 @@ public void create()
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"));
Expand All @@ -520,6 +519,56 @@ public void create()
}
}
}, "Plot Swing Durations");
ThreadTools.startAsDaemon(() ->
{
String logFolderName = logDirectory.getFileName().toString();
Path csvFile = logDirectory.resolve(logFolderName + "_ICPError.csv");

if (Files.exists(csvFile))
{
try
{
Plot pyplot = Plot.create();

ArrayList<Double> times = new ArrayList<>();
ArrayList<Double> errorXs = new ArrayList<>();
ArrayList<Double> errorYs = new ArrayList<>();
try (BufferedReader reader = Files.newBufferedReader(csvFile))
{
reader.readLine(); // skip header
String line;
while ((line = reader.readLine()) != null)
{
String[] values = line.split(",");

times.add(Double.parseDouble(values[0]));
errorXs.add(Double.parseDouble(values[1]));
errorYs.add(Double.parseDouble(values[2]));
}
}
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, errorXs).label("ICP Error X");
pyplot.plot().add(times, errorYs).label("ICP Error Y");

pyplot.xlabel("Time (s)");
pyplot.title("%s ICP Error".formatted(titleCaseString));
pyplot.legend();
pyplot.show();
}
catch (IOException | PythonExecutionException e)
{
throw new RuntimeException(e);
}
}
}, "Plot ICP Error");
}
}

Expand Down

0 comments on commit 04e7308

Please sign in to comment.