public void packRobotComCopSeries(ArrayList<Point3d> outCom, ArrayList<Point3d> outCop) {
outCom.clear();
outCop.clear();
for (int i = 0; i < selectedFrames.length; i++) {
dataBuffer.setIndexButDoNotNotifySimulationRewoundListeners(selectedFrames[i]);
// model predicted CoM
robot.update(); // this pull data from dataBuffer magically through YoVariables
Point3d modelCoM = new Point3d();
robot.computeCenterOfMass(modelCoM);
outCom.add(modelCoM);
// sensedCoP
Point3d sensedCoP =
new Point3d(
dataBuffer.getVariable("sensedCoPX").getValueAsDouble(),
dataBuffer.getVariable("sensedCoPY").getValueAsDouble(),
dataBuffer.getVariable("sensedCoPZ").getValueAsDouble());
outCop.add(sensedCoP);
}
}
