public void appendTrajectoryPoint(int jointIndex, SimpleTrajectoryPoint1D trajectoryPoint) { jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex).addTrajectoryPoint(trajectoryPoint); }
@Override public void setFromMessage(OneDoFJointTrajectoryMessage message) { this.clear(); List<TrajectoryPoint1DMessage> trajectoryPointMessages = message.getTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { TrajectoryPoint1DMessage trajectoryPoint1DMessage = trajectoryPointMessages.get(i); this.addTrajectoryPoint(trajectoryPoint1DMessage.getTime(), trajectoryPoint1DMessage.getPosition(), trajectoryPoint1DMessage.getVelocity()); } setWeight(message.getWeight()); }
private int queueExcedingTrajectoryPointsIfNeeded(int jointIndex, OneDoFJointTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); OneDoFJoint joint = controlledJoints[jointIndex]; MultipleWaypointsTrajectoryGenerator jointTrajectoryGenerator = jointTrajectoryGenerators.get(joint); int maximumNumberOfWaypoints = jointTrajectoryGenerator.getMaximumNumberOfWaypoints(); maximumNumberOfWaypoints -= jointTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; OneDoFJointTrajectoryCommand commandForExcedent = commandQueues.get(joint).addFirst(); numberOfQueuedCommands.get(joint).increment(); commandForExcedent.clear(); commandForExcedent.setCommandId(command.getCommandId()); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }