public void variableChanged(YoVariable<?> v) { if (userDoChestOrientation.getBooleanValue()) { userDesiredChestOrientation.getFrameOrientationIncludingFrame(frameOrientation); ChestTrajectoryCommand chestTrajectoryControllerCommand = new ChestTrajectoryCommand(); chestTrajectoryControllerCommand.addTrajectoryPoint(userDesiredChestTrajectoryTime.getDoubleValue(), frameOrientation.getQuaternionCopy(), new Vector3d()); controllerCommandInputManager.submitCommand(chestTrajectoryControllerCommand); userDoChestOrientation.set(false); } } });
private int queueExceedingTrajectoryPointsIfNeeded(ChestTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = orientationTrajectoryGenerator.getMaximumNumberOfWaypoints() - orientationTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; ChestTrajectoryCommand 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 boolean queueChestTrajectoryCommand(ChestTrajectoryCommand 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; }
ChestTrajectoryCommand chestCommand = new ChestTrajectoryCommand(); FrameSO3TrajectoryPointList chestTrajectoryPointList = new FrameSO3TrajectoryPointList(); chestTrajectoryPointList.addTrajectoryPoint(0.0, new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D()); chestTrajectoryPointList.addTrajectoryPoint(2.0, new Quaternion(-0.2, 0.0, 0.0, 1.0), new Vector3D()); chestTrajectoryPointList.addTrajectoryPoint(3.0, new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D()); chestCommand.getSO3Trajectory().setTrajectoryPointList(chestTrajectoryPointList); chestCommand.getSO3Trajectory().setTrajectoryFrame(ReferenceFrame.getWorldFrame()); queuedControllerCommands.add(chestCommand);
public void handleChestTrajectoryCommand(ChestTrajectoryCommand command, FrameOrientation currentDesired) { switch (command.getExecutionMode()) { case OVERRIDE: isReadyToHandleQueuedCommands.set(true); clearCommandQueue(command.getCommandId()); receivedNewChestOrientationTime.set(yoTime.getDoubleValue()); initializeTrajectoryGenerator(command, 0.0, currentDesired); return; case QUEUE: boolean success = queueChestTrajectoryCommand(command); if (!success) { isReadyToHandleQueuedCommands.set(false); clearCommandQueue(INVALID_MESSAGE_ID); holdOrientation(currentDesired); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); return; } }
private void initializeTrajectoryGenerator(ChestTrajectoryCommand command, double firstTrajectoryPointTime, FrameOrientation currentDesired) { command.addTimeOffset(firstTrajectoryPointTime); orientationTrajectoryGenerator.clear(worldFrame); if (command.getTrajectoryPoint(0).getTime() > firstTrajectoryPointTime + 1.0e-5) { desiredOrientation.setIncludingFrame(currentDesired); desiredOrientation.changeFrame(worldFrame); desiredAngularVelocity.setToZero(worldFrame); orientationTrajectoryGenerator.appendWaypoint(0.0, desiredOrientation, desiredAngularVelocity); } int numberOfTrajectoryPoints = queueExceedingTrajectoryPointsIfNeeded(command); for (int trajectoryPointIndex = 0; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { orientationTrajectoryGenerator.appendWaypoint(command.getTrajectoryPoint(trajectoryPointIndex)); } orientationTrajectoryGenerator.changeFrame(pelvisZUpFrame); orientationTrajectoryGenerator.initialize(); isTrackingOrientation.set(true); isTrajectoryStopped.set(false); }
@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(); }
command.getLastTrajectoryPoint().getOrientation(desiredChestOrientation); desiredChestOrientationReference.set(desiredChestOrientation); chestSelectionMatrix = new DenseMatrix64F(command.getSelectionMatrix());