Skip to content

Commit

Permalink
Improve plot visuals and plot ICP
Browse files Browse the repository at this point in the history
  • Loading branch information
@calvertdw
calvertdw committed Oct 4, 2024
1 parent ca03300 commit 1df4753
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 54 deletions.
61 changes: 35 additions & 26 deletions ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/logProcessor/SCS2LogDataProcessor.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,25 +2,25 @@

import org.jfree.svg.SVGGraphics2D;
import org.jfree.svg.SVGHints;
import org.jfree.svg.SVGHints.Key;
import org.jfree.svg.SVGUnits;
import org.jfree.svg.SVGUtils;
import us.ihmc.commonWalkingControlModules.desiredFootStep.FootstepListVisualizer;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.log.LogTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.scs2.session.log.LogSession;
import us.ihmc.tools.io.JSONFileTools;
import us.ihmc.tools.thread.MissingThreadTools;

import java.awt.*;
import java.awt.Color;
import java.awt.BasicStroke;
import java.awt.Font;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.UUID;
import java.util.List;
import java.util.function.Consumer;

public class SCS2LogDataProcessor
Expand Down Expand Up @@ -183,7 +183,7 @@ private void drawSVG()
svgGraphics2D = new SVGGraphics2D(documentSizeMillimeters, documentSizeMillimeters, SVGUnits.MM);

svgGraphics2D.setColor(Color.BLACK);
svgGraphics2D.setStroke(new BasicStroke(7));
svgGraphics2D.setStroke(new BasicStroke(5));
svgGraphics2D.setFont(new Font("Arial", Font.PLAIN, 20));
svgGraphics2D.setFontSizeUnits(SVGUnits.MM);

Expand All @@ -199,7 +199,8 @@ private void drawSVG()

for (SCS2LogDataFootstep footstep : logWalk.getFootsteps())
{
svgGraphics2D.setColor(footstep.getSide() == RobotSide.LEFT ? Color.RED : Color.GREEN);
Color color = FootstepListVisualizer.defaultFeetColors.get(footstep.getSide());
svgGraphics2D.setColor(color);

double[] polygon = footstep.getPolygon();
LogTools.info("Drawing step at {} {}", new Point2D(polygon[0], polygon[4]), new Point2D(metersToMMX(polygon[0]), metersToMMY(polygon[4])));
Expand All @@ -214,10 +215,17 @@ private void drawSVG()
4);
}

svgGraphics2D.setColor(Color.BLACK);

svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, groupName);

svgGraphics2D.setStroke(new BasicStroke(2.5f));
svgGraphics2D.setColor(Color.BLACK);
plot(logWalk.getComs(), "Coms");
svgGraphics2D.setColor(Color.BLUE);

svgGraphics2D.setStroke(new BasicStroke(2.5f, BasicStroke.CAP_BUTT, BasicStroke.JOIN_MITER, 10.0f, new float[] {10.0f}, 0.0f));
plot(logWalk.getIcps(), "ICPs");

svgGraphics2D.setColor(Color.BLACK);
if (logWalk.isEndedWithFall())
{
Point2D fallLocation;
Expand All @@ -234,27 +242,11 @@ private void drawSVG()
svgGraphics2D.drawString("Fall %d".formatted(walk), metersToMMX(fallLocation.getX()), metersToMMY(fallLocation.getY() + 0.3));
}


svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, walkName);

++walk;
}

RecyclingArrayList<Point2D> coms = locomotionData.getComs();
if (!coms.isEmpty())
{
int[] comXs = new int[coms.size()];
int[] comYs = new int[coms.size()];
for (int i = 0; i < coms.size(); i++)
{
comXs[i] = metersToMMX(coms.get(i).getX());
comYs[i] = metersToMMY(coms.get(i).getY());
}
svgGraphics2D.setColor(Color.BLUE);
svgGraphics2D.setRenderingHint(SVGHints.KEY_BEGIN_GROUP, "Coms");
svgGraphics2D.drawPolyline(comXs, comYs, comXs.length);
svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, "Coms");
}

LogTools.info("Saving SVG to {}", svgPath);

Expand All @@ -271,6 +263,23 @@ private void drawSVG()
}
}

private void plot(List<Point2D> points, String name)
{
if (!points.isEmpty())
{
int[] xs = new int[points.size()];
int[] ys = new int[points.size()];
for (int i = 0; i < points.size(); i++)
{
xs[i] = metersToMMX(points.get(i).getX());
ys[i] = metersToMMY(points.get(i).getY());
}
svgGraphics2D.setRenderingHint(SVGHints.KEY_BEGIN_GROUP, name);
svgGraphics2D.drawPolyline(xs, ys, xs.length);
svgGraphics2D.setRenderingHint(SVGHints.KEY_END_GROUP, name);
}
}

