public static FootstepDataMessage createFootstepDataMessage(Footstep footstep)
{
FootstepDataMessage message = new FootstepDataMessage();
message.setRobotSide(footstep.getRobotSide().toByte());
FramePoint3D location = new FramePoint3D();
FrameQuaternion orientation = new FrameQuaternion();
footstep.getPose(location, orientation);
footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
message.getLocation().set(location);
message.getOrientation().set(orientation);
packPredictedContactPoints(footstep.getPredictedContactPoints(), message);
message.setTrajectoryType(footstep.getTrajectoryType().toByte());
message.setSwingHeight(footstep.getSwingHeight());
message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration());
if (footstep.getCustomPositionWaypoints().size() != 0)
{
for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++)
{
FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i);
framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
message.getCustomPositionWaypoints().add().set(framePoint);
}
}
return message;
}