public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime)
{
FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime,transferTime);
for (int i = 0; i < footsteps.size(); i++)
{
Footstep footstep = footsteps.get(i);
Point3d location = new Point3d(footstep.getX(), footstep.getY(), footstep.getZ());
Quat4d orientation = new Quat4d();
footstep.getOrientation(orientation);
RobotSide footstepSide = footstep.getRobotSide();
FootstepDataMessage footstepData = new FootstepDataMessage(footstepSide, location, orientation);
footstepDataList.add(footstepData);
}
set(footstepDataList);
}