private int metersToMMX(double x)
{
double fromStart = x - locomotionData.getRobotStartLocation().getX();
Expand Down Expand Up @@ -337,7 +346,7 @@ public int getNumberOfFootstepsStat()

public int getNumberOfComsStat()
{
return locomotionData == null ? numberOfComsStat : locomotionData.getComs().size();
return locomotionData == null ? numberOfComsStat : locomotionData.getNumberOfComs();
}

public int getWorkingCounterMismatchStat()
Expand Down
60 changes: 32 additions & 28 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 @@ -2,13 +2,13 @@

import com.fasterxml.jackson.databind.node.ObjectNode;
import us.ihmc.commonWalkingControlModules.controlModules.foot.FootControlModule.ConstraintType;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.log.LogTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.scs2.session.log.LogSession;
import us.ihmc.yoVariables.euclid.YoPoint2D;
import us.ihmc.yoVariables.euclid.YoPoint3D;
import us.ihmc.yoVariables.registry.YoRegistry;
import us.ihmc.yoVariables.variable.YoBoolean;
Expand All @@ -26,14 +26,14 @@ public class SCS2LogLocomotionData
private YoInteger workingCounterMismatch;
private YoBoolean isRobotFalling;
private SCS2LogDataEnum<HighLevelControllerName> controllerState;
private final Point2D robotStartLocation = new Point2D();
private final Point2D robotStartLocation = new Point2D(Double.NaN, Double.NaN);
private final SideDependentList<SCS2LogDataFootState> footStates = new SideDependentList<>();
private final ArrayList<SCS2LogWalk> logWalks = new ArrayList<>();
private final Point2D lastCenterOfMass = new Point2D(Double.NaN, Double.NaN);
private YoPoint3D centerOfMass;
private final double comPlotResolution = 0.1;
private YoPoint2D capturePoint;
private final double plotTimeResolution = 0.1;
private double lastCoMPlotTime = Double.NaN;
private final RecyclingArrayList<Point2D> coms = new RecyclingArrayList<>(Point2D::new);
private boolean requestStopProcessing = false;

public void setup(LogSession logSession)
Expand All @@ -58,6 +58,10 @@ public void setup(LogSession logSession)
&& rootRegistry.findVariable(momentumRateControl + "centerOfMassY") instanceof YoDouble yVariable
&& rootRegistry.findVariable(momentumRateControl + "centerOfMassX") instanceof YoDouble zVariable)
centerOfMass = new YoPoint3D(xVariable, yVariable, zVariable);

if (rootRegistry.findVariable(momentumRateControl + "capturePointX") instanceof YoDouble xVariable
&& rootRegistry.findVariable(momentumRateControl + "capturePointY") instanceof YoDouble yVariable)
capturePoint = new YoPoint2D(xVariable, yVariable);

String feetManager = highLevelController + "HighLevelHumanoidControllerFactory.HighLevelControlManagerFactory.FeetManager.";
for (RobotSide side : RobotSide.values)
Expand Down Expand Up @@ -106,44 +110,39 @@ private void afterRead(double currentTime)
footStates.get(side).getFootsteps().clear();
}

if (recentSteps)
if (robotStartLocation.containsNaN())
{
if (Double.isNaN(lastCoMPlotTime) || currentTime - lastCoMPlotTime > comPlotResolution)
{
if (coms.isEmpty())
{
robotStartLocation.set(centerOfMass.getX(), centerOfMass.getY());
LogTools.info("Robot start location: {}", robotStartLocation);
}

coms.add().set(centerOfMass);

lastCenterOfMass.set(centerOfMass);
lastCoMPlotTime = currentTime;
}
robotStartLocation.set(centerOfMass.getX(), centerOfMass.getY());
LogTools.info("Robot start location: {}", robotStartLocation);
}


if (lastCenterOfMass.containsNaN())
{
logWalk.getComs().add().set(centerOfMass);
lastCenterOfMass.set(centerOfMass);
lastCoMPlotTime = currentTime;
}
else if (centerOfMass.distanceXY(lastCenterOfMass) > 0.001 && currentTime - lastCoMPlotTime > plotTimeResolution)
{
logWalk.getComs().add().set(centerOfMass);
logWalk.getIcps().add().set(capturePoint);
lastCenterOfMass.set(centerOfMass);
lastCoMPlotTime = currentTime;
}
}


// TODO:
// # Falls
// # Runs of action (split by 30 seconds of inactivity)
// Timestamps where runs start
// Arm motions




}

public void writeJSON(ObjectNode rootNode)
{
rootNode.put("numberOfWalks", logWalks.size());
rootNode.put("numberOfFalls", getFalls());
rootNode.put("numberOfFootsteps", getNumberOfFootsteps());
rootNode.put("numberOfComs", coms.size());
rootNode.put("numberOfComs", getNumberOfComs());
rootNode.put("workingCounterMismatch", getWorkingCounterMismatch());
}

Expand Down Expand Up @@ -172,9 +171,14 @@ public int getNumberOfFootsteps()
return numberOfFootsteps;
}

public RecyclingArrayList<Point2D> getComs()
public int getNumberOfComs()
{
return coms;
int numberOfComs = 0;
for (SCS2LogWalk logWalk : logWalks)
{
numberOfComs += logWalk.getComs().size();
}
return numberOfComs;
}

public ArrayList<SCS2LogWalk> getLogWalks()
Expand Down
6 changes: 6 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 @@ -12,6 +12,7 @@ public class SCS2LogWalk
private final Point2D walkStart = new Point2D();
private final ArrayList<SCS2LogDataFootstep> footsteps = new ArrayList<>();
private final RecyclingArrayList<Point2D> coms = new RecyclingArrayList<>(Point2D::new);
private final RecyclingArrayList<Point2D> icps = new RecyclingArrayList<>(Point2D::new);

private boolean endedWithFall = false;
private int initialWorkingCounterMismatch = -1;
Expand Down Expand Up @@ -60,6 +61,11 @@ public RecyclingArrayList<Point2D> getComs()
return coms;
}

public RecyclingArrayList<Point2D> getIcps()
{
return icps;
}

public int getInitialWorkingCounterMismatch()
{
return initialWorkingCounterMismatch;
Expand Down

0 comments on commit 1df4753

Please sign in to comment.