public void set(PelvisTrajectoryCommand pelvisTrajectoryCommand) { setIncludingFrame(pelvisTrajectoryCommand); setCommandId(pelvisTrajectoryCommand.getCommandId()); setExecutionMode(pelvisTrajectoryCommand.getExecutionMode()); setPreviousCommandId(pelvisTrajectoryCommand.getPreviousCommandId()); }
private int queueExceedingTrajectoryPointsIfNeeded(PelvisOrientationTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = orientationOffsetTrajectoryGenerator.getMaximumNumberOfWaypoints() - orientationOffsetTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; PelvisOrientationTrajectoryCommand 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; }
private void sendPelvisOrientationTrajectoryCommand() { userDesiredPelvisPose.getFramePoseIncludingFrame(framePose); framePose.changeFrame(worldFrame); double time = userDesiredPelvisPoseTrajectoryTime.getDoubleValue(); framePose.getOrientation(orientation); orientationCommand.clear(); orientationCommand.addTrajectoryPoint(time, orientation, zeroVelocity); orientationCommand.setExecutionMode(ExecutionMode.OVERRIDE); orientationCommand.setCommandId(Packet.VALID_MESSAGE_DEFAULT_ID); if (DEBUG) System.out.println("Submitting " + orientationCommand); controllerCommandInputManager.submitCommand(orientationCommand); } }
private boolean queuePelvisOrientationTrajectoryCommand(PelvisOrientationTrajectoryCommand command) { if (!isReadyToHandleQueuedCommands.getBooleanValue()) { PrintTools.warn(this, "The very first " + command.getClass().getSimpleName() + " of a series must be " + ExecutionMode.OVERRIDE + ". Aborting motion."); return false; } long previousCommandId = command.getPreviousCommandId(); if (previousCommandId != INVALID_MESSAGE_ID && lastCommandId.getLongValue() != INVALID_MESSAGE_ID && lastCommandId.getLongValue() != previousCommandId) { PrintTools.warn(this, "Previous command ID mismatch: previous ID from command = " + previousCommandId + ", last message ID received by the controller = " + lastCommandId.getLongValue() + ". Aborting motion."); return false; } if (command.getTrajectoryPoint(0).getTime() < 1.0e-5) { PrintTools.warn(this, "Time of the first trajectory point of a queued command must be greater than zero. Aborting motion."); return false; } commandQueue.add(command); numberOfQueuedCommands.increment(); lastCommandId.set(command.getCommandId()); return true; }
public void handlePelvisOrientationTrajectoryCommands(PelvisOrientationTrajectoryCommand command) { switch (command.getExecutionMode()) { case OVERRIDE: isReadyToHandleQueuedCommands.set(true); clearCommandQueue(command.getCommandId()); initialPelvisOrientationOffsetTime.set(yoTime.getDoubleValue()); initializeOffsetTrajectoryGenerator(command, 0.0); return; case QUEUE: boolean success = queuePelvisOrientationTrajectoryCommand(command); if (!success) { isReadyToHandleQueuedCommands.set(false); clearCommandQueue(INVALID_MESSAGE_ID); setToHoldCurrent(pelvisOrientationTrajectoryGenerator.getCurrentTrajectoryFrame()); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); break; } }
private void initializeOffsetTrajectoryGenerator(PelvisOrientationTrajectoryCommand command, double firstTrajectoryPointTime) { command.addTimeOffset(firstTrajectoryPointTime); if (command.getTrajectoryPoint(0).getTime() > firstTrajectoryPointTime + 1.0e-5) { orientationOffsetTrajectoryGenerator.getOrientation(tempOrientation); tempOrientation.changeFrame(worldFrame); tempAngularVelocity.setToZero(worldFrame); orientationOffsetTrajectoryGenerator.clear(); orientationOffsetTrajectoryGenerator.switchTrajectoryFrame(worldFrame); orientationOffsetTrajectoryGenerator.appendWaypoint(0.0, tempOrientation, tempAngularVelocity); } else { orientationOffsetTrajectoryGenerator.clear(); orientationOffsetTrajectoryGenerator.switchTrajectoryFrame(worldFrame); } int numberOfTrajectoryPoints = queueExceedingTrajectoryPointsIfNeeded(command); for (int trajectoryPointIndex = 0; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { orientationOffsetTrajectoryGenerator.appendWaypoint(command.getTrajectoryPoint(trajectoryPointIndex)); } orientationOffsetTrajectoryGenerator.changeFrame(desiredPelvisFrame); orientationOffsetTrajectoryGenerator.initialize(); isTrajectoryStopped.set(false); }
command.getLastTrajectoryPoint().getOrientation(desiredPelvisOrientation); desiredPelvisOrientationReference.set(desiredPelvisOrientation); pelvisSelectionMatrix = new DenseMatrix64F(command.getSelectionMatrix());