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);
desiredFootstepCommand.setPose(desiredPosition.getPoint(), desiredOrientation.getQuaternion());