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(); }
private int queueExceedingTrajectoryPointsIfNeeded(PelvisTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = positionTrajectoryGenerator.getMaximumNumberOfWaypoints() - positionTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; PelvisTrajectoryCommand 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; }