Skip to content

Commit

Permalink
fix choreo splitting in pplib
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjansen4857
mjansen4857 committed Oct 12, 2024
1 parent 10b7208 commit dbf46b0
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 59 deletions.
16 changes: 9 additions & 7 deletions pathplannerlib-python/pathplannerlib/path.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -580,18 +580,20 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
fullPath.name = trajectory_name
PathPlannerPath._choreoPathCache[trajectory_name] = fullPath

splits = trajJson['splits']
for i in range(-1, len(splits)):
name = trajectory_name + '.' + str(i + 1)
splitsJson = trajJson['splits']
splits = [int(s) for s in splitsJson]
if len(splits) == 0 or int(splits[0]) != 0:
splits.insert(0, 0)

for i in range(len(splits)):
name = trajectory_name + '.' + str(i)
states = []

splitStartIdx = 0
if i != -1:
splitStartIdx = int(splits[i])
splitStartIdx = splits[i]

splitEndIdx = len(fullTrajStates)
if i < len(splits) - 1:
splitEndIdx = int(splits[i + 1])
splitEndIdx = splits[i + 1]

startTime = fullTrajStates[splitStartIdx].timeSeconds
endTime = fullTrajStates[splitEndIdx - 1].timeSeconds
Expand Down
22 changes: 14 additions & 8 deletions pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -420,19 +420,25 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
fullPath.name = trajectoryName;
choreoPathCache.put(trajectoryName, fullPath);

JSONArray splits = (JSONArray) trajJson.get("splits");
for (int i = -1; i < splits.size(); i++) {
String name = trajectoryName + "." + (i + 1);
JSONArray splitsJson = (JSONArray) trajJson.get("splits");
List<Integer> splits = new ArrayList<>();
for (Object o : splitsJson) {
splits.add(((Number) o).intValue());
}

if (splits.isEmpty() || splits.get(0) != 0) {
splits.add(0, 0);
}

for (int i = 0; i < splits.size(); i++) {
String name = trajectoryName + "." + i;
List<PathPlannerTrajectoryState> states = new ArrayList<>();

int splitStartIdx = 0;
if (i != -1) {
splitStartIdx = ((Number) splits.get(i)).intValue();
}
int splitStartIdx = splits.get(i);

int splitEndIdx = fullTrajStates.size();
if (i < splits.size() - 1) {
splitEndIdx = ((Number) splits.get(i + 1)).intValue();
splitEndIdx = splits.get(i + 1);
}

double startTime = fullTrajStates.get(splitStartIdx).timeSeconds;
Expand Down
93 changes: 49 additions & 44 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,58 +243,63 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache(
fullPath->name = trajectoryName;
PathPlannerPath::getChoreoPathCache().emplace(trajectoryName, fullPath);

auto splits = trajJson.at("splits");
if (splits.is_array()) {
int splitsSize = static_cast<int>(splits.size());
for (int i = -1; i < splitsSize; i++) {
std::string name = trajectoryName + "." + std::to_string(i + 1);
std::vector < PathPlannerTrajectoryState > states;

size_t splitStartIdx = 0;
if (i != -1) {
splitStartIdx = splits[i].get<size_t>();
}
auto splitsJson = trajJson.at("splits");
std::vector < size_t > splits;

size_t splitEndIdx = fullTrajStates.size();
if (i < splitsSize) {
splitEndIdx = splits[i + 1].get<size_t>();
}
if (splitsJson.is_array()) {
for (auto split : splitsJson) {
splits.emplace_back(split.get<size_t>());
}
}

auto startTime = fullTrajStates[splitStartIdx].time;
auto endTime = fullTrajStates[splitEndIdx - 1].time;
for (size_t s = splitStartIdx; s < splitEndIdx; s++) {
states.emplace_back(
fullTrajStates[s].copyWithTime(
fullTrajStates[s].time - startTime));
}
if (splits.empty() || splits[0] != 0) {
splits.insert(splits.begin(), 0);
}

std::vector < std::shared_ptr < Event >> events;
for (auto originalEvent : fullEvents) {
if (originalEvent->getTimestamp() >= startTime
&& originalEvent->getTimestamp() < endTime) {
events.emplace_back(
originalEvent->copyWithTimestamp(
originalEvent->getTimestamp() - startTime));
}
}
for (size_t i = 0; i < splits.size(); i++) {
std::string name = trajectoryName + "." + std::to_string(i);
std::vector < PathPlannerTrajectoryState > states;

auto path =
std::make_shared < PathPlannerPath
> (PathConstraints::unlimitedConstraints(12_V), GoalEndState(
states[states.size() - 1].linearVelocity,
states[states.size() - 1].pose.Rotation()));
size_t splitStartIdx = splits[i];

std::vector < PathPoint > pathPoints;
for (auto state : states) {
pathPoints.emplace_back(state.pose.Translation());
size_t splitEndIdx = fullTrajStates.size();
if (i < splits.size()) {
splitEndIdx = splits[i + 1];
}

auto startTime = fullTrajStates[splitStartIdx].time;
auto endTime = fullTrajStates[splitEndIdx - 1].time;
for (size_t s = splitStartIdx; s < splitEndIdx; s++) {
states.emplace_back(
fullTrajStates[s].copyWithTime(
fullTrajStates[s].time - startTime));
}

std::vector < std::shared_ptr < Event >> events;
for (auto originalEvent : fullEvents) {
if (originalEvent->getTimestamp() >= startTime
&& originalEvent->getTimestamp() < endTime) {
events.emplace_back(
originalEvent->copyWithTimestamp(
originalEvent->getTimestamp() - startTime));
}
}

auto path = std::make_shared < PathPlannerPath
> (PathConstraints::unlimitedConstraints(12_V), GoalEndState(
states[states.size() - 1].linearVelocity,
states[states.size() - 1].pose.Rotation()));

path->m_allPoints = pathPoints;
path->m_isChoreoPath = true;
path->m_idealTrajectory = PathPlannerTrajectory(states, events);
path->name = name;
PathPlannerPath::getChoreoPathCache().emplace(name, path);
std::vector < PathPoint > pathPoints;
for (auto state : states) {
pathPoints.emplace_back(state.pose.Translation());
}

path->m_allPoints = pathPoints;
path->m_isChoreoPath = true;
path->m_idealTrajectory = PathPlannerTrajectory(states, events);
path->name = name;
PathPlannerPath::getChoreoPathCache().emplace(name, path);
}
}

Expand Down

0 comments on commit dbf46b0

Please sign in to comment.