footPointList.addTrajectoryPoint(4.0, new Point3D(0.0, -0.2, 0.15), new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D(), new Vector3D());
footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList);
footTrajectoryCommand.setRobotSide(RobotSide.RIGHT);
queuedControllerCommands.add(footTrajectoryCommand);
footPointList.addTrajectoryPoint(4.0, new Point3D(0.0, -0.2, 0.15), new Quaternion(0.0, 0.1, 0.0, 1.0), new Vector3D(), new Vector3D());
footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList);
footTrajectoryCommand.setRobotSide(RobotSide.RIGHT);
queuedControllerCommands.add(footTrajectoryCommand);