private int queueExceedingTrajectoryPointsIfNeeded(PelvisHeightTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = offsetHeightTrajectoryGenerator.getMaximumNumberOfWaypoints() - offsetHeightTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; PelvisHeightTrajectoryCommand 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 queuePelvisHeightTrajectoryCommand(PelvisHeightTrajectoryCommand 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; }
private void initializeOffsetTrajectoryGenerator(PelvisHeightTrajectoryCommand command, double firstTrajectoryPointTime) { command.addTimeOffset(firstTrajectoryPointTime); offsetHeightTrajectoryGenerator.clear(); if (command.getTrajectoryPoint(0).getTime() > firstTrajectoryPointTime + 1.0e-5) { offsetHeightTrajectoryGenerator.appendWaypoint(0.0, offsetHeightAboveGroundPrevValue.getDoubleValue(), 0.0); } int numberOfTrajectoryPoints = queueExceedingTrajectoryPointsIfNeeded(command); for (int trajectoryPointIndex = 0; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { SimpleTrajectoryPoint1D waypoint = command.getTrajectoryPoint(trajectoryPointIndex); double time = waypoint.getTime(); double z = waypoint.getPosition(); double zDot = waypoint.getVelocity(); // TODO (Sylvain) Check if that's the right way to do it desiredPosition.setIncludingFrame(worldFrame, 0.0, 0.0, z); desiredPosition.changeFrame(frameOfLastFoostep); double zOffset = desiredPosition.getZ() - splineOutput[0]; offsetHeightTrajectoryGenerator.appendWaypoint(time, zOffset, zDot); } offsetHeightTrajectoryGenerator.initialize(); isTrajectoryOffsetStopped.set(false); }