Quaternion orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand);
Quaternion orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand);
desiredFootstepCommand.setRobotSide(swingSide); desiredFootstepCommand.setSwingHeight(swingHeight.getDoubleValue());