diff --git a/mader/include/mader_types.hpp b/mader/include/mader_types.hpp index 5c067ea2..8a4db489 100644 --- a/mader/include/mader_types.hpp +++ b/mader/include/mader_types.hpp @@ -629,6 +629,8 @@ struct parameters double alpha = 0.0; double beta = 0.0; double gamma = 0.5; + + bool use_linear_collision_constraints; }; typedef std::vector trajectory; diff --git a/mader/include/solver_gurobi.hpp b/mader/include/solver_gurobi.hpp index 65c3cd4f..9aed6e03 100644 --- a/mader/include/solver_gurobi.hpp +++ b/mader/include/solver_gurobi.hpp @@ -195,5 +195,13 @@ class SolverGurobi OctopusSearch *octopusSolver_; double Ra_ = 1e10; + + bool use_linear_collision_constraints_ = true; + + std::vector d_exp_; // the scalar parameter of the plane + std::vector> n_exp_; // Each n_exp_[i] has 3 elements (x,y,z) + int solutions_found_ = 0; + int total_runs_ = 0; + double time_total_ms_ = 0.0; }; #endif \ No newline at end of file diff --git a/mader/include/solver_params.hpp b/mader/include/solver_params.hpp index dcd134a4..6a9212a7 100644 --- a/mader/include/solver_params.hpp +++ b/mader/include/solver_params.hpp @@ -37,6 +37,7 @@ struct par_solver double Ra; double alpha_shrink; + bool use_linear_collision_constraints; }; } // namespace ms diff --git a/mader/param/mader.yaml b/mader/param/mader.yaml index cf91f362..956dc4c8 100644 --- a/mader/param/mader.yaml +++ b/mader/param/mader.yaml @@ -72,6 +72,7 @@ alpha: 0.0 #[m] Error in position beta: 0.0 #[m] Deviation between the trajectory and the segment between two discretization points gamma: 0.1 #[seconds] >0 Time step between discretization points +use_linear_collision_constraints: true ##### Not used right now in the MADER algorithm fov_horiz_deg: 100 #[deg] \in (0,180] , angle between two faces of the tetrahedron diff --git a/mader/src/mader.cpp b/mader/src/mader.cpp index 03b775ee..78d61076 100644 --- a/mader/src/mader.cpp +++ b/mader/src/mader.cpp @@ -92,6 +92,7 @@ Mader::Mader(mt::parameters par) : par_(par) par_for_solver.a_star_bias = par_.a_star_bias; par_for_solver.allow_infeasible_guess = par_.allow_infeasible_guess; par_for_solver.alpha_shrink = par_.alpha_shrink; + par_for_solver.use_linear_collision_constraints = par_.use_linear_collision_constraints; mt::basisConverter basis_converter; diff --git a/mader/src/mader_ros.cpp b/mader/src/mader_ros.cpp index ca859424..b02e3051 100644 --- a/mader/src/mader_ros.cpp +++ b/mader/src/mader_ros.cpp @@ -94,6 +94,7 @@ MaderRos::MaderRos(ros::NodeHandle nh1, ros::NodeHandle nh2, ros::NodeHandle nh3 mu::safeGetParam(nh1_, "fov_horiz_deg", par_.fov_horiz_deg); mu::safeGetParam(nh1_, "fov_vert_deg", par_.fov_vert_deg); mu::safeGetParam(nh1_, "fov_depth", par_.fov_depth); + mu::safeGetParam(nh1_, "use_linear_collision_constraints", par_.use_linear_collision_constraints); std::cout << "Parameters obtained" << std::endl; diff --git a/mader/src/solver_gurobi.cpp b/mader/src/solver_gurobi.cpp index 7b78c80c..7d3699d3 100644 --- a/mader/src/solver_gurobi.cpp +++ b/mader/src/solver_gurobi.cpp @@ -94,6 +94,8 @@ SolverGurobi::SolverGurobi(ms::par_solver &par) separator_solver_ = new separator::Separator(); octopusSolver_ = new OctopusSearch(par.basis, num_pol_, deg_pol_, par.alpha_shrink); + + use_linear_collision_constraints_ = par.use_linear_collision_constraints; } SolverGurobi::~SolverGurobi() @@ -126,7 +128,7 @@ void SolverGurobi::addObjective() void SolverGurobi::addConstraints() { - // double epsilon = 1.0; // See http://www.joyofdata.de/blog/testing-linear-separability-linear-programming-r-glpk/ + double epsilon = 1.0; // See http://www.joyofdata.de/blog/testing-linear-separability-linear-programming-r-glpk/ addVectorEqConstraint(m_, q_exp_[0], q0_); addVectorEqConstraint(m_, q_exp_[1], q1_); @@ -156,28 +158,56 @@ void SolverGurobi::addConstraints() /////// Plane constraints ///////////////////////////////////////////////// - for (int obst_index = 0; obst_index < num_of_obst_; obst_index++) + if (use_linear_collision_constraints_) { - int ip = obst_index * num_of_segments_ + i; // index plane + for (int obst_index = 0; obst_index < num_of_obst_; obst_index++) + { + int ip = obst_index * num_of_segments_ + i; // index plane - // impose that all the vertexes of the obstacle are on one side of the plane - // for (int j = 0; j < hulls_[obst_index][i].cols(); j++) // Eigen::Vector3d vertex : hulls_[obst_index][i] - // { - // Eigen::Vector3d vertex = hulls_[obst_index][i].col(j); - // m_.addConstr((-(n_[ip].dot(vertex) + d_[ip] - epsilon)) <= 0, "plane" + std::to_string(j)); - // // constraints[r] = -(n[ip].dot(vertex) + d[ip] - epsilon); // f<=0 - // } + // impose that all the vertexes of the obstacle are on one side of the plane + // for (int j = 0; j < hulls_[obst_index][i].cols(); j++) // Eigen::Vector3d vertex : hulls_[obst_index][i] + // { + // Eigen::Vector3d vertex = hulls_[obst_index][i].col(j); + // m_.addConstr((-(n_[ip].dot(vertex) + d_[ip] - epsilon)) <= 0, "plane" + std::to_string(j)); + // // constraints[r] = -(n[ip].dot(vertex) + d[ip] - epsilon); // f<=0 + // } - // and the control points on the other side + // and the control points on the other side - for (int u = 0; u <= 3; u++) + for (int u = 0; u <= 3; u++) + { + GRBVector q_ipu = getColumn(Qmv, u); + GRBLinExpr dot = n_[ip].x() * q_ipu[0] + n_[ip].y() * q_ipu[1] + n_[ip].z() * q_ipu[2]; + m_.addConstr(dot + d_[ip] - 1 <= 0); + } + } + } + else + { + for (int obst_index = 0; obst_index < num_of_obst_; obst_index++) { - GRBVector q_ipu = getColumn(Qmv, u); - GRBLinExpr dot = n_[ip].x() * q_ipu[0] + n_[ip].y() * q_ipu[1] + n_[ip].z() * q_ipu[2]; - m_.addConstr(dot + d_[ip] - 1 <= 0); - } + int ip = obst_index * num_of_segments_ + i; // index plane + + // impose that all the vertexes of the obstacle are on one side of the plane + for (int j = 0; j < hulls_[obst_index][i].cols(); j++) // Eigen::Vector3d vertex : hulls_[obst_index][i] + { + Eigen::Vector3d vertex = hulls_[obst_index][i].col(j); + m_.addConstr(n_exp_[ip][0] * vertex(0) + n_exp_[ip][1] * vertex(1) + + n_exp_[ip][2] * vertex(2) + d_exp_[ip] - epsilon >= 0); + } + + // and the control points on the other side + + for (int u = 0; u <= 3; u++) + { + GRBVector q_ipu = getColumn(Qmv, u); + GRBQuadExpr dot = n_exp_[ip][0] * q_ipu[0] + n_exp_[ip][1] * q_ipu[1] + n_exp_[ip][2] * q_ipu[2]; + m_.addQConstr(dot + d_exp_[ip] + epsilon <= 0); + } + } } + /////// Sphere constraints /////////////////////////////////////////////// Eigen::Vector3d q_ipu; @@ -445,6 +475,30 @@ void SolverGurobi::setHulls(mt::ConvexHullsOfCurves_Std &hulls) int num_of_cpoints = N_ + 1; num_of_normals_ = num_of_segments_ * num_of_obst_; + + d_exp_.clear(); + n_exp_.clear(); + + if (!use_linear_collision_constraints_) + { + for (int obst_index = 0; obst_index < num_of_obst_; obst_index++) + { + for (int seg_index = 0; seg_index < num_of_segments_; seg_index++) + { + std::vector n_seg; + n_seg.push_back(GRBLinExpr(m_.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, + "nx_" + std::to_string(obst_index) + std::to_string(seg_index)))); + n_seg.push_back(GRBLinExpr(m_.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, + "ny_" + std::to_string(obst_index) + std::to_string(seg_index)))); + n_seg.push_back(GRBLinExpr(m_.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, + "nz_" + std::to_string(obst_index) + std::to_string(seg_index)))); + n_exp_.push_back(n_seg); + + d_exp_.push_back(GRBLinExpr(m_.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, + "d_" + std::to_string(obst_index) + std::to_string(seg_index)))); + } + } + } } ////////////////////////////////////////////////////////// @@ -626,13 +680,27 @@ bool SolverGurobi::optimize() m_.set("OutputFlag", std::to_string(0)); // verbose (1) or not (0) // See https://www.gurobi.com/documentation/9.0/refman/cpp_parameter_examples.html m_.set("TimeLimit", std::to_string(mu_ * max_runtime_)); + + if (!use_linear_collision_constraints_) + { + m_.set("NonConvex", std::to_string(2)); + } + + addObjective(); addConstraints(); m_.update(); // needed due to the lazy evaluation // m_.write("/home/jtorde/Desktop/ws/src/mader/model.lp"); std::cout << "Starting optimization, allowing time = " << mu_ * max_runtime_ * 1000 << " ms" << std::endl; + // opt_timer_.Reset(); + m_.optimize(); + total_runs_++; + // std::cout< q; + // optimstatus == GRB_SUBOPTIMAL //Don't include this one (sometimes it generates very long/suboptimal // trajectories) @@ -652,12 +721,29 @@ bool SolverGurobi::optimize() number_of_stored_solutions > 0) { + solutions_found_++; std::cout << green << "Gurobi found a solution" << reset << std::endl; // copy the solution for (auto tmp : q_exp_) { q.push_back(Eigen::Vector3d(tmp[0].getValue(), tmp[1].getValue(), tmp[2].getValue())); } + + if (!use_linear_collision_constraints_) + { + n_.clear(); + d_.clear(); + + for (auto tmp : n_exp_) + { + n_.push_back(Eigen::Vector3d(tmp[0].getValue(), tmp[1].getValue(), tmp[2].getValue())); + } + for (auto tmp : d_exp_) + { + d_.push_back(tmp.getValue()); + } + } + } else { @@ -676,7 +762,8 @@ bool SolverGurobi::optimize() // Uncomment the following line if you wanna visualize the planes // fillPlanesFromNDQ(n_, d_, q); // TODO: move this outside the SolverGurobi class - + // std::cout << on_cyan << bold << "GUROBI Solved so far" << solutions_found_ << "/" << total_runs_ << reset + // << std::endl; return true; }