From 4dabfb6d706ad5c82cb76de59ee0e4eb48480676 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 11 Oct 2023 11:36:51 +0200 Subject: [PATCH 1/6] Potential bug for IntervalVector += in Python --- python/src/core/domains/interval/codac_py_IntervalVector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/src/core/domains/interval/codac_py_IntervalVector.cpp b/python/src/core/domains/interval/codac_py_IntervalVector.cpp index d14372136..31fd80d1f 100644 --- a/python/src/core/domains/interval/codac_py_IntervalVector.cpp +++ b/python/src/core/domains/interval/codac_py_IntervalVector.cpp @@ -260,7 +260,7 @@ void export_IntervalVector(py::module& m) .def(py::self |= py::self) .def("__add__", [](IntervalVector& a, const Vector& x) { return a+x; }) - .def("__iadd__", [](IntervalVector& a, const Vector& x) { return a+x; }) + .def("__iadd__", [](IntervalVector& a, const Vector& x) { return a+=x; }) .def("__radd__", [](IntervalVector& a, const Vector& x) { return a+x; }) .def("__sub__", [](IntervalVector& a, const Vector& x) { return a-x; }) From 54966a6be0ddd6fafe64bd90fe6401822ba49587 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Tue, 10 Oct 2023 22:53:37 +0200 Subject: [PATCH 2/6] Workarounds of unsupported operators or special functions for MATLAB compatibility, see https://fr.mathworks.com/help/matlab/matlab_external/differences-between-matlab-python.html --- .../domains/interval/codac_py_Interval.cpp | 34 +++++++++++- .../interval/codac_py_IntervalMatrix.cpp | 2 + .../interval/codac_py_IntervalVector.cpp | 12 +++++ .../src/core/domains/tube/codac_py_Tube.cpp | 52 +++++++++++++++++++ .../core/domains/tube/codac_py_TubeVector.cpp | 44 ++++++++++++++++ 5 files changed, 143 insertions(+), 1 deletion(-) diff --git a/python/src/core/domains/interval/codac_py_Interval.cpp b/python/src/core/domains/interval/codac_py_Interval.cpp index ce7b3511c..616e82d9d 100644 --- a/python/src/core/domains/interval/codac_py_Interval.cpp +++ b/python/src/core/domains/interval/codac_py_Interval.cpp @@ -134,7 +134,15 @@ void export_Interval(py::module& m) .def(py::self * py::self) .def(py::self / py::self) .def(py::self & py::self) + // For MATLAB compatibility. + //.def_static("inter", [](const Interval& x, const Interval& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Interval& s, const Interval& y) { return s&y; }) .def(py::self | py::self) + // For MATLAB compatibility. + //.def_static("union", [](const Interval& x, const Interval& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Interval& s, const Interval& y) { return s|y; }) .def("__iadd__", [](Interval& x, Interval& o) { return x += o; }) .def("__isub__", [](Interval& x, Interval& o) { return x -= o; }) @@ -143,7 +151,11 @@ void export_Interval(py::module& m) .def("__ifloordiv__", [](Interval& x, Interval& o) { return x /= o; }) .def(py::self &= py::self) + // For MATLAB compatibility. + .def("inter_self", [](Interval& s, const Interval& a) { return s&=a; }) .def(py::self |= py::self) + // For MATLAB compatibility. + .def("union_self", [](Interval& s, const Interval& a) { return s|=a; }) .def(py::self + double()) .def(py::self += double()) @@ -160,9 +172,25 @@ void export_Interval(py::module& m) .def(-py::self) .def("__abs__", [](const Interval& a) { return ibex::abs(a); }) + // For MATLAB compatibility. + //.def_static("abs", [](const Interval& a) { return ibex::abs(a); }) + // For MATLAB compatibility. + .def("abs", [](const Interval& s) { return ibex::abs(s); }) .def("__pow__", [](const Interval& x, int n) { return ibex::pow(x, n); }) + // For MATLAB compatibility. + //.def_static("pow", [](const Interval& x, int n) { return ibex::pow(x, n); }) + // For MATLAB compatibility. + .def("pow", [](const Interval& s, int n) { return ibex::pow(s, n); }) .def("__pow__", [](const Interval& x, double d) { return ibex::pow(x, d); }) - .def("__pow__", [](const Interval &x, const Interval &y) { return ibex::pow(x, y); }) + // For MATLAB compatibility. + //.def_static("pow", [](const Interval& x, double d) { return ibex::pow(x, d); }) + // For MATLAB compatibility. + .def("pow", [](const Interval& s, double d) { return ibex::pow(s, d); }) + .def("__pow__", [](const Interval& x, const Interval& y) { return ibex::pow(x, y); }) + // For MATLAB compatibility. + //.def_static("pow", [](const Interval& x, const Interval& y) { return ibex::pow(x, y); }) + // For MATLAB compatibility. + .def("pow", [](const Interval& s, const Interval& y) { return ibex::pow(s, y); }) .def("lb", &Interval::lb, "return the upper bound") .def("ub", &Interval::ub, "return the lower bound") @@ -212,8 +240,12 @@ void export_Interval(py::module& m) .def("bisect", &Interval::bisect, DOCS_INTERVAL_BISECT, py::arg("ratio")=0.5) .def("__get_item__", get_item, "x[0] returns the lb and x[1] returns ub") + // For MATLAB compatibility. + .def_static("get_item", [](Interval& x, size_t i) { return get_item(x, i); }) .def("copy", &interval_copy, "return a new object which is the copy of x") .def("__hash__", [](const Interval& s1) { return reinterpret_cast(&s1); }) + // For MATLAB compatibility. + .def("hash", [](const Interval& s1) { return reinterpret_cast(&s1); }) //.def( "__pow__", pow__) // Constants diff --git a/python/src/core/domains/interval/codac_py_IntervalMatrix.cpp b/python/src/core/domains/interval/codac_py_IntervalMatrix.cpp index e7aac5f25..abac3d269 100644 --- a/python/src/core/domains/interval/codac_py_IntervalMatrix.cpp +++ b/python/src/core/domains/interval/codac_py_IntervalMatrix.cpp @@ -132,6 +132,8 @@ void export_IntervalMatrix(py::module& m) .def(py::self - py::self) .def(py::self * py::self) .def(py::self &= py::self) + // For MATLAB compatibility. + .def("inter_self", [](IntervalMatrix& s, const IntervalMatrix& a) { return s&=a; }) .def(-py::self) .def(py::self += py::self) // todo: .def(py::self += other()) diff --git a/python/src/core/domains/interval/codac_py_IntervalVector.cpp b/python/src/core/domains/interval/codac_py_IntervalVector.cpp index 31fd80d1f..1d7670dc6 100644 --- a/python/src/core/domains/interval/codac_py_IntervalVector.cpp +++ b/python/src/core/domains/interval/codac_py_IntervalVector.cpp @@ -243,7 +243,15 @@ void export_IntervalVector(py::module& m) .def(py::self - py::self) .def(py::self * py::self) .def(py::self & py::self) + // For MATLAB compatibility. + //.def_static("inter", [](const IntervalVector& x, const IntervalVector& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const IntervalVector& s, const IntervalVector& y) { return s&y; }) .def(py::self | py::self) + // For MATLAB compatibility. + //.def_static("union", [](const IntervalVector& x, const IntervalVector& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const IntervalVector& s, const IntervalVector& y) { return s|y; }) .def(-py::self) .def(py::self += py::self) @@ -257,7 +265,11 @@ void export_IntervalVector(py::module& m) .def("__rmul__", [](IntervalVector& a, const Interval& x) { return x*a; }) .def(py::self &= py::self) + // For MATLAB compatibility. + .def("inter_self", [](IntervalVector& s, const IntervalVector& a) { return s&=a; }) .def(py::self |= py::self) + // For MATLAB compatibility. + .def("union_self", [](IntervalVector& s, const IntervalVector& a) { return s|=a; }) .def("__add__", [](IntervalVector& a, const Vector& x) { return a+x; }) .def("__iadd__", [](IntervalVector& a, const Vector& x) { return a+=x; }) diff --git a/python/src/core/domains/tube/codac_py_Tube.cpp b/python/src/core/domains/tube/codac_py_Tube.cpp index 9bff4710f..8892ebd9b 100644 --- a/python/src/core/domains/tube/codac_py_Tube.cpp +++ b/python/src/core/domains/tube/codac_py_Tube.cpp @@ -370,21 +370,45 @@ void export_Tube(py::module& m) .def("__ior__", [](Tube& s,const Interval& o) { return s |= o; }, TUBE_CONSTTUBE_OPERATORUNIEQ_INTERVAL) + // For MATLAB compatibility. + .def("union_self", [](Tube& s,const Interval& o) { return s |= o; }, + TUBE_CONSTTUBE_OPERATORUNIEQ_INTERVAL) + .def("__ior__", [](Tube& s,const Trajectory& o) { return s |= o; }, TUBE_CONSTTUBE_OPERATORUNIEQ_TRAJECTORY) + // For MATLAB compatibility. + .def("union_self", [](Tube& s,const Trajectory& o) { return s |= o; }, + TUBE_CONSTTUBE_OPERATORUNIEQ_TRAJECTORY) + .def("__ior__", [](Tube& s,const Tube& o) { return s |= o; }, TUBE_CONSTTUBE_OPERATORUNIEQ_TUBE) + // For MATLAB compatibility. + .def("union_self", [](Tube& s,const Tube& o) { return s |= o; }, + TUBE_CONSTTUBE_OPERATORUNIEQ_TUBE) + .def("__iand__", [](Tube& s,const Interval& o) { return s &= o; }, TUBE_CONSTTUBE_OPERATORINTEQ_INTERVAL) + // For MATLAB compatibility. + .def("inter_self", [](Tube& s,const Interval& o) { return s &= o; }, + TUBE_CONSTTUBE_OPERATORINTEQ_INTERVAL) + .def("__iand__", [](Tube& s,const Trajectory& o) { return s &= o; }, TUBE_CONSTTUBE_OPERATORINTEQ_TRAJECTORY) + // For MATLAB compatibility. + .def("inter_self", [](Tube& s,const Trajectory& o) { return s &= o; }, + TUBE_CONSTTUBE_OPERATORINTEQ_TRAJECTORY) + .def("__iand__", [](Tube& s,const Tube& o) { return s &= o; }, TUBE_CONSTTUBE_OPERATORINTEQ_TUBE) + // For MATLAB compatibility. + .def("inter_self", [](Tube& s,const Tube& o) { return s &= o; }, + TUBE_CONSTTUBE_OPERATORINTEQ_TUBE) + // String .def("class_name", &Tube::class_name, @@ -493,21 +517,49 @@ void export_Tube(py::module& m) .def("__rtruediv__", [](const Tube& y, const TubeVector& x) { return x/y; }) .def("__or__", [](const Tube& x, const Tube& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& x, const Tube& y) { return x|y; }) .def("__or__", [](const Tube& x, double y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& x, double y) { return x|y; }) .def("__or__", [](const Tube& x, const Interval& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& x, const Interval& y) { return x|y; }) .def("__or__", [](const Tube& x, const Trajectory& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& x, const Trajectory& y) { return x|y; }) .def("__ror__", [](const Tube& y, double x) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& y, double x) { return x|y; }) .def("__ror__", [](const Tube& y, const Interval& x) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& y, const Interval& x) { return x|y; }) .def("__ror__", [](const Tube& y, const Trajectory& x) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const Tube& y, const Trajectory& x) { return x|y; }) .def("__and__", [](const Tube& x, const Tube& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& x, const Tube& y) { return x&y; }) .def("__and__", [](const Tube& x, double y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& x, double y) { return x&y; }) .def("__and__", [](const Tube& x, const Interval& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& x, const Interval& y) { return x&y; }) .def("__and__", [](const Tube& x, const Trajectory& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& x, const Trajectory& y) { return x&y; }) .def("__rand__", [](const Tube& y, double x) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& y, double x) { return x&y; }) .def("__rand__", [](const Tube& y, const Interval& x) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& y, const Interval& x) { return x&y; }) .def("__rand__", [](const Tube& y, const Trajectory& x) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const Tube& y, const Trajectory& x) { return x&y; }) ; } \ No newline at end of file diff --git a/python/src/core/domains/tube/codac_py_TubeVector.cpp b/python/src/core/domains/tube/codac_py_TubeVector.cpp index aca0dd891..720237e8a 100644 --- a/python/src/core/domains/tube/codac_py_TubeVector.cpp +++ b/python/src/core/domains/tube/codac_py_TubeVector.cpp @@ -394,21 +394,45 @@ void export_TubeVector(py::module& m) .def("__ior__", [](TubeVector& s,const IntervalVector& o) { return s |= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_INTERVALVECTOR) + // For MATLAB compatibility. + .def("union_self", [](TubeVector& s,const IntervalVector& o) { return s |= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_INTERVALVECTOR) + .def("__ior__", [](TubeVector& s,const TrajectoryVector& o) { return s |= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_TRAJECTORYVECTOR) + // For MATLAB compatibility. + .def("union_self", [](TubeVector& s,const TrajectoryVector& o) { return s |= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_TRAJECTORYVECTOR) + .def("__ior__", [](TubeVector& s,const TubeVector& o) { return s |= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_TUBEVECTOR) + // For MATLAB compatibility. + .def("union_self", [](TubeVector& s,const TubeVector& o) { return s |= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORUNIEQ_TUBEVECTOR) + .def("__iand__", [](TubeVector& s,const IntervalVector& o) { return s &= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_INTERVALVECTOR) + // For MATLAB compatibility. + .def("inter_self", [](TubeVector& s,const IntervalVector& o) { return s &= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_INTERVALVECTOR) + .def("__iand__", [](TubeVector& s,const TrajectoryVector& o) { return s &= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_TRAJECTORYVECTOR) + // For MATLAB compatibility. + .def("inter_self", [](TubeVector& s,const TrajectoryVector& o) { return s &= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_TRAJECTORYVECTOR) + .def("__iand__", [](TubeVector& s,const TubeVector& o) { return s &= o;}, TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_TUBEVECTOR) + // For MATLAB compatibility. + .def("inter_self", [](TubeVector& s,const TubeVector& o) { return s &= o;}, + TUBEVECTOR_CONSTTUBEVECTOR_OPERATORINTEQ_TUBEVECTOR) + // String .def("class_name", &TubeVector::class_name, @@ -568,17 +592,37 @@ void export_TubeVector(py::module& m) // todo .def("__truediv__", [](const IntervalVector& x, const Tube& y); .def("__or__", [](const TubeVector& x, const TubeVector& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const TubeVector& x, const TubeVector& y) { return x|y; }) .def("__or__", [](const TubeVector& x, const IntervalVector& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const TubeVector& x, const IntervalVector& y) { return x|y; }) .def("__or__", [](const TubeVector& x, const TrajectoryVector& y) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const TubeVector& x, const TrajectoryVector& y) { return x|y; }) .def("__ror__", [](const TubeVector& y, const IntervalVector& x) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const TubeVector& y, const IntervalVector& x) { return x|y; }) .def("__ror__", [](const TubeVector& y, const TrajectoryVector& x) { return x|y; }) + // For MATLAB compatibility. + .def("union", [](const TubeVector& y, const TrajectoryVector& x) { return x|y; }) .def("__and__", [](const TubeVector& x, const TubeVector& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const TubeVector& x, const TubeVector& y) { return x&y; }) .def("__and__", [](const TubeVector& x, const IntervalVector& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const TubeVector& x, const IntervalVector& y) { return x&y; }) .def("__and__", [](const TubeVector& x, const TrajectoryVector& y) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const TubeVector& x, const TrajectoryVector& y) { return x&y; }) .def("__rand__", [](const TubeVector& y, const IntervalVector& x) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const TubeVector& y, const IntervalVector& x) { return x&y; }) .def("__rand__", [](const TubeVector& y, const TrajectoryVector& x) { return x&y; }) + // For MATLAB compatibility. + .def("inter", [](const TubeVector& y, const TrajectoryVector& x) { return x&y; }) ; } \ No newline at end of file From d8a498e8fd2b884020ae3e9b7847fb5d9ae4a3d2 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Tue, 10 Oct 2023 23:25:04 +0200 Subject: [PATCH 3/6] Update examples --- .../01_getting_started/01_getting_started.py | 2 +- .../01_getting_started/a01_getting_started.m | 5 +- .../a03_static_rangebearing.m | 86 ++++++++++++++ .../05_dyn_rangebearing.py | 2 +- .../a05_dyn_rangebearing.m | 111 ++++++++++++++++++ 5 files changed, 201 insertions(+), 5 deletions(-) create mode 100644 examples/tuto/03_static_rangebearing/a03_static_rangebearing.m create mode 100644 examples/tuto/05_dyn_rangebearing/a05_dyn_rangebearing.m diff --git a/examples/tuto/01_getting_started/01_getting_started.py b/examples/tuto/01_getting_started/01_getting_started.py index f6f1c5379..692375f1f 100644 --- a/examples/tuto/01_getting_started/01_getting_started.py +++ b/examples/tuto/01_getting_started/01_getting_started.py @@ -1,5 +1,5 @@ # Codac - Examples -# Dynamic range-only localization +# Getting started: 2 minutes to Codac # ---------------------------------------------------------------------------- from codac import * diff --git a/examples/tuto/01_getting_started/a01_getting_started.m b/examples/tuto/01_getting_started/a01_getting_started.m index 030702085..e593481f9 100644 --- a/examples/tuto/01_getting_started/a01_getting_started.m +++ b/examples/tuto/01_getting_started/a01_getting_started.m @@ -1,5 +1,5 @@ % Codac - Examples -% Dynamic range-only localization +% Getting started: 2 minutes to Codac % ---------------------------------------------------------------------------- import py.codac.* @@ -87,5 +87,4 @@ % Checking if this example still works: -if x.volume() < 5; check = true; else; check = false; end % todo: x.contains(x_truth) -assert(check) +assert(x.volume() < 5) % todo: x.contains(x_truth) diff --git a/examples/tuto/03_static_rangebearing/a03_static_rangebearing.m b/examples/tuto/03_static_rangebearing/a03_static_rangebearing.m new file mode 100644 index 000000000..95abb361c --- /dev/null +++ b/examples/tuto/03_static_rangebearing/a03_static_rangebearing.m @@ -0,0 +1,86 @@ +% Codac - Examples +% Static range-bearing localization +% ---------------------------------------------------------------------------- + +import py.codac.* + +% =================== 0. Parameters, truth and data ==================== + +% Truth (unknown pose) +x_truth = [0,0,pi/6]; % (x,y,heading) + +% Creating random map of landmarks +map_area = IntervalVector(int32(2), [-8,8]); +v_map = DataLoader().generate_landmarks_boxes(map_area, int32(1)); + +% The following function generates a set of [range]x[bearing] values +v_obs = DataLoader().generate_static_observations(py.list(x_truth), v_map, false); + +% Adding uncertainties on the measurements +for i=1:length(v_obs) % for each observation: + v_obs{i}.getitem(int32(0)).inflate(0.3); % range + v_obs{i}.getitem(int32(1)).inflate(0.1); % bearing +end + + +% =============== 1. Defining domains for our variables ================ + +x = IntervalVector(int32(2)); % unknown position +heading = Interval(x_truth(3)).inflate(0.01); % measured heading + + +% =========== 2. Defining contractors to deal with equations =========== + +ctc_plus = CtcFunction(Function('a', 'b', 'c', 'a+b-c')); % a+b=c +ctc_minus = CtcFunction(Function('a', 'b', 'c', 'a-b-c')); % a-b=c +% We also use the predefined contractor CtcPolar(), no need to build it + + +% =============== 3. Adding the contractors to a network =============== + +cn = ContractorNetwork(); + +for i=1:length(v_obs) + + % Intermediate variables + alpha = cn.create_interm_var(Interval()); + d = cn.create_interm_var(IntervalVector(int32(2))); + + cn.add(ctc_plus, py.list({v_obs{i}.getitem(int32(1)), heading, alpha})); + cn.add(ctc_minus, py.list({v_map{i}, x, d})); + cn.add(CtcPolar(), py.list({d, v_obs{i}.getitem(int32(0)), alpha})); +end + + +% ======================= 4. Solving the problem ======================= + +cn.contract(); + + +% ============================ 5. Graphics ============================= + +beginDrawing(); + +fig = VIBesFigMap('Map'); +fig.set_properties(int32(50), int32(50), int32(600), int32(600)); + +for i=1:length(v_map) + iv = v_map{i}; + fig.add_beacon(iv.mid(), 0.2); +end + +for i=1:length(v_obs) + y = v_obs{i}; + fig.draw_pie(x_truth(1), x_truth(2), y.getitem(int32(0)).union(Interval(0)), heading+y.getitem(int32(1)), 'lightGray'); + fig.draw_pie(x_truth(1), x_truth(2), y.getitem(int32(0)), heading+y.getitem(int32(1)), 'gray'); +end + +fig.draw_vehicle(py.list(x_truth),0.5); +fig.draw_box(x); % estimated position +fig.show(); + +endDrawing(); + + +% Checking if this example still works: +assert(x.contains(py.list(x_truth(1:2)))) diff --git a/examples/tuto/05_dyn_rangebearing/05_dyn_rangebearing.py b/examples/tuto/05_dyn_rangebearing/05_dyn_rangebearing.py index 0104b19e4..3e4256fa7 100644 --- a/examples/tuto/05_dyn_rangebearing/05_dyn_rangebearing.py +++ b/examples/tuto/05_dyn_rangebearing/05_dyn_rangebearing.py @@ -1,5 +1,5 @@ # Codac - Examples -# Dynamic range-only localization +# Dynamic range-bearing localization # ---------------------------------------------------------------------------- from codac import * diff --git a/examples/tuto/05_dyn_rangebearing/a05_dyn_rangebearing.m b/examples/tuto/05_dyn_rangebearing/a05_dyn_rangebearing.m new file mode 100644 index 000000000..1bf058759 --- /dev/null +++ b/examples/tuto/05_dyn_rangebearing/a05_dyn_rangebearing.m @@ -0,0 +1,111 @@ +% Codac - Examples +% Dynamic range-bearing localization +% ---------------------------------------------------------------------------- + +import py.codac.* + + +% =================== 0. Parameters, truth and data ==================== + +dt = 0.05; % timestep for tubes accuracy +tdomain = Interval(0,3); % temporal limits [t_0,t_f]=[0,3] + +x_truth = TrajectoryVector(tdomain, TFunction(['(' ... + '10*cos(t)+t ;' ... + '5*sin(2*t)+t ;' ... + 'atan2((10*cos(2*t)+1),(-10*sin(t)+1)) ;' ... + 'sqrt((-10*sin(t)+1)^2+(10*cos(2*t)+1)^2))'])); % actual trajectory + +% Continuous measurements coming from the truth +measured_psi = x_truth.getitem(int32(2)).sample(dt).make_continuous(); +measured_psi = measured_psi + RandTrajectory(tdomain, dt, Interval(-0.01,0.01)); % adding some noise +measured_speed = x_truth.getitem(int32(3)).sample(dt); +measured_speed = measured_speed + RandTrajectory(tdomain, dt, Interval(-0.01,0.01)); % adding some noise + +% Creating random map of landmarks +map_area = IntervalVector(int32(2), [-8,8]); +v_map = DataLoader().generate_landmarks_boxes(map_area, int32(30)); + +% The following function generates a set of [range]x[bearing] values +v_obs = DataLoader().generate_observations(x_truth, v_map, int32(10)); + +% Adding uncertainties on the measurements +for i=1:length(v_obs) % for each observation: + obs = v_obs{i}; + obs.getitem(int32(1)).inflate(0.3); % range + obs.getitem(int32(2)).inflate(0.1); % bearing +end + +% =============== 1. Defining domains for our variables ================ + +x = TubeVector(tdomain, dt, int32(4)); % 4d tube for state vectors +v = TubeVector(tdomain, dt, int32(4)); % 4d tube for derivatives of the states +u = TubeVector(tdomain, dt, int32(2)); % 2d tube for inputs of the system + +x.setitem(int32(2), Tube(measured_psi, dt).inflate(0.01)); % measured_psi is a set of measurements +x.setitem(int32(3), Tube(measured_speed, dt).inflate(0.01)); + + +% =========== 2. Defining contractors to deal with equations =========== + +ctc_f = CtcFunction(Function('v[4]', 'x[4]', 'u[2]', ... + '(v[0]-x[3]*cos(x[2]) ; v[1]-x[3]*sin(x[2]) ; v[2]-u[0] ; v[3]-u[1])')); +ctc_plus = CtcFunction(Function('a', 'b', 'c', 'a+b-c')); % a+b=c +ctc_minus = CtcFunction(Function('a', 'b', 'c', 'a-b-c')); % a-b=c +% We also use the predefined contractors CtcPolar(), CtcEval(), no need to build them + + +% =============== 3. Adding the contractors to a network =============== + +cn = ContractorNetwork(); % creating a network +cn.add(ctc_f, py.list({v, x, u})); % adding the f constraint + +for i=1:length(v_obs) % we add the observ. constraint for each range-only measurement + y = v_obs{i}; + + % Intermediate variables + alpha = cn.create_interm_var(Interval()); % absolute angle robot-landmark + d = cn.create_interm_var(IntervalVector(int32(2))); % dist robot-landmark + p = cn.create_interm_var(IntervalVector(int32(4))); % state at t_i + + cn.add(ctc_plus, py.list({y.getitem(int32(2)), p.getitem(int32(2)), alpha})); + cn.add(ctc_minus, py.list({cn.subvector(y,int32(3),int32(4)), cn.subvector(p,int32(0),int32(1)), d})); + cn.add(CtcPolar(), py.list({d, y.getitem(int32(1)), alpha})); + cn.add(CtcEval(), py.list({y.getitem(int32(0)), p, x, v})); +end + + +% ======================= 4. Solving the problem ======================= + +cn.contract(true); + + +% ============================ 5. Graphics ============================= + +beginDrawing(); +fig = VIBesFigMap('fig'); +fig.set_properties(int32(50), int32(50), int32(900), int32(550)); +fig.add_trajectory(x_truth, 'xtruth', int32(0), int32(1), int32(2)); +fig.add_tube(x, 'x', int32(0), int32(1)); +fig.smooth_tube_drawing(true); + +for i=1:length(v_map) + b = v_map{i}; + fig.add_beacon(b.mid(), 0.2); % drawing beacons +end + +for i=1:length(v_obs) + y = v_obs{i}; + t_obs = y.getitem(int32(0)).mid(); + t_state = x_truth(t_obs); + fig.draw_pie(t_state{1}, t_state{2}, y.getitem(int32(1)).union(Interval(0.01)), t_state{3} + y.getitem(int32(2)), 'lightGray'); % drawing range-bearing measurements + fig.draw_pie(t_state{1}, t_state{2}, y.getitem(int32(1)), t_state{3} + y.getitem(int32(2)), 'darkGray'); % drawing range-bearing measurements + fig.draw_vehicle(t_obs, x_truth, 0.7); +end + +fig.show(double(0)); +endDrawing(); + + +% Checking if this example still works: +assert(x.contains(x_truth) == py.codac.core.BoolInterval(int32(2))); From db14531410fa2aaa0fb1d473fd70e4fea3c591f3 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 11 Oct 2023 22:53:34 +0200 Subject: [PATCH 4/6] Update MATLAB documentation --- doc/doc/install/04-start-matlab-project.rst | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/doc/doc/install/04-start-matlab-project.rst b/doc/doc/install/04-start-matlab-project.rst index f62a1e11e..b69699ea5 100644 --- a/doc/doc/install/04-start-matlab-project.rst +++ b/doc/doc/install/04-start-matlab-project.rst @@ -18,8 +18,15 @@ Start a MATLAB project | Integers in function calls from the Codac library should be cast to `int32`, e.g. `4` would be `int32(4)` (however do not change those that are in strings), otherwise remember that all numbers are `double` by default in MATLAB. | The operator `[index]` for a Codac object should be replaced with `.getitem(int32(index))` when getting its value while it should be replaced with `.setitem(int32(index))` when setting its value (please note that indices of Codac objects start at `0` contrary to typical MATLAB objects such as MATLAB arrays or cell arrays). + | The operators/functions `x&y` (intersection) for a Codac object should be replaced with `x.inter(y)`, `x|y` (union) with `x.union(y)`, `x&=y` (intersection and update of x) with `x.inter_self(y)`, `x|=y` (union and update of x) with `x.union_self(y)`, `x**y` (power) with `x.pow(y)`, `abs(x)` (absolute) with `x.abs()`. | Python lists of objects should be converted to MATLAB cell arrays. - | Also, when a Codac function needs a Python list parameter, the corresponding MATLAB cell array should be given as `py.list(...)`. + | Also, when a Codac function needs a `py.list` or `Vector` parameter, the corresponding MATLAB cell array should be given as `py.list(...)` (however when the Codac function do not need a `py.list` or `Vector` parameter but just an element of a MATLAB cell array, do not convert with `py.list(...)` and be sure to get the cell array element with `{}` operator). + | Be sure that Python multiline strings are correctly converted to multiline MATLAB strings between `'`. Remember that multiline statements in MATLAB need `...` before next line. + | Please also convert Python `for` loops to typical MATLAB `for` loops, same for `if-else`, `+=` statements. + +.. admonition:: Automating Python to MATLAB conversions + + | `Bing Chat with GPT-4 `_ in `Precise` conversation style has been used successfully to help convert Python examples to MATLAB, using the description above and by providing `01_getting_started.py` and the corresponding `a01_getting_started.m` as example (about 80% of the code for `03_static_rangebearing.m` and `05_dyn_rangebearing.m` was correctly converted this way). .. code-block:: matlab From 1c90377fdabdc4193ecd47deaec5351063dbbbbe Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 15 Oct 2023 00:59:51 +0200 Subject: [PATCH 5/6] Update examples --- doc/doc/tutorial/08-rangeonly-slam/solution.m | 125 ++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 doc/doc/tutorial/08-rangeonly-slam/solution.m diff --git a/doc/doc/tutorial/08-rangeonly-slam/solution.m b/doc/doc/tutorial/08-rangeonly-slam/solution.m new file mode 100644 index 000000000..5560d2cd6 --- /dev/null +++ b/doc/doc/tutorial/08-rangeonly-slam/solution.m @@ -0,0 +1,125 @@ +% Codac - Examples +% Dynamic range-bearing localization +% ---------------------------------------------------------------------------- + +import py.codac.* + +% =========== CREATING DATA =========== + +dt = 0.05; +iteration_dt = 0.2; +tdomain = Interval(0,15); % [t0,tf] + +% Initial pose x0=(0,0,2) +x0 = [0, 0, 2]; + +% System input +u = Trajectory(tdomain, TFunction('3*(sin(t)^2)+t/100'), dt); + +% Noise +i_n = Interval(-0.03,0.03); % the noises are known to be bounded by i_n + +n_u = RandTrajectory(tdomain, dt, i_n); % input noise +n_theta = RandTrajectory(tdomain, dt, i_n); % heading noise + +% Actual trajectories (state + derivative) +v_truth = TrajectoryVector(int32(3)); +x_truth = TrajectoryVector(int32(3)); +v_truth.setitem(int32(2), u + n_u); +x_truth.setitem(int32(2), v_truth.getitem(int32(2)).primitive() + x0(3)); +v_truth.setitem(int32(0), 10*cos(x_truth.getitem(int32(2)))); +v_truth.setitem(int32(1), 10*sin(x_truth.getitem(int32(2)))); +x_truth.setitem(int32(0), v_truth.getitem(int32(0)).primitive() + x0(1)); +x_truth.setitem(int32(1), v_truth.getitem(int32(1)).primitive() + x0(2)); + +% Bounded trajectories (dead reckoning) +v = TubeVector(tdomain, dt, int32(3)); +x = TubeVector(tdomain, dt, int32(3)); +v.setitem(int32(2), Tube(u, dt).inflate(i_n.rad())); % command u with bounded uncertainties +x.setitem(int32(2), Tube(x_truth.getitem(int32(2))+n_theta, dt).inflate(i_n.rad())); % heading measurement with bounded uncertainties +v.setitem(int32(0), 10*cos(x.getitem(int32(2)))); +v.setitem(int32(1), 10*sin(x.getitem(int32(2)))); +x = v.primitive()+IntervalVector(x0); % dead reckoning + +% Set of landmarks +v_m = { py.list([6,12]), py.list([-2,-5]), py.list([-3,20]), py.list([3,4]) }; + +% =========== GRAPHICS =========== + +beginDrawing(); + +fig_map = VIBesFigMap('slam'); +fig_map.set_properties(int32(50), int32(50), int32(1200), int32(600)); +fig_map.add_tube(x, 'x', int32(0), int32(1)); +fig_map.add_trajectory(x_truth, 'truth', int32(0), int32(1), 'white'); +fig_map.smooth_tube_drawing(true); +fig_map.add_landmarks(py.list(v_m), single(0.4)); +fig_map.show(double(1)); + +% =========== CONTRACTOR NETWORK =========== + +v_m_boxes = cell(size(v_m)); +for i=1:length(v_m) + v_m_boxes(i) = {IntervalVector(int32(2))}; +end + +% Contractor Network: + +cn = ContractorNetwork(); + +t = tdomain.lb(); +prev_t_obs = t; + +while t < tdomain.ub() + + if t-prev_t_obs > 2*dt % new observation each 2*delta + + % Creating new observation to a random landmark + + landmark_id = randi([1 length(v_m)]); % a random landmark is perceived + + xt = double(x_truth(t)); + pos_x = [xt(1), xt(2)]; + pos_b = double(v_m{landmark_id}); + + yi = Interval(sqrt((pos_x(1)-pos_b(1))^2+(pos_x(2)-pos_b(2))^2)); + yi.inflate(0.03); % adding range bounded uncertainty + + prev_t_obs = t; + + % Adding related observation constraints to the network + + % Alias (for ease of reading) + b = v_m_boxes{landmark_id}; + + % Intermediate variables + ti = Interval(t); + xi = IntervalVector(int32(3)); + + % Contractors + cn.add(CtcEval(), py.list({ti, xi, x, v})); + cn.add(CtcDist(), py.list({xi.getitem(int32(0)), xi.getitem(int32(1)), b.getitem(int32(0)), b.getitem(int32(1)), yi})); + + end + + contraction_dt = cn.contract_during(iteration_dt); + if iteration_dt>contraction_dt + pause(iteration_dt-contraction_dt); % iteration delay + end + + % Display the current slice x + fig_map.draw_box(x(t).subvector(int32(0),int32(1))); + + t = t + dt; + +end + +cn.contract(true); % lets the solver run the remaining contractions + +fig_map.show(); +for i=1:length(v_m_boxes) + b = v_m_boxes{i}; + fig_map.draw_box(b); +end + +endDrawing(); From a4500627cafd21a45b1d15bcb37352746482d33a Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 15 Oct 2023 01:36:55 +0200 Subject: [PATCH 6/6] Corrections in union and inter for MATLAB compatibility --- python/src/core/domains/tube/codac_py_Tube.cpp | 12 ++++++------ python/src/core/domains/tube/codac_py_TubeVector.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/python/src/core/domains/tube/codac_py_Tube.cpp b/python/src/core/domains/tube/codac_py_Tube.cpp index 8892ebd9b..a77f3a8db 100644 --- a/python/src/core/domains/tube/codac_py_Tube.cpp +++ b/python/src/core/domains/tube/codac_py_Tube.cpp @@ -531,13 +531,13 @@ void export_Tube(py::module& m) .def("__ror__", [](const Tube& y, double x) { return x|y; }) // For MATLAB compatibility. - .def("union", [](const Tube& y, double x) { return x|y; }) + .def("union", [](double x, const Tube& y) { return x|y; }) .def("__ror__", [](const Tube& y, const Interval& x) { return x|y; }) // For MATLAB compatibility. - .def("union", [](const Tube& y, const Interval& x) { return x|y; }) + .def("union", [](const Interval& x, const Tube& y) { return x|y; }) .def("__ror__", [](const Tube& y, const Trajectory& x) { return x|y; }) // For MATLAB compatibility. - .def("union", [](const Tube& y, const Trajectory& x) { return x|y; }) + .def("union", [](const Trajectory& x, const Tube& y) { return x|y; }) .def("__and__", [](const Tube& x, const Tube& y) { return x&y; }) // For MATLAB compatibility. @@ -554,12 +554,12 @@ void export_Tube(py::module& m) .def("__rand__", [](const Tube& y, double x) { return x&y; }) // For MATLAB compatibility. - .def("inter", [](const Tube& y, double x) { return x&y; }) + .def("inter", [](double x, const Tube& y) { return x&y; }) .def("__rand__", [](const Tube& y, const Interval& x) { return x&y; }) // For MATLAB compatibility. - .def("inter", [](const Tube& y, const Interval& x) { return x&y; }) + .def("inter", [](const Interval& x, const Tube& y) { return x&y; }) .def("__rand__", [](const Tube& y, const Trajectory& x) { return x&y; }) // For MATLAB compatibility. - .def("inter", [](const Tube& y, const Trajectory& x) { return x&y; }) + .def("inter", [](const Trajectory& x, const Tube& y) { return x&y; }) ; } \ No newline at end of file diff --git a/python/src/core/domains/tube/codac_py_TubeVector.cpp b/python/src/core/domains/tube/codac_py_TubeVector.cpp index 720237e8a..06078f9a8 100644 --- a/python/src/core/domains/tube/codac_py_TubeVector.cpp +++ b/python/src/core/domains/tube/codac_py_TubeVector.cpp @@ -603,10 +603,10 @@ void export_TubeVector(py::module& m) .def("__ror__", [](const TubeVector& y, const IntervalVector& x) { return x|y; }) // For MATLAB compatibility. - .def("union", [](const TubeVector& y, const IntervalVector& x) { return x|y; }) + .def("union", [](const IntervalVector& x, const TubeVector& y) { return x|y; }) .def("__ror__", [](const TubeVector& y, const TrajectoryVector& x) { return x|y; }) // For MATLAB compatibility. - .def("union", [](const TubeVector& y, const TrajectoryVector& x) { return x|y; }) + .def("union", [](const TrajectoryVector& x, const TubeVector& y) { return x|y; }) .def("__and__", [](const TubeVector& x, const TubeVector& y) { return x&y; }) // For MATLAB compatibility. @@ -620,9 +620,9 @@ void export_TubeVector(py::module& m) .def("__rand__", [](const TubeVector& y, const IntervalVector& x) { return x&y; }) // For MATLAB compatibility. - .def("inter", [](const TubeVector& y, const IntervalVector& x) { return x&y; }) + .def("inter", [](const IntervalVector& x, const TubeVector& y) { return x&y; }) .def("__rand__", [](const TubeVector& y, const TrajectoryVector& x) { return x&y; }) // For MATLAB compatibility. - .def("inter", [](const TubeVector& y, const TrajectoryVector& x) { return x&y; }) + .def("inter", [](const TrajectoryVector& x, const TubeVector& y) { return x&y; }) ; } \ No newline at end of file