footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation);
footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand);
footstepCommandList.addFootstep(desiredFootstepCommand);