Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix VTS indices (again) #30

Merged
merged 2 commits into from
Feb 19, 2024
Merged

Fix VTS indices (again) #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a50792f
Fix build on arm64
camearle20 Feb 19, 2024
1078d63
Refactor trajectory_service.cpp to handle indices between segments co…
camearle20 Feb 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 21 additions & 7 deletions trajectory_native/Earthfile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,11 @@ grpc:
RUN git clone --recurse-submodules -b v1.60.0 --depth 1 --shallow-submodules https://github.com/grpc/grpc
RUN mkdir grpc/build
WORKDIR grpc/build
ENV CC=clang
ENV CXX=clang++
ARG TARGETARCH
IF [ "$TARGETARCH" = "arm64" ]
ENV CC=clang
ENV CXX=clang++
END
RUN cmake -DgRPC_INSTALL=ON -DgRPC_BUILD_TESTS=OFF ..
RUN make -j4
RUN make DESTDIR=$(pwd)/installroot_top install
Expand All @@ -27,11 +30,14 @@ trajoptlib:
GIT CLONE --branch 95c20be79be7557673d75d631703dc92fe6a191e https://github.com/SleipnirGroup/TrajoptLib TrajoptLib
WORKDIR TrajoptLib
COPY trajoptlib-aarch64.patch .
RUN git apply trajoptlib-aarch64.patch
RUN git apply --ignore-space-change --ignore-whitespace trajoptlib-aarch64.patch
RUN mkdir build
WORKDIR build
ENV CC=clang
ENV CXX=clang++
ARG TARGETARCH
IF [ "$TARGETARCH" = "arm64" ]
ENV CC=clang
ENV CXX=clang++
END
RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF ..
RUN make -j4
RUN make DESTDIR=$(pwd)/installroot_top install
Expand All @@ -45,8 +51,11 @@ dev-image:
BUILD +trajoptlib
COPY +grpc/installroot /usr/local/
COPY +trajoptlib/installroot /usr/local/
ENV CC=clang
ENV CXX=clang++
ARG TARGETARCH
IF [ "$TARGETARCH" = "arm64" ]
ENV CC=clang
ENV CXX=clang++
END
SAVE IMAGE littletonrobotics/vts-dev

vts:
Expand All @@ -56,6 +65,11 @@ vts:
COPY CMakeLists.txt CMakeLists.txt
RUN mkdir build
WORKDIR build
ARG TARGETARCH
IF [ "$TARGETARCH" = "arm64" ]
ENV CC=clang
ENV CXX=clang++
END
RUN cmake ..
RUN make -j4
EXPOSE 56328
Expand Down
280 changes: 139 additions & 141 deletions trajectory_native/src/trajectory_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,10 @@

namespace vts = org::littletonrobotics::vehicletrajectoryservice;

// Scales the Choreo algorithm's estimate of initial guess points, since we've found it to sometimes underestimate.
// We take the tradeoff of increased computation time since we cache paths anyway.
static const double CONTROL_INTERVAL_GUESS_SCALAR = 1.5;

