From 21a277c9410ee3a12be65fb6da30335987f428eb Mon Sep 17 00:00:00 2001 From: SimonRohou Date: Mon, 12 Feb 2024 12:53:21 +0100 Subject: [PATCH] [tuto] added Wall class + CtcUnion binding --- .../core/contractors/static/codac_py_Ctc.cpp | 34 +++++++- .../src/core/graphics/codac_py_VIBesFig.cpp | 4 + python/src/robotics/codac_py_Wall.cpp | 42 ++++++++++ .../contractors/static/codac_CtcSegment.cpp | 8 +- .../contractors/static/codac_CtcSegment.h | 8 +- src/core/contractors/static/codac_CtcUnion.h | 69 +++++++++++++++- src/robotics/CMakeLists.txt | 2 + src/robotics/objects/codac_Wall.cpp | 82 +++++++++++++++++++ src/robotics/objects/codac_Wall.h | 37 +++++++++ 9 files changed, 275 insertions(+), 11 deletions(-) create mode 100644 python/src/robotics/codac_py_Wall.cpp create mode 100644 src/robotics/objects/codac_Wall.cpp create mode 100644 src/robotics/objects/codac_Wall.h diff --git a/python/src/core/contractors/static/codac_py_Ctc.cpp b/python/src/core/contractors/static/codac_py_Ctc.cpp index d798bf5e..8d2c1ec3 100644 --- a/python/src/core/contractors/static/codac_py_Ctc.cpp +++ b/python/src/core/contractors/static/codac_py_Ctc.cpp @@ -49,8 +49,11 @@ py::class_ export_Ctc(py::module& m) .def("__or__", [](Ctc& c1, Ctc& c2) { - return new CtcUnion(c1, c2); + CtcUnion *cu = new CtcUnion(2); + cu->add_raw_ptr(&c1); + cu->add_raw_ptr(&c2); // todo: manage delete + return cu; }, DOC_CTC_OR, py::return_value_policy::take_ownership, @@ -77,9 +80,34 @@ py::class_ export_Ctc(py::module& m) ; // Export CtcUnion - py::class_(m, "CtcUnion", ctc, DOC_CTCUNION_TYPE) - .def(py::init>(), py::keep_alive<1,2>(), "list"_a) + py::class_(m, "CtcUnion", ctc, "todo") + .def(py::init(), "nb_var"_a) + .def(py::init([](Ctc& c1) + { + CtcUnion *cu = new CtcUnion(c1.nb_var); + cu->add_raw_ptr(&c1); + return cu; + }), + py::keep_alive<0,1>(), "c1"_a) + .def(py::init([](Ctc& c1, Ctc& c2) + { + CtcUnion *cu = new CtcUnion(c1.nb_var); + cu->add_raw_ptr(&c1); + cu->add_raw_ptr(&c2); + return cu; + }), + py::keep_alive<0,1>(), py::keep_alive<0,2>(), "c1"_a, "c2"_a) + .def(py::init([](Ctc& c1, Ctc& c2, Ctc& c3) + { + CtcUnion *cu = new CtcUnion(c1.nb_var); + cu->add_raw_ptr(&c1); + cu->add_raw_ptr(&c2); + cu->add_raw_ptr(&c3); + return cu; + }), + py::keep_alive<0,1>(), py::keep_alive<0,2>(), py::keep_alive<0,3>(), "c1"_a, "c2"_a, "c3"_a) .def("contract", (void (Ctc::*)(IntervalVector&)) &CtcUnion::contract) + .def("__ior__", [](CtcUnion& cu, Ctc& c) { return cu.add_raw_ptr(&c); }, py::keep_alive<1,2>(), py::return_value_policy::take_ownership) ; // Export CtcCompo diff --git a/python/src/core/graphics/codac_py_VIBesFig.cpp b/python/src/core/graphics/codac_py_VIBesFig.cpp index 61a69a21..eb895a90 100644 --- a/python/src/core/graphics/codac_py_VIBesFig.cpp +++ b/python/src/core/graphics/codac_py_VIBesFig.cpp @@ -92,5 +92,9 @@ void export_VIBesFig(py::module& m) .def("draw_vehicle", (void (VIBesFig::*)(double,double,double,double,const string&,const vibes::Params &))&VIBesFig::draw_vehicle, VIBESFIG_VOID_DRAW_VEHICLE_DOUBLE_DOUBLE_DOUBLE_DOUBLE_STRING_VIBESPARAMS, "x"_a, "y"_a, "heading"_a, "size"_a, "color"_a="", "params"_a=vibes::Params()) + + .def("draw_line", (void (VIBesFig::*)(const std::vector&,const std::vector&,const string&,const vibes::Params &))&VIBesFig::draw_line, + "todo", + "v_x"_a, "v_y"_a, "color"_a="", "params"_a=vibes::Params()) ; } \ No newline at end of file diff --git a/python/src/robotics/codac_py_Wall.cpp b/python/src/robotics/codac_py_Wall.cpp new file mode 100644 index 00000000..56b304b8 --- /dev/null +++ b/python/src/robotics/codac_py_Wall.cpp @@ -0,0 +1,42 @@ +/** + * \file + * Wall Python binding + * ---------------------------------------------------------------------------- + * \date 2024 + * \author Simon Rohou, Benoît Desrochers + * \copyright Copyright 2024 Codac Team + * \license This program is distributed under the terms of + * the GNU Lesser General Public License (LGPL). + */ + +#include +#include +#include +#include +#include "codac_type_caster.h" + +#include "codac_Wall.h" +// Generated file from Doxygen XML (doxygen2docstring.py): +#include "codac_py_Wall_docs.h" + +using namespace std; +using namespace codac; +namespace py = pybind11; +using namespace pybind11::literals; + + +void export_Wall(py::module& m) +{ + py::class_ wall(m, "Wall", WALL_MAIN); + wall + + .def(py::init(), "todo") + .def("contains", &Wall::contains, "todo", "p"_a) + .def(py::self & py::self) + .def_readwrite("c1", &Wall::c1) + .def_readwrite("c2", &Wall::c2) + ; + + m.def("shorter_dist_to_walls", &shorter_dist_to_walls, "todo", "v_walls"_a, "p"_a, "bearing"_a); + m.def("shorter_contact_to_walls", &shorter_contact_to_walls, "todo", "v_walls"_a, "p"_a); +} \ No newline at end of file diff --git a/src/core/contractors/static/codac_CtcSegment.cpp b/src/core/contractors/static/codac_CtcSegment.cpp index 2a2000a6..2721281a 100644 --- a/src/core/contractors/static/codac_CtcSegment.cpp +++ b/src/core/contractors/static/codac_CtcSegment.cpp @@ -14,7 +14,8 @@ using namespace std; namespace codac { CtcSegment::CtcSegment(double ax, double ay, double bx, double by) : Ctc(2), - X_with_params(2+4) { + X_with_params(2+4) +{ init(); @@ -24,6 +25,11 @@ CtcSegment::CtcSegment(double ax, double ay, double bx, double by) : Ctc(2), X_with_params[5] = Interval(by); } +CtcSegment::CtcSegment(const CtcSegment& ctc) : CtcSegment(ctc.X_with_params[2].lb(),ctc.X_with_params[3].lb(),ctc.X_with_params[4].lb(),ctc.X_with_params[5].lb()) +{ + +} + CtcSegment::CtcSegment() : Ctc(6), X_with_params(0 /* unused */) { init(); } diff --git a/src/core/contractors/static/codac_CtcSegment.h b/src/core/contractors/static/codac_CtcSegment.h index 45659e47..5d6ea535 100644 --- a/src/core/contractors/static/codac_CtcSegment.h +++ b/src/core/contractors/static/codac_CtcSegment.h @@ -15,15 +15,15 @@ #include "ibex_CtcFwdBwd.h" +namespace codac { + + using ibex::Interval; using ibex::IntervalVector; using ibex::Ctc; using ibex::NumConstraint; using ibex::CtcFwdBwd; - -namespace codac { - /** * \ingroup geometry * @@ -58,6 +58,8 @@ class CtcSegment : public Ctc { */ CtcSegment(); + CtcSegment(const CtcSegment& ctc); + /** * \brief Contract a box. * \param box to be contracted diff --git a/src/core/contractors/static/codac_CtcUnion.h b/src/core/contractors/static/codac_CtcUnion.h index b9c73b41..336162db 100644 --- a/src/core/contractors/static/codac_CtcUnion.h +++ b/src/core/contractors/static/codac_CtcUnion.h @@ -2,9 +2,9 @@ * \file * CtcUnion class * ---------------------------------------------------------------------------- - * \date 2022 + * \date 2024 * \author Simon Rohou - * \copyright Copyright 2022 Codac Team + * \copyright Copyright 2024 Codac Team * \license This program is distributed under the terms of * the GNU Lesser General Public License (LGPL). */ @@ -12,11 +12,72 @@ #ifndef __CODAC_CTCUNION_H__ #define __CODAC_CTCUNION_H__ -#include "ibex_CtcUnion.h" +#include +//#include "ibex_CtcUnion.h" +#include "codac_Ctc.h" namespace codac { - using ibex::CtcUnion; + class CtcUnion : public Ctc + { + public: + + CtcUnion(int nb_var) : Ctc(nb_var) + { } + + template + CtcUnion(const C1& c1) : Ctc(c1.nb_var) + { + _v_ctc.push_back(std::make_shared(c1)); + } + + template + CtcUnion(const C1& c1, const C&... c) : CtcUnion(c1) + { + (_v_ctc.push_back(std::make_shared(c)), ...); + for(const auto& ci : _v_ctc) { assert(ci->nb_var == nb_var); } + } + + void contract(IntervalVector& x) + { + IntervalVector result(nb_var, Interval::empty_set()); + + for(auto& ci : _v_ctc) + { + IntervalVector saved_x = x; + ci->contract(saved_x); + result |= saved_x; + } + + for(auto& ci : _v_ctc_ptrs) + { + IntervalVector saved_x = x; + ci->contract(saved_x); + result |= saved_x; + } + + x = result; + } + + template + CtcUnion& operator|=(const C& c) + { + assert(c.nb_var == nb_var); + _v_ctc.push_back(std::make_shared(c)); + return *this; + } + + CtcUnion& add_raw_ptr(Ctc *c) + { + _v_ctc_ptrs.push_back(c); + return *this; + } + + protected: + + std::vector> _v_ctc; + std::vector _v_ctc_ptrs; + }; } #endif \ No newline at end of file diff --git a/src/robotics/CMakeLists.txt b/src/robotics/CMakeLists.txt index c75f3049..6d551d21 100644 --- a/src/robotics/CMakeLists.txt +++ b/src/robotics/CMakeLists.txt @@ -6,6 +6,8 @@ ${CMAKE_CURRENT_SOURCE_DIR}/graphics/codac_VIBesFigMap.h ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Beacon.cpp ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Beacon.h + ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Wall.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Wall.h ${CMAKE_CURRENT_SOURCE_DIR}/loops/codac_TPlane.h ${CMAKE_CURRENT_SOURCE_DIR}/loops/codac_TPlane.cpp ${CMAKE_CURRENT_SOURCE_DIR}/data/codac_DataLoaderLissajous.cpp diff --git a/src/robotics/objects/codac_Wall.cpp b/src/robotics/objects/codac_Wall.cpp new file mode 100644 index 00000000..4842c4e6 --- /dev/null +++ b/src/robotics/objects/codac_Wall.cpp @@ -0,0 +1,82 @@ +/** + * Wall class + * ---------------------------------------------------------------------------- + * \date 2024 + * \author Simon Rohou + * \copyright Copyright 2024 Codac Team + * \license This program is distributed under the terms of + * the GNU Lesser General Public License (LGPL). + */ + +#include +#include "codac_Wall.h" +#include "codac_predef_values.h" + +using namespace std; +using namespace ibex; + +namespace codac +{ + Wall::Wall(const Vector& c1_, const Vector& c2_) : c1(c1_), c2(c2_) + { + } + + bool Wall::contains(const Vector& p) const + { + Vector ab { c2[0]-c1[0], c2[1]-c1[1] }; + Vector ac { p[0]-c1[0], p[1]-c1[1] }; + + double dp_AB = ab[0]*ab[0] + ab[1]*ab[1]; + double dp_AC = ab[0]*ac[0] + ab[1]*ac[1]; + return Interval(0.,dp_AB).contains(dp_AC); + } + + Vector operator&(const Wall& w1, const Wall& w2) + { + const double& x1 = w1.c1[0]; const double& x2 = w1.c2[0]; + const double& x3 = w2.c1[0]; const double& x4 = w2.c2[0]; + + const double& y1 = w1.c1[1]; const double& y2 = w1.c2[1]; + const double& y3 = w2.c1[1]; const double& y4 = w2.c2[1]; + + return { + ((x1*y2-y1*x2)*(x3-x4)-(x1-x2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4)), + ((x1*y2-y1*x2)*(y3-y4)-(y1-y2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4)) + }; + } + + double shorter_dist_to_walls(const std::vector& v_walls, const Vector& p, double bearing) + { + Wall w0 { p, { p[0]+999999.*cos(bearing), p[1]+999999.*sin(bearing) }}; + double min_dist = oo; + + for(const auto& wi : v_walls) + { + Vector pi = w0 & wi; + if(!wi.contains(pi) || !w0.contains(pi)) + continue; + double dist = pow(p[0]-pi[0],2) + pow(p[1]-pi[1],2); + min_dist = dist < min_dist ? dist : min_dist; + } + + return sqrt(min_dist); + } + + Vector shorter_contact_to_walls(const std::vector& v_walls, const Vector& p) + { + double min_dist = oo; + double bearing; + + for(double a = 0. ; a < 2.*M_PI ; a+=0.1) + { + double dist = shorter_dist_to_walls(v_walls, p, a); + if(dist < min_dist) + { + min_dist = dist; + bearing = a; + } + } + + return { min_dist, bearing }; + } +} \ No newline at end of file diff --git a/src/robotics/objects/codac_Wall.h b/src/robotics/objects/codac_Wall.h new file mode 100644 index 00000000..af7de4ca --- /dev/null +++ b/src/robotics/objects/codac_Wall.h @@ -0,0 +1,37 @@ +/** + * Beacon class + * ---------------------------------------------------------------------------- + * \date 2024 + * \author Simon Rohou + * \copyright Copyright 2024 Codac Team + * \license This program is distributed under the terms of + * the GNU Lesser General Public License (LGPL). + */ + +#ifndef __CODAC_WALL_H__ +#define __CODAC_WALL_H__ + +#include "codac_Vector.h" +#include "codac_IntervalVector.h" + +namespace codac +{ + class Wall + { + public: + + Wall(const Vector& c1, const Vector& c2); + bool contains(const Vector& p) const; + + //protected: + + Vector c1, c2; + }; + + Vector operator&(const Wall& w1, const Wall& w2); + + double shorter_dist_to_walls(const std::vector& v_walls, const Vector& p, double bearing); + Vector shorter_contact_to_walls(const std::vector& v_walls, const Vector& p); +} + +#endif \ No newline at end of file