@Override public void clear() { for (RobotSide robotSide : RobotSide.values) { handTrajectoryControllerCommands.get(robotSide).clear(); armTrajectoryControllerCommands.get(robotSide).clear(); footTrajectoryControllerCommands.get(robotSide).clear(); } chestTrajectoryControllerCommand.clear(); pelvisTrajectoryControllerCommand.clear(); }
private void sendPelvisTrajectoryCommand() { userDesiredPelvisPose.getFramePoseIncludingFrame(framePose); framePose.changeFrame(worldFrame); double time = userDesiredPelvisPoseTrajectoryTime.getDoubleValue(); framePose.getPose(position, orientation); poseCommand.clear(); poseCommand.addTrajectoryPoint(time, position, orientation, zeroVelocity, zeroVelocity); poseCommand.setExecutionMode(ExecutionMode.OVERRIDE); poseCommand.setCommandId(Packet.VALID_MESSAGE_DEFAULT_ID); if (DEBUG) System.out.println("Submitting " + poseCommand); controllerCommandInputManager.submitCommand(poseCommand); }
private int queueExceedingTrajectoryPointsIfNeeded(PelvisTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = positionTrajectoryGenerator.getMaximumNumberOfWaypoints() - positionTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; PelvisTrajectoryCommand commandForExcedent = commandQueue.addFirst(); numberOfQueuedCommands.increment(); commandForExcedent.clear(); commandForExcedent.setPropertiesOnly(command); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }