From cac42402e05c4df256dbf9b8f8cb7b9c6c850c92 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Fri, 9 Feb 2024 18:11:26 -0500 Subject: [PATCH] Fix build for vts --- trajectory_native/CMakeLists.txt | 3 +- trajectory_native/Earthfile | 27 ++----- trajectory_native/src/trajectory_service.cpp | 85 +++++++++++++++++--- trajectory_native/trajoptlib-aarch64.patch | 22 +++++ 4 files changed, 106 insertions(+), 31 deletions(-) create mode 100644 trajectory_native/trajoptlib-aarch64.patch diff --git a/trajectory_native/CMakeLists.txt b/trajectory_native/CMakeLists.txt index 9d4af1d7..d0560559 100644 --- a/trajectory_native/CMakeLists.txt +++ b/trajectory_native/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(gRPC CONFIG REQUIRED) find_package(Threads) find_package(fmt REQUIRED) find_package(nlohmann_json REQUIRED) +find_package(TrajoptLib REQUIRED) add_library(trajectory_native_proto proto/VehicleTrajectoryService.proto) target_link_libraries(trajectory_native_proto PUBLIC protobuf::libprotobuf gRPC::grpc gRPC::grpc++ gRPC::grpc++_reflection) @@ -17,7 +18,7 @@ target_include_directories(trajectory_native_proto PUBLIC ${CMAKE_CURRENT_BINARY get_target_property(grpc_cpp_plugin_location gRPC::grpc_cpp_plugin LOCATION) protobuf_generate(TARGET trajectory_native_proto LANGUAGE cpp) protobuf_generate(TARGET trajectory_native_proto LANGUAGE grpc GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc PLUGIN "protoc-gen-grpc=${grpc_cpp_plugin_location}") - + find_library(TRAJOPT_LIBRARY NAMES TrajoptLib) message(${TRAJOPT_LIBRARY}) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index d8aadddc..f18e3ee7 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -21,28 +21,15 @@ grpc: RUN mv installroot_top/usr/local/* installroot/ SAVE ARTIFACT installroot -ipopt: - FROM +apt-deps - RUN wget https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz - RUN tar -zvxf 3.14.14.tar.gz - RUN mkdir Ipopt-releases-3.14.14/build - WORKDIR Ipopt-releases-3.14.14/build - ENV CC=clang - ENV CXX=clang++ - RUN ../configure - RUN make -j4 - RUN make DESTDIR=$(pwd)/installroot_top install - RUN mkdir installroot - RUN mv installroot_top/usr/local/* installroot/ - SAVE ARTIFACT installroot - trajoptlib: FROM +apt-deps - BUILD +ipopt - COPY +ipopt/installroot /usr/local/ - GIT CLONE https://github.com/camearle20/TrajoptLib TrajoptLib - RUN mkdir TrajoptLib/build - WORKDIR TrajoptLib/build + # Latest commit seems to break casadi at least on arm64 + GIT CLONE --branch 95c20be79be7557673d75d631703dc92fe6a191e https://github.com/SleipnirGroup/TrajoptLib TrajoptLib + WORKDIR TrajoptLib + COPY trajoptlib-aarch64.patch . + RUN git apply trajoptlib-aarch64.patch + RUN mkdir build + WORKDIR build ENV CC=clang ENV CXX=clang++ RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF .. diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp index 065e6bcc..057fe509 100644 --- a/trajectory_native/src/trajectory_service.cpp +++ b/trajectory_native/src/trajectory_service.cpp @@ -14,23 +14,23 @@ trajopt::SwerveDrivetrain create_drivetrain(const vts::VehicleModel &model) { trajopt::SwerveDrivetrain drivetrain{ .mass = model.mass(), .moi = model.moi(), - .modules = {{.x = model.vehicle_length(), - .y = model.vehicle_width(), + .modules = {{.x = model.vehicle_length() / 2.0, + .y = model.vehicle_width() / 2.0, .wheelRadius = model.wheel_radius(), .wheelMaxAngularVelocity = model.max_wheel_omega(), .wheelMaxTorque = model.max_wheel_torque()}, - {.x = model.vehicle_length(), - .y = -model.vehicle_width(), + {.x = model.vehicle_length() / 2.0, + .y = -model.vehicle_width() / 2.0, .wheelRadius = model.wheel_radius(), .wheelMaxAngularVelocity = model.max_wheel_omega(), .wheelMaxTorque = model.max_wheel_torque()}, - {.x = -model.vehicle_length(), - .y = model.vehicle_width(), + {.x = -model.vehicle_length() / 2.0, + .y = model.vehicle_width() / 2.0, .wheelRadius = model.wheel_radius(), .wheelMaxAngularVelocity = model.max_wheel_omega(), .wheelMaxTorque = model.max_wheel_torque()}, - {.x = -model.vehicle_length(), - .y = -model.vehicle_width(), + {.x = -model.vehicle_length() / 2.0, + .y = -model.vehicle_width() / 2.0, .wheelRadius = model.wheel_radius(), .wheelMaxAngularVelocity = model.max_wheel_omega(), .wheelMaxTorque = model.max_wheel_torque()}}}; @@ -74,6 +74,65 @@ void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::Holonomi } } +std::string hash_request(const vts::PathRequest &request) { + // Function is somewhat messy but this is the simplest way I could think of to hash the request with + // fixed precision while accounting for optional fields + + std::stringstream stream; + stream << std::fixed << std::setprecision(6); + stream << request.model().mass(); + stream << request.model().moi(); + stream << request.model().vehicle_length(); + stream << request.model().vehicle_width(); + stream << request.model().wheel_radius(); + stream << request.model().max_wheel_omega(); + stream << request.model().max_wheel_torque(); + + for (const vts::PathSegment &segment: request.segments()) { + for (const vts::Waypoint &waypoint: segment.waypoints()) { + stream << waypoint.x(); + stream << waypoint.y(); + + if (waypoint.has_heading_constraint()) { + stream << waypoint.heading_constraint(); + } + + if (waypoint.has_samples()) { + stream << waypoint.samples(); + } + + switch (waypoint.velocity_constraint_case()) { + case vts::Waypoint::kZeroVelocity: + stream << "0"; + break; + case vts::Waypoint::kVehicleVelocity: + stream << waypoint.vehicle_velocity().vx(); + stream << waypoint.vehicle_velocity().vy(); + stream << waypoint.vehicle_velocity().omega(); + break; + case org::littletonrobotics::vehicletrajectoryservice::Waypoint::VELOCITY_CONSTRAINT_NOT_SET: + break; + } + } + + if (segment.has_max_velocity()) { + stream << segment.max_velocity(); + } + + if (segment.has_max_omega()) { + stream << segment.max_omega(); + } + + if (segment.straight_line()) { + stream << "1"; + } else { + stream << "0"; + } + } + + return std::to_string(std::hash{}(stream.str())); +} + class VehicleTrajectoryService final : public vts::VehicleTrajectoryService::Service { public: @@ -134,6 +193,13 @@ class VehicleTrajectoryService final }, trajopt::CoordinateSystem::kField }); + builder.WptConstraint(segment_start_offset + waypoint_idx, + trajopt::AngularVelocityConstraint{ + trajopt::IntervalSet1d{ + waypoint.vehicle_velocity().omega(), + waypoint.vehicle_velocity().omega() + } + }); break; } @@ -187,8 +253,7 @@ class VehicleTrajectoryService final trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)}; fmt::print("Generation finished\n"); convert_trajectory(response->mutable_trajectory(), trajectory); - std::size_t hash = std::hash{}(request->SerializeAsString()); - response->mutable_trajectory()->set_hash_code(std::to_string(hash)); + response->mutable_trajectory()->set_hash_code(hash_request(*request)); } catch (std::exception &e) { fmt::print("Generation failed: {}\n", std::string(e.what())); response->mutable_error()->set_reason(std::string(e.what())); diff --git a/trajectory_native/trajoptlib-aarch64.patch b/trajectory_native/trajoptlib-aarch64.patch new file mode 100644 index 00000000..bb118f65 --- /dev/null +++ b/trajectory_native/trajoptlib-aarch64.patch @@ -0,0 +1,22 @@ +diff --git a/cmake/modules/FetchCasADi.cmake b/cmake/modules/FetchCasADi.cmake +index 631f55f..ccb4511 100644 +--- a/cmake/modules/FetchCasADi.cmake ++++ b/cmake/modules/FetchCasADi.cmake +@@ -68,7 +68,7 @@ macro(fetch_casadi) + endif() + set(CASADI_INSTALL_DEST "lib") + elseif(UNIX) +- if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "ARM64") ++ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") + message(STATUS "Building for Linux arm64") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-linux-aarch64-py311.zip +@@ -79,8 +79,6 @@ macro(fetch_casadi) + ${CASADI_LIBDIR}/libipopt.so.3 + ${CASADI_LIBDIR}/libcoinmumps.so.3 + ${CASADI_LIBDIR}/libcoinmetis.so.2 +- ${CASADI_LIBDIR}/libgfortran-040039e1.so.5.0.0 +- ${CASADI_LIBDIR}/libquadmath-96973f99.so.0.0.0 + ${CASADI_LIBDIR}/libcasadi-tp-openblas.so.0 + ) + else()