public void set(PelvisTrajectoryCommand pelvisTrajectoryCommand) { setIncludingFrame(pelvisTrajectoryCommand); setCommandId(pelvisTrajectoryCommand.getCommandId()); setExecutionMode(pelvisTrajectoryCommand.getExecutionMode()); setPreviousCommandId(pelvisTrajectoryCommand.getPreviousCommandId()); }
private boolean queuePelvisTrajectoryCommand(PelvisTrajectoryCommand 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 handlePelvisTrajectoryCommand(PelvisTrajectoryCommand command) { switch (command.getExecutionMode()) { case OVERRIDE: isReadyToHandleQueuedCommands.set(true); clearCommandQueue(command.getCommandId()); initialPelvisPositionTime.set(yoTime.getDoubleValue()); initializeTrajectoryGenerator(command, 0.0); return; case QUEUE: boolean success = queuePelvisTrajectoryCommand(command); if (!success) { isReadyToHandleQueuedCommands.set(false); clearCommandQueue(INVALID_MESSAGE_ID); holdCurrentPosition(); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); break; } }
public void set(PelvisTrajectoryCommand command) { command.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); clear(); for (int i = 0; i < command.getNumberOfTrajectoryPoints(); i++) { FrameSE3TrajectoryPoint trajectoryPoint = command.getTrajectoryPoint(i); double time = trajectoryPoint.getTime(); double position = trajectoryPoint.getPositionZ(); double velocity = trajectoryPoint.getLinearVelocityZ(); addTrajectoryPoint(time, position, velocity); } commandId = command.getCommandId(); executionMode = command.getExecutionMode(); previousCommandId = command.getPreviousCommandId(); }