public FrameTrajectoryPointList(Class<F> frameTrajectoryPointClass) { trajectoryPoints = new RecyclingArrayList<>(frameTrajectoryPointClass); }
@Override public void addTrajectoryPoint(P trajectoryPoint) { trajectoryPoints.add().set(trajectoryPoint); }
@Override public void setWaypoints(ArrayList<FramePoint> waypointPositions) { if (waypointPositions.size() > maxWaypoints) throw new RuntimeException("Too many waypoints"); this.waypointPositions.clear(); coefficients.clear(); coefficients.add(); for (int i = 0; i < waypointPositions.size(); i++) { waypointPosition.setIncludingFrame(waypointPositions.get(i)); waypointPosition.changeFrame(swingFrame); TDoubleArrayList waypoint = this.waypointPositions.add(); waypoint.set(0, this.waypointPosition.getX()); waypoint.set(1, this.waypointPosition.getZ()); coefficients.add(); } optimizer.setWaypoints(this.waypointPositions); segments.set(waypointPositions.size() + 1); }
@Override public void clear() { trajectoryPoints.clear(); }
public boolean constructFromInteriorOfRays(List<Line2d> rays, ConvexPolygon2d polygonToPack) removeRay.clear(); removeRay.getAndGrowIfNeeded(i).setValue(false); if (removeRay.get(rayIndex).booleanValue()) if (removeRay.get(previousIndexToCheck).booleanValue()) continue; // That ray was removed already. Check a previous ray. removeRay.get(previousIndexToCheck).setValue(true); removeIntersectionPoint(intersectionPoints, previousIndexToCheck); if (removeRay.get(evenMorePreviousIndexToCheck).booleanValue()) continue; // That ray was removed already. Check a previous ray. Line2d evenMorePreviousRayToCheck = rays.get(evenMorePreviousIndexToCheck); removeRay.get(previousIndexToCheck).setValue(true); removeIntersectionPoint(intersectionPoints, previousIndexToCheck);
@Override public F getTrajectoryPoint(int trajectoryPointIndex) { return trajectoryPoints.get(trajectoryPointIndex); }
@Override public int getNumberOfTrajectoryPoints() { return trajectoryPoints.size(); }
/** * Add a new element at the end of this list. * @return the new element. */ public T add() { return getAndGrowIfNeeded(size); }
@Override public boolean isCommandValid() { return timestamp != -1L && !scan.isEmpty(); } }
@Override public P getLastTrajectoryPoint() { return trajectoryPoints.getLast(); }
protected F addAndInitializeTrajectoryPoint() { F newTrajectoryPoint = trajectoryPoints.add(); newTrajectoryPoint.setToZero(referenceFrame); return newTrajectoryPoint; }
/** * Resets the optimizer and removes all waypoints as well as previous start and end conditions. */ public void reset() { initialPosition.setToNaN(trajectoryFrame); initialVelocity.setToNaN(trajectoryFrame); finalPosition.setToNaN(trajectoryFrame); finalVelocity.setToNaN(trajectoryFrame); segments.set(1); this.waypointPositions.clear(); optimizer.setWaypoints(waypointPositions); coefficients.clear(); coefficients.add(); }
public void clear(RobotSide robotSide) { commandId = Packet.VALID_MESSAGE_DEFAULT_ID; jointTrajectoryInputs.clear(); this.robotSide = robotSide; executionMode = null; previousCommandId = Packet.INVALID_MESSAGE_ID; }
@Override public P getTrajectoryPoint(int trajectoryPointIndex) { return trajectoryPoints.get(trajectoryPointIndex); }
@Override public int getNumberOfTrajectoryPoints() { return trajectoryPoints.size(); }