public boolean queueHandTrajectoryCommand(HandTrajectoryCommand 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.");
isReadyToHandleQueuedCommands.set(false);
clearCommandQueue(INVALID_MESSAGE_ID);
abortTaskspaceControlState.set(true);
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.");
isReadyToHandleQueuedCommands.set(false);
clearCommandQueue(INVALID_MESSAGE_ID);
abortTaskspaceControlState.set(true);
return false;
}
commandQueue.add(command);
numberOfQueuedCommands.increment();
lastCommandId.set(command.getCommandId());
return true;
}