trajopt::SwerveDrivetrain create_drivetrain(const vts::VehicleModel &model) {
trajopt::SwerveDrivetrain drivetrain{
.mass = model.mass(),
Expand Down Expand Up @@ -55,7 +59,7 @@ int guess_control_interval_count(const vts::Waypoint &waypoint, const vts::Waypo
2 * (sqrt(distance * max_accel) / max_accel) :
distance / max_vel + max_vel / max_accel;
total_time += heading_weight * abs(dtheta);
return ceil(total_time / 0.1);
return ceil(total_time / 0.1 * CONTROL_INTERVAL_GUESS_SCALAR);
}

void convert_sample(vts::TimestampedVehicleState *state_out, const trajopt::HolonomicTrajectorySample &sample_in) {
Expand All @@ -76,161 +80,155 @@ void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::Holonomi
}

double angle_modulus(double value) {
double minInput = -std::numbers::pi;
double maxInput = std::numbers::pi;
double modulus = maxInput - minInput;

int numMax = (value - minInput) / modulus;
value -= numMax * modulus;
int numMin = (value - maxInput) / modulus;
value -= numMin * modulus;
return value;
}
double minInput = -std::numbers::pi;
double maxInput = std::numbers::pi;
double modulus = maxInput - minInput;

class VehicleTrajectoryService final
: public vts::VehicleTrajectoryService::Service {
public:
::grpc::Status GenerateTrajectory(::grpc::ServerContext *context,
const vts::PathRequest *request,
vts::TrajectoryResponse *response) override {
trajopt::SwervePathBuilder builder;
builder.SetDrivetrain(create_drivetrain(request->model()));
std::vector<size_t> control_intervals;

int segment_start_offset = 0;
int full_rots = 0;
double prev_heading = 0;
for (int segment_idx = 0; segment_idx < request->segments_size(); segment_idx++) {
fmt::print("Starting segment {}\n", segment_idx);
// Add segment waypoints
const vts::PathSegment &segment = request->segments(segment_idx);
const int last_waypoint_idx = segment.waypoints_size() - 1;
const vts::Waypoint *prev_waypoint;

if (segment.waypoints_size() == 0) {
response->mutable_error()->set_reason("Empty segment");
return grpc::Status::OK;
}
int numMax = (value - minInput) / modulus;
value -= numMax * modulus;
int numMin = (value - maxInput) / modulus;
value -= numMin * modulus;
return value;
}

for (int waypoint_idx = 0; waypoint_idx < segment.waypoints_size(); waypoint_idx++) {
const vts::Waypoint &waypoint = segment.waypoints(waypoint_idx);
int total_waypoint_idx = segment_start_offset + waypoint_idx;
if (segment_start_offset > 0) {
total_waypoint_idx++;
}
std::vector<vts::Waypoint> add_waypoints(trajopt::SwervePathBuilder &builder, const vts::PathRequest &request) {
int full_rots = 0;
double prev_heading = 0;
bool found_prev_heading = false;
int idx = 0;
std::vector<vts::Waypoint> all_wpts;

if (waypoint.has_heading_constraint()) {
if (total_waypoint_idx == 0) {
prev_heading = waypoint.heading_constraint();
}
double prev_heading_mod = angle_modulus(prev_heading);
double heading_mod = angle_modulus(waypoint.heading_constraint());
if (prev_heading_mod < 0 && heading_mod > prev_heading_mod + std::numbers::pi) {
full_rots--;
} else if (prev_heading_mod > 0 && heading_mod < prev_heading_mod - std::numbers::pi) {
full_rots++;
}
double heading = full_rots * 2 * std::numbers::pi + heading_mod;
for (const vts::PathSegment &segment: request.segments()) {
for (const vts::Waypoint &waypoint: segment.waypoints()) {
if (waypoint.has_heading_constraint()) {
if (!found_prev_heading) {
prev_heading = waypoint.heading_constraint();

fmt::print("Adding pose waypoint {} ({}, {}, {})\n", total_waypoint_idx,
waypoint.x(), waypoint.y(), heading);
builder.PoseWpt(total_waypoint_idx, waypoint.x(), waypoint.y(), heading);
} else {
fmt::print("Adding translation waypoint {} ({}, {})\n", total_waypoint_idx,
waypoint.x(), waypoint.y());
builder.TranslationWpt(total_waypoint_idx, waypoint.x(), waypoint.y());
}

switch (waypoint.velocity_constraint_case()) {
case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET:
// If this is the first or last waypoint, add an implicit stop if no other constraint is added
if ((segment_idx == 0 && waypoint_idx == 0) ||
(segment_idx == request->segments_size() - 1 && waypoint_idx == last_waypoint_idx)) {
fmt::print("Adding implicit zero velocity constraint to waypoint {}\n",
total_waypoint_idx);
builder.WptZeroVelocity(total_waypoint_idx);
builder.WptZeroAngularVelocity(total_waypoint_idx);
}
break;
case vts::Waypoint::kZeroVelocity:
fmt::print("Adding zero velocity constraint to waypoint {}\n",
total_waypoint_idx);
builder.WptZeroVelocity(total_waypoint_idx);
builder.WptZeroAngularVelocity(total_waypoint_idx);
break;
case vts::Waypoint::kVehicleVelocity:
fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n",
waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(),
waypoint.vehicle_velocity().omega(), total_waypoint_idx);
builder.WptConstraint(total_waypoint_idx,
trajopt::HolonomicVelocityConstraint{
trajopt::RectangularSet2d{
{waypoint.vehicle_velocity().vx(),
waypoint.vehicle_velocity().vx()},
{waypoint.vehicle_velocity().vy(),
waypoint.vehicle_velocity().vy()},
},
trajopt::CoordinateSystem::kField
});
builder.WptConstraint(total_waypoint_idx,
trajopt::AngularVelocityConstraint{
trajopt::IntervalSet1d{
waypoint.vehicle_velocity().omega(),
waypoint.vehicle_velocity().omega()
}
});
break;
found_prev_heading = true;
}

if (segment_idx > 0 || waypoint_idx > 0) {
unsigned int sample_count = waypoint.has_samples() ? waypoint.samples()
: guess_control_interval_count(waypoint,
*prev_waypoint,
request->model());
fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, total_waypoint_idx - 1,
total_waypoint_idx);
control_intervals.push_back(sample_count);
double prev_heading_mod = angle_modulus(prev_heading);
double heading_mod = angle_modulus(waypoint.heading_constraint());
if (prev_heading_mod < 0 && heading_mod > prev_heading_mod + std::numbers::pi) {
full_rots--;
} else if (prev_heading_mod > 0 && heading_mod < prev_heading_mod - std::numbers::pi) {
full_rots++;
}
double heading = full_rots * 2 * std::numbers::pi + heading_mod;
prev_heading = waypoint.heading_constraint();

prev_waypoint = &waypoint;
fmt::print("Adding pose waypoint {} ({}, {}, {})\n", idx, waypoint.x(), waypoint.y(), heading);
builder.PoseWpt(idx, waypoint.x(), waypoint.y(), heading);
} else {
fmt::print("Adding translation waypoint {} ({}, {})\n", idx, waypoint.x(), waypoint.y());
builder.TranslationWpt(idx, waypoint.x(), waypoint.y());
}

// Segment constraints
if (segment.has_max_velocity()) {
fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(),
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
segment.max_velocity());
}
all_wpts.push_back(waypoint);
idx++;
}
}

if (segment.has_max_omega()) {
fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(),
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
trajopt::AngularVelocityConstraint{segment.max_omega()});
}
return std::move(all_wpts);
}

void apply_waypoint_constraints(trajopt::SwervePathBuilder &builder, const std::vector<vts::Waypoint> &waypoints,
const vts::VehicleModel &model) {
std::vector<size_t> control_intervals;

for (int i = 0; i < waypoints.size(); i++) {
const vts::Waypoint &waypoint = waypoints.at(i);

if (segment.straight_line()) {
if (segment.waypoints_size() > 2) {
response->mutable_error()->set_reason("More than two waypoints in a straight segment");
return grpc::Status::OK;
switch (waypoint.velocity_constraint_case()) {
case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET:
// If this is the first or last waypoint, add an implicit stop if no other constraint is added
if (i == 0 || i == waypoints.size() - 1) {
fmt::print("Adding implicit zero velocity constraint to waypoint {}\n", i);
builder.WptZeroVelocity(i);
builder.WptZeroAngularVelocity(i);
}
double x1 = segment.waypoints(0).x();
double x2 = segment.waypoints(1).x();
double y1 = segment.waypoints(0).y();
double y2 = segment.waypoints(1).y();
double angle = atan2(y2 - y1, x2 - x1);
fmt::print("Adding straight line constraint with angle {} to segment {} (waypoints {}-{})\n", angle,
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
angle);
}
break;
case vts::Waypoint::kZeroVelocity:
fmt::print("Adding zero velocity constraint to waypoint {}\n", i);
builder.WptZeroVelocity(i);
builder.WptZeroAngularVelocity(i);
break;
case vts::Waypoint::kVehicleVelocity:
fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n",
waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(),
waypoint.vehicle_velocity().omega(), i);
builder.WptConstraint(i,
trajopt::HolonomicVelocityConstraint{
trajopt::RectangularSet2d{
{waypoint.vehicle_velocity().vx(),
waypoint.vehicle_velocity().vx()},
{waypoint.vehicle_velocity().vy(),
waypoint.vehicle_velocity().vy()},
},
trajopt::CoordinateSystem::kField
});
builder.WptConstraint(i,
trajopt::AngularVelocityConstraint{
trajopt::IntervalSet1d{
waypoint.vehicle_velocity().omega(),
waypoint.vehicle_velocity().omega()
}
});
break;
}

segment_start_offset += last_waypoint_idx;
// Apply sample count
if (i > 0) {
unsigned int sample_count = waypoint.has_samples() ? waypoint.samples()
: guess_control_interval_count(waypoint,
waypoints.at(i - 1),
model);
fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, i - 1, i);
control_intervals.push_back(sample_count);
}

builder.ControlIntervalCounts(std::move(control_intervals));
}
}

void apply_segment_constraints(trajopt::SwervePathBuilder &builder, const vts::PathRequest &request) {
int constraint_start_idx = -1;
for (int i = 0; i < request.segments_size(); i++) {
const vts::PathSegment &segment = request.segments(i);
int constraint_end_idx = constraint_start_idx + segment.waypoints_size();
if (constraint_start_idx < 0) {
// First segment starts from the first waypoint since there is no segment behind it.
constraint_start_idx = 0;
}

if (segment.has_max_velocity()) {
fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(),
i, constraint_start_idx, constraint_end_idx);
builder.SgmtVelocityMagnitude(constraint_start_idx, constraint_end_idx,
segment.max_velocity());
}

if (segment.has_max_omega()) {
fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(),
i, constraint_start_idx, constraint_end_idx);
builder.SgmtConstraint(constraint_start_idx, constraint_end_idx,
trajopt::AngularVelocityConstraint{segment.max_omega()});
}

constraint_start_idx = constraint_end_idx;
}
}

class VehicleTrajectoryService final
: public vts::VehicleTrajectoryService::Service {
public:
::grpc::Status GenerateTrajectory(::grpc::ServerContext *context,
const vts::PathRequest *request,
vts::TrajectoryResponse *response) override {
trajopt::SwervePathBuilder builder;
builder.SetDrivetrain(create_drivetrain(request->model()));

auto all_waypoints = add_waypoints(builder, *request);
apply_waypoint_constraints(builder, all_waypoints, request->model());
apply_segment_constraints(builder, *request);

try {
fmt::print("Generating trajectory\n");
Expand Down Expand Up @@ -259,4 +257,4 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv) {
server->Wait();

return 0;
}
}
Loading