@Override public void set(PelvisHeightTrajectoryCommand other) { super.set(other); setPropertiesOnly(other); }
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; }