Implements first version of MPPI for collision avoidance

Author: haiot4105

.gitignore

@@ -1,3 +1,4 @@
 /cmake-build-debug/
 /cmake-build-release/
-/.idea/
+/.idea/
+CMakeLists.txt.user

CMakeLists.txt

@@ -1,18 +1,16 @@
-cmake_minimum_required(VERSION 3.24)
+cmake_minimum_required(VERSION 3.20)
 project(mppi_ca_cpp)
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
 
-#add_definitions(-DHAVE_CBLAS=1)
-#add_definitions(-DUSE_CBLAS -DUSE_LAPACK)
 add_compile_options(-lcblas)
 add_compile_options(-lopenblas)
 
 find_package(Python REQUIRED COMPONENTS Interpreter Development NumPy)
 find_package(BLAS REQUIRED)
 find_package(LAPACK REQUIRED)
-#find_package(CBLAS  REQUIRED)
+find_package(ortools CONFIG REQUIRED)
 
 message(STATUS "BLAS VENDOR:    " ${BLA_VENDOR})
 message(STATUS "BLAS LIBRARIES: " ${BLAS_LIBRARIES})
@@ -31,8 +29,9 @@ add_subdirectory(external/pybind11)
 
 add_library(controller_library SHARED
         src/controller.cpp
-        src/diff_drive.cpp
-        src/geometry.cpp)
+        src/dynamics.cpp
+        src/geometry.cpp
+        src/orca_lines.cpp)
 
 
 target_include_directories(controller_library
@@ -43,7 +42,6 @@ target_include_directories(controller_library
 target_compile_definitions(controller_library PUBLIC -DUSE_CBLAS)
 
 target_link_libraries(controller_library ${LAPACK_LIBRARIES})
-#target_link_libraries(controller_library ${BLAS_LIBRARIES})
 
 target_link_libraries(controller_library xtensor)
 target_link_libraries(controller_library xtensor-blas)
@@ -52,15 +50,10 @@ target_link_libraries(controller_library xsimd)
 
 pybind11_add_module(mppi_ca_cpp python_bind/converters.hpp python_bind/converters.cpp python_bind/mppi_ca_cpp.cpp NO_EXTRAS)
 target_link_libraries(mppi_ca_cpp PRIVATE xtensor-python Python::NumPy)
-target_link_libraries(mppi_ca_cpp PRIVATE controller_library)
-
-add_executable(mppi_test src/main.cpp)
-target_link_libraries(mppi_test PRIVATE controller_library)
+target_link_libraries(mppi_ca_cpp PRIVATE controller_library ortools::ortools)
 
 
 add_custom_target(mppi_all)
 add_dependencies(mppi_all
         controller_library
-        mppi_test
-        mppi_ca_cpp
-        )
+        mppi_ca_cpp)

README.md

@@ -0,0 +1,4 @@
+# Model Predictive Path Integral for Multi-Agent Collision Avoidance
+
+A detailed description of the ***MPPI for collision avoidance*** algorithm, 
+corresponding parameters and build will be given here soon.

include/controller.hpp

@@ -11,10 +11,16 @@
 #include <iostream>
 #include <ctime>
 #include <limits>
+#include <cmath>
+#include <memory>
+#include <random>
+
+#include "ortools/linear_solver/linear_solver.h"
 
 #include "consts.hpp"
-#include "diff_drive.hpp"
+#include "dynamics.hpp"
 #include "geometry.hpp"
+#include "orca_lines.hpp"
 
 namespace mppica {
 	using std::cout;
@@ -38,16 +44,9 @@ namespace mppica {
 		double ma_dist_weight = 1000.0;
 		double ma_collision_value = 10000.0;
 		double ma_dist_threshold = 2.0;
-
-		size_t max_safe_distr_step; // TODO
-		size_t max_bvc_cost_step; // TODO
-		double bvc_weight; // TODO
-		double bvc_w_base; // TODO
-		double bvc_w_mean; // TODO
-		double bvc_w_std; // TODO
-
-		bool importance_sampl_en; // TODO importance sampling
-		double alpha; // TODO sampling from zero alpha % of batches
+		int max_safe_distr_step = 0;
+		bool car_like_dyn = false;
+        double alpha; // TODO: Sampling with initial mean value in alpha% of batches
 	};
 
 	struct AgentParams {
@@ -60,15 +59,13 @@ namespace mppica {
 		double dd_w_min = -2.0;
 	};
 
-//	xt::xtensor<double, 2> control_limits;
-
 
 	class Controller {
 		public:
 			Controller();
 
 			auto nextStep(const xt::xtensor<double, 1> &curr_state, const xt::xtensor<double, 2> &neighbors_states,
-						  const xt::xtensor<double, 1> &goal_state) -> xt::xtensor<double, 1>;
+						  const xt::xtensor<double, 1> &goal_state) -> std::tuple<xt::xtensor<double, 1>, xt::xtensor<double, 3>>;
 
 			void setAgentParams(AgentParams ag_params);
 
@@ -118,18 +115,31 @@ namespace mppica {
 
 			void multiAgentCost(size_t batch, size_t time_step, const xt::xtensor<double, 1> &goal_state);
 
+			void computeLinearConstraintsForState(const xt::xtensor<double, 1> &state,
+												  const xt::xtensor<double, 2> &neighbors_states);
+
+			void resampleInitialWithConstraints();
+
+
+			std::tuple<bool, double, double> computeNewDistr(const xt::xtensor<double, 1> &state, const xt::xtensor<double, 1> &control);
+
+			std::tuple<xt::xtensor<double, 1>, xt::xtensor<double, 1>> translateConstraintsToControlBounds(const xt::xtensor<double, 1> &state);
 
 			bool computationShouldStop(); // TODO
 			void updateStopComputationConditions(); // TODO
 			void computeMainTrajectory(); // TODO
 
 
+			size_t step; // For debug purpuses
+			size_t controller_id; // For debug purpuses
+
 			bool ma_cost_en;
 			size_t max_neighbors_num;
 			size_t neighbors_num;
 			double ma_dist_weight;
 			double ma_collision_value;
 			double ma_dist_threshold;
+			int max_safe_distr_step;
 
 			size_t batch_size;
 			size_t time_steps;
@@ -148,17 +158,26 @@ namespace mppica {
 			xt::xtensor<double, 2> control_sequence;
 			xt::xtensor<double, 3> generated_trajectories;
 			xt::xtensor<double, 1> traj_costs;
-
 			xt::xtensor<double, 3> neighbors_traj;
+			xt::xtensor<double, 2> linear_constraints;
+			std::vector<ORCALine> orca_lines;
+			bool orca_complete = true;
+
+
 
 			double common_run_cost_weight;
 			double common_terminal_cost_weight;
 
 			double agent_size;
 			double vis_radius;
+			double rad_eps;
 
+			bool use_car_like_dyn;
 
 			DiffDrive dyn_model; //TODO Generalize for other models
+			CarLike car_like_dyn;
+
+
 	};
 
 }

include/diff_drive.hpp include/dynamics.hppinclude/diff_drive.hpp renamed to include/dynamics.hpp

@@ -13,13 +13,25 @@
 #define MPPI_CA_CPP_DIFF_DRIVE_HPP
 
 
-class DiffDrive
-{
+class DiffDrive {
 	public:
 		auto shift(const xt::xtensor<double, 1> &state, double dt) -> xt::xtensor<double, 1>;
-		auto dynamic(const xt::xtensor<double, 1> &state, const xt::xtensor<double, 1> &control, double dt) -> xt::xtensor<double, 1>;
 
+		auto dynamic(const xt::xtensor<double, 1> &state, const xt::xtensor<double, 1> &control,
+					 double dt) -> xt::xtensor<double, 1>;
 
+
+};
+
+
+class CarLike {
+	public:
+		auto shift(const xt::xtensor<double, 1> &state, double dt) -> xt::xtensor<double, 1>;
+
+		auto dynamic(const xt::xtensor<double, 1> &state, const xt::xtensor<double, 1> &control,
+					 double dt) -> xt::xtensor<double, 1>;
+
+		double dist_between_axles;
 };
 
 

include/geometry.hpp

@@ -10,9 +10,14 @@
 #ifndef MPPI_CA_CPP_GEOMETRY_HPP
 #define MPPI_CA_CPP_GEOMETRY_HPP
 
-double point_to_line_segment_distance(const xt::xtensor<double, 1> &line_p1,
-									  const xt::xtensor<double, 1> &line_p2,
-									  const xt::xtensor<double, 1> &point);
+double pointToLineSegmentDistance(const xt::xtensor<double, 1> &line_p1,
+								  const xt::xtensor<double, 1> &line_p2,
+								  const xt::xtensor<double, 1> &point);
+
+double determinant2D(const xt::xtensor<double, 1> &ab, const xt::xtensor<double, 1> &cd);
+
+
+std::tuple<bool, xt::xtensor<double, 1>> lineIntersection(const xt::xtensor<double, 1> &line1, const xt::xtensor<double, 1> &line2);
 
 
 #endif //MPPI_CA_CPP_GEOMETRY_HPP

include/orca_lines.hpp

@@ -0,0 +1,52 @@
+#ifndef MPPI_CA_CPP_ORCA_LINES_HPP
+#define MPPI_CA_CPP_ORCA_LINES_HPP
+
+#include <xtensor/xtensor.hpp>
+#include <xtensor/xarray.hpp>
+#include <xtensor/xview.hpp>
+#include <xtensor/xfixed.hpp>
+#include <xtensor/xio.hpp>
+#include <xtensor/xrandom.hpp>
+#include <xtensor-blas/xlinalg.hpp>
+#include <iostream>
+#include <ctime>
+#include <limits>
+
+#include "consts.hpp"
+#include "geometry.hpp"
+
+
+struct ORCALine {
+	xt::xtensor<double, 1> dir = xt::zeros<double>({2});
+	xt::xtensor<double, 1> point_lies_on = xt::zeros<double>({2});;
+};
+
+struct ORCAParams {
+	size_t agents_max_num;
+	double agent_size;
+	double time_boundary;
+	double time_step;
+	double responsibility_factor = 0.5;
+	bool opt_zero_vel = true;
+};
+
+std::vector<ORCALine> computeORCALines(ORCAParams params, xt::xtensor<double, 1> agent_state, xt::xtensor<double, 2> neighbors_states,
+					  xt::xtensor<double, 1> neighbors_sizes);
+
+xt::xtensor<double, 2> computeORCALinesXT(size_t agents_max_num,
+										  double agent_size,
+										  double time_boundary,
+										  double time_step,
+										  double responsibility_factor,
+										  bool opt_zero_vel,
+										  xt::xtensor<double, 1> agent_state,
+										  xt::xtensor<double, 2> neighbors_states,
+										  xt::xtensor<double, 1> neighbors_sizes);
+
+
+bool linearProgram1(const std::vector<ORCALine> &lines, unsigned long curr, float radius, const xt::xtensor<double, 1> &opt_velocity,
+					bool direction_opt, xt::xtensor<double, 1> &result);
+
+unsigned long int linearProgram2(const std::vector<ORCALine> &lines, double radius, const xt::xtensor<double, 1> &opt_velocity, bool direction_opt,
+								 xt::xtensor<double, 1> &result);
+#endif //MPPI_CA_CPP_ORCA_LINES_HPP

python_bind/converters.cpp

@@ -35,6 +35,8 @@ mppica::MPPIParams convertAlgParams(const py::dict &params_dict) {
 	params.ma_dist_weight = params_dict["ma_dist_weight"].cast<double>();
 	params.ma_collision_value = params_dict["ma_collision_weight"].cast<double>();
 	params.ma_dist_threshold = params_dict["ma_dist_threashhold"].cast<double>();
+	params.max_safe_distr_step = params_dict["max_safe_distr_step"].cast<int>();
+	params.car_like_dyn = params_dict["car_like_dyn"].cast<bool>();
 
 	return params;
 }

python_bind/converters.hpp

@@ -1,10 +1,12 @@
-#include "../include/controller.hpp"
-#include <pybind11/pybind11.h>
-#include "xtensor-python/pyarray.hpp"
+
 
 #ifndef MPPI_CA_CPP_PYTHON_CONTROLLER_H
 #define MPPI_CA_CPP_PYTHON_CONTROLLER_H
 
+#include "../include/controller.hpp"
+#include <pybind11/pybind11.h>
+#include "xtensor-python/pyarray.hpp"
+
 namespace py = pybind11;
 
 mppica::AgentParams convertAgentParams(const py::dict& params_dict);

python_bind/mppi_ca_cpp.cpp

@@ -2,6 +2,7 @@
 #define FORCE_IMPORT_ARRAY
 #include <xtensor-python/pytensor.hpp>
 #include "../include/controller.hpp"
+#include "../include/orca_lines.hpp"
 #include "converters.hpp"
 
 namespace py = pybind11;
@@ -20,14 +21,6 @@ PYBIND11_MODULE(mppi_ca_cpp, m) {
 
 	m.def("convert_agent_params", &convertAgentParams, "Coverts agent params");
 	m.def("convert_alg_params", &convertAlgParams, "Coverts alg parameters");
+	m.def("compute_orca_lines", &computeORCALinesXT, "Computes ORCA lines for agents");
 }
 
-
-//PYBIND11_MODULE(bind_module, m) {
-//	xt::import_numpy();
-//	py::class_<Controller>(m, "Controller")
-//			.def(py::init<>())
-//			.def("next_step", &Controller::nextStep);
-//}
-
-