From 194dc491660f80476cb57e6f031d5bcba17c10cd Mon Sep 17 00:00:00 2001 From: godardma Date: Wed, 14 Jan 2026 15:14:56 +0100 Subject: [PATCH 1/4] [tuto] correction for lesson A --- doc/manual/tuto/cp_robotics/src/lesson_A.asv | 13 +++++++++++ doc/manual/tuto/cp_robotics/src/lesson_A.cpp | 2 +- doc/manual/tuto/cp_robotics/src/lesson_A.m | 23 ++++++++++++++++++++ doc/manual/tuto/cp_robotics/src/lesson_A.py | 2 +- 4 files changed, 38 insertions(+), 2 deletions(-) create mode 100644 doc/manual/tuto/cp_robotics/src/lesson_A.asv create mode 100644 doc/manual/tuto/cp_robotics/src/lesson_A.m diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.asv b/doc/manual/tuto/cp_robotics/src/lesson_A.asv new file mode 100644 index 000000000..5409d4a15 --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.asv @@ -0,0 +1,13 @@ +import py.codac4matlab.* + +% [A-q2-beg] +x_truth = Vector([2,1,PI/6.0]); +y_truth = Vector([6,PI/6.0]); +m_truth = Vector([5,6]); +% [A-q2-end] + +% [A-q3-beg] +x = cart_prod(Interval(-oo,oo),Interval(-oo,oo),x_truth(3)); +m = IntervalVector(m_truth).inflate(0.2); +y = IntervalVector(y_truth).inflate(Vector([])); +% [A-q3-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.cpp b/doc/manual/tuto/cp_robotics/src/lesson_A.cpp index 98c9f7b16..c17dfbce6 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.cpp +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.cpp @@ -6,7 +6,7 @@ int main() // [A-q2-beg] Vector x_truth({2,1,PI/6}); Vector y_truth({6,PI/6}); - Vector m_truth({5,6}); + Vector m_truth({5,6.2}); // [A-q2-end] // [A-q3-beg] diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.m b/doc/manual/tuto/cp_robotics/src/lesson_A.m new file mode 100644 index 000000000..8902c9598 --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.m @@ -0,0 +1,23 @@ +import py.codac4matlab.* + +% [A-q2-beg] +x_truth = Vector([2.,1.,PI/6.0]); +y_truth = Vector([6.,PI/6.0]); +m_truth = Vector([5.,6.]); +% [A-q2-end] + +% [A-q3-beg] +x = cart_prod(Interval(-oo,oo),Interval(-oo,oo),x_truth(3)); +m = IntervalVector(m_truth).inflate(0.2); +y = IntervalVector(y_truth).inflate(Vector([0.001,0.1])); +% [A-q3-end] + +test = m - x_truth.subvector(1,2) + +DefaultFigure().draw_box(m,Color().red()); +DefaultFigure().draw_tank(x_truth, 1, StyleProperties({Color().black(),Color().yellow()})); + +% [A-q5-beg] +DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1), x_truth(3)+y(2), Color().red()) +% DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1) | 0, x_truth(3)+y(2), Color().light_gray()) +% [A-q5-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.py b/doc/manual/tuto/cp_robotics/src/lesson_A.py index 08d8cca4c..b1d4479b5 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.py +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.py @@ -3,7 +3,7 @@ # [A-q2-beg] x_truth = Vector([2,1,PI/6]) y_truth = Vector([6,PI/6]) -m_truth = Vector([5,6]) +m_truth = Vector([5,6.2]) # [A-q2-end] # [A-q3-beg] From 32191a3f832bbd992046b79290311fcc5c2e806c Mon Sep 17 00:00:00 2001 From: godardma Date: Thu, 15 Jan 2026 13:29:58 +0100 Subject: [PATCH 2/4] [doc] update for intervals --- doc/manual/manual/intervals/Interval_class.rst | 3 ++- doc/manual/manual/intervals/src.cpp | 1 + doc/manual/manual/intervals/src.py | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/manual/manual/intervals/Interval_class.rst b/doc/manual/manual/intervals/Interval_class.rst index f7493db6e..25aafb0c3 100644 --- a/doc/manual/manual/intervals/Interval_class.rst +++ b/doc/manual/manual/intervals/Interval_class.rst @@ -248,10 +248,11 @@ Mathematical functions such as :math:`\sin`, :math:`\cos`, :math:`\exp`, :math:` .. code-tab:: matlab - x = Interval.half_pi(); + x = Interval().half_pi(); x.self_union(0); % x = [0, π/2] y = sin(x); % y = [0, 1] z = exp(x); % z = [1, exp(π/2)] + w = y.inter(z); % w = [1, 1] For a complete list of additional operations, see the page :ref:`sec-functions-analytic-operators`. diff --git a/doc/manual/manual/intervals/src.cpp b/doc/manual/manual/intervals/src.cpp index a2da233cd..35bf55fab 100644 --- a/doc/manual/manual/intervals/src.cpp +++ b/doc/manual/manual/intervals/src.cpp @@ -87,6 +87,7 @@ TEST_CASE("Interval class - manual") x |= 0; // x = [0, π/2] Interval y = sin(x); // y = [0, 1] Interval z = exp(x); // z = [1, e^(π/2)] + Interval w = y & z; // w = [1, 1] // [interval-class-6-end] CHECK(Approx(x) == Interval(0,PI/2)); CHECK(Approx(y) == Interval(0,1)); diff --git a/doc/manual/manual/intervals/src.py b/doc/manual/manual/intervals/src.py index 960ededa6..a5b7869a8 100644 --- a/doc/manual/manual/intervals/src.py +++ b/doc/manual/manual/intervals/src.py @@ -71,6 +71,7 @@ def tests_Interval_manual(test): x |= 0 # x = [0, π/2] y = sin(x) # y = [0, 1] z = exp(x) # z = [1, e^(π/2)] + w = y & z # w = [1, 1] # [interval-class-6-end] test.assertTrue(Approx(x) == Interval(0,PI/2)) test.assertTrue(Approx(y) == Interval(0,1)) From e9cb6c8d0a9d297ee459db549f6eec124ebd9598 Mon Sep 17 00:00:00 2001 From: godardma Date: Fri, 16 Jan 2026 18:28:41 +0100 Subject: [PATCH 3/4] [doc] update for matlab + lesson A for tuto --- doc/manual/tuto/cp_robotics/src/lesson_A.asv | 13 ----- doc/manual/tuto/cp_robotics/src/lesson_A.m | 53 ++++++++++++++++--- examples/11_peibos/main.m | 16 +++--- .../src/core/matrices/codac2_py_MatrixBase.h | 12 ++--- .../src/core/matrices/codac2_py_VectorBase.h | 2 +- 5 files changed, 60 insertions(+), 36 deletions(-) delete mode 100644 doc/manual/tuto/cp_robotics/src/lesson_A.asv diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.asv b/doc/manual/tuto/cp_robotics/src/lesson_A.asv deleted file mode 100644 index 5409d4a15..000000000 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.asv +++ /dev/null @@ -1,13 +0,0 @@ -import py.codac4matlab.* - -% [A-q2-beg] -x_truth = Vector([2,1,PI/6.0]); -y_truth = Vector([6,PI/6.0]); -m_truth = Vector([5,6]); -% [A-q2-end] - -% [A-q3-beg] -x = cart_prod(Interval(-oo,oo),Interval(-oo,oo),x_truth(3)); -m = IntervalVector(m_truth).inflate(0.2); -y = IntervalVector(y_truth).inflate(Vector([])); -% [A-q3-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_A.m b/doc/manual/tuto/cp_robotics/src/lesson_A.m index 8902c9598..0982505d3 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.m +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.m @@ -3,21 +3,60 @@ % [A-q2-beg] x_truth = Vector([2.,1.,PI/6.0]); y_truth = Vector([6.,PI/6.0]); -m_truth = Vector([5.,6.]); +m_truth = Vector([5.,6.2]); % [A-q2-end] % [A-q3-beg] x = cart_prod(Interval(-oo,oo),Interval(-oo,oo),x_truth(3)); m = IntervalVector(m_truth).inflate(0.2); -y = IntervalVector(y_truth).inflate(Vector([0.001,0.1])); +y = IntervalVector(y_truth).inflate(Vector([0.3,0.1])); % [A-q3-end] -test = m - x_truth.subvector(1,2) - DefaultFigure().draw_box(m,Color().red()); DefaultFigure().draw_tank(x_truth, 1, StyleProperties({Color().black(),Color().yellow()})); % [A-q5-beg] -DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1), x_truth(3)+y(2), Color().red()) -% DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1) | 0, x_truth(3)+y(2), Color().light_gray()) -% [A-q5-end] \ No newline at end of file +DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1), x_truth(3)+y(2), Color().red()); +DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1).union(0), x_truth(3)+y(2), Color().light_gray()); +% [A-q5-end] + +% [A-q6-beg] +ctc_polar = CtcPolar(); + +x123 = VectorVar(7); +f_minus = AnalyticFunction({x123},vec(x123(1)-x123(3)-x123(6), x123(2)-x123(4)-x123(7))); +ctc_minus = CtcInverse(f_minus, Vector([0,0])); + +s =VectorVar(3); +f_plus = AnalyticFunction({s}, s(1)+s(2)-s(3)); +ctc_plus = CtcInverse(f_plus, Interval(0,0)); +% [A-q6-end] + +% [A-q7-beg] +a = Interval(); +d = IntervalVector(2); +% [A-q7-end] + +% [A-q8-beg] +% Either with a smart order of contractor calls: +res_ctc_plus = ctc_plus.contract(cart_prod(x(3), y(2), a)); % The result is a 3D IntervalVector +x.setitem(3,res_ctc_plus(1)); +y.setitem(2,res_ctc_plus(2)); +a = res_ctc_plus(3); + +res_ctc_polar = ctc_polar.contract(cart_prod(d(1),d(2),y(1),a)); % The result is a 4D IntervalVector +d.setitem(1,res_ctc_polar(1)); +d.setitem(2,res_ctc_polar(2)); +y.setitem(1,res_ctc_polar(3)); +a = res_ctc_polar(4); + +res_ctc_minus = ctc_minus.contract(cart_prod(m,x,d)); % The result is a 7D IntervalVector +m = res_ctc_minus.subvector(1,2); +x = res_ctc_minus.subvector(3,5); +d = res_ctc_minus.subvector(6,7); +% [A-q8-end] + +% [A-q9-beg] +x +DefaultFigure().draw_box(x) % does not display anything if unbounded +% [A-q9-end] \ No newline at end of file diff --git a/examples/11_peibos/main.m b/examples/11_peibos/main.m index 6db6169f8..332930011 100644 --- a/examples/11_peibos/main.m +++ b/examples/11_peibos/main.m @@ -16,8 +16,8 @@ X_2d = VectorVar(1); psi0_2d = AnalyticFunction({X_2d},vec(sin(X_2d(1)*PI/4.),cos(X_2d(1)*PI/4.))); -id_2d = OctaSym(int64([1,2])); -s = OctaSym(int64([-2,1])); +id_2d = OctaSym([1,2]); +s = OctaSym([-2,1]); v_par_2d = PEIBOS(f_2d,psi0_2d,{id_2d,s,s*s,s.invert()},0.2,Vector([-0.2,0.]),true); @@ -45,9 +45,9 @@ X_3d = VectorVar(2); psi0_3d = AnalyticFunction({X_3d},vec(1/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2))),X_3d(1)/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2))),X_3d(2)/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2))))); -id_3d = OctaSym(int64([1,2,3])); -s1 = OctaSym(int64([-2, 1, 3])); -s2 = OctaSym(int64([3, 2, -1])); +id_3d = OctaSym([1, 2, 3]); +s1 = OctaSym([-2, 1, 3]); +s2 = OctaSym([3, 2, -1]); v_par_3d = PEIBOS(f_3d,psi0_3d,{id_3d,s1,s1*s1,s1.invert(),s2,s2.invert()},0.2,true); @@ -76,9 +76,9 @@ X_nd = VectorVar(1); psi0_nd = AnalyticFunction({X_nd},vec(X_nd(1),1,1)); -id_nd = OctaSym(int64([1, 2, 3])); -s1_nd = OctaSym(int64([-2, 1, 3])); -s2_nd = OctaSym(int64([3, 2, -1])); +id_nd = OctaSym([1, 2, 3]); +s1_nd = OctaSym([-2, 1, 3]); +s2_nd = OctaSym([3, 2, -1]); figure_3d_nd = Figure3D("Cube on Sphere matlab"); figure_3d_nd.draw_axes(0.5); diff --git a/python/src/core/matrices/codac2_py_MatrixBase.h b/python/src/core/matrices/codac2_py_MatrixBase.h index b0bdaa0c4..980dd123f 100644 --- a/python/src/core/matrices/codac2_py_MatrixBase.h +++ b/python/src/core/matrices/codac2_py_MatrixBase.h @@ -119,15 +119,13 @@ void export_MatrixBase(py::module& m, py::class_& pyclass) #else "__setitem__" #endif - , [](S& x, const py::tuple& ij, const T& a) + , [](S& x, std::vector ij, const T& a) { - if constexpr(FOR_MATLAB) - assert_release(py::isinstance(ij[0]) && py::isinstance(ij[1])); - - int i = ij[0].cast(); - int j = ij[1].cast(); + auto ij_conv = matlab::convert_indices(ij); + int i = ij_conv[0]; + int j = ij_conv[1]; - x(matlab::input_index(i), matlab::input_index(j)) = a; + x(i, j) = a; }, MATRIX_ADDONS_BASE_SCALAR_REF_OPERATORCALL_INDEX_INDEX) diff --git a/python/src/core/matrices/codac2_py_VectorBase.h b/python/src/core/matrices/codac2_py_VectorBase.h index 848c6a0c6..9b67c698f 100644 --- a/python/src/core/matrices/codac2_py_VectorBase.h +++ b/python/src/core/matrices/codac2_py_VectorBase.h @@ -55,7 +55,7 @@ void export_VectorBase([[maybe_unused]] py::module& m, py::class_& pyclass) .def( #if FOR_MATLAB - "__call__" + "setitem" #else "__setitem__" #endif From 031d1eef20ba9dc9e0feb1fd3e3fd692ac800b14 Mon Sep 17 00:00:00 2001 From: godardma Date: Fri, 16 Jan 2026 18:38:34 +0100 Subject: [PATCH 4/4] [doc] completing tuto lessonA with matlab code --- .../lesson_a_static_range_and_bearing.rst | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/doc/manual/tuto/cp_robotics/lesson_a_static_range_and_bearing.rst b/doc/manual/tuto/cp_robotics/lesson_a_static_range_and_bearing.rst index 56f8e3117..671f06655 100644 --- a/doc/manual/tuto/cp_robotics/lesson_a_static_range_and_bearing.rst +++ b/doc/manual/tuto/cp_robotics/lesson_a_static_range_and_bearing.rst @@ -286,6 +286,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal :end-before: [A-q2-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q2-beg] + :end-before: [A-q2-end] + :dedent: 0 + **A.3.** Create the bounded sets related to the state, the measurement and the landmark position: :math:`[\mathbf{x}]\in\mathbb{IR}^3`, :math:`[\mathbf{y}]\in\mathbb{IR}^2`, :math:`[\mathbf{m}]\in\mathbb{IR}^2`. We can for instance use the ``.inflate(float radius)`` method on intervals or boxes. @@ -321,6 +329,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal :end-before: [A-q3-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q3-beg] + :end-before: [A-q3-end] + :dedent: 0 + **A.4.** Display the vehicle and the landmark with: .. tabs:: @@ -374,6 +390,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal :end-before: [A-q5-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q5-beg] + :end-before: [A-q5-end] + :dedent: 0 + As one can see, intervals are not limited to axis-aligned boxes: we sometimes perform rotational mapping to better fit the set to represent. This polar constraint is a case in point. @@ -411,6 +435,14 @@ We will implement the decomposition of Question **A.1** using contractors and a :end-before: [A-q6-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q6-beg] + :end-before: [A-q6-end] + :dedent: 0 + | **A.7.** Create the intermediate variables introduced in Question **A.1**. They are ``Interval`` and ``IntervalVector`` objects, as for the other variables. | Note that the intermediate variables do not have to be initialized with prior values. For ``IntervalVector`` objects, you only have to define their size. @@ -434,6 +466,14 @@ We will implement the decomposition of Question **A.1** using contractors and a :end-before: [A-q7-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q7-beg] + :end-before: [A-q7-end] + :dedent: 0 + The current problem can be solved in one step with a smart order of contractors. You may find this order and run the contractors without iterative loop. However, this order may not always exist (due to dependencies in the constraints) or, in a pure declarative paradigm, we may not want to think about it. Using fixed-point iteration can then simplify the algorithm. In Codac, this is allowed using the ``fixpoint(..)`` function that will execute some ``contractors_list`` function until a fixpoint is reached. The `fixpoint` corresponds to a point where the involved domains are not contracted anymore. The following code illustrates how to implement such fixpoint: @@ -485,6 +525,14 @@ The ``fixpoint`` function will execute the content of the function ``contractors :end-before: [A-q8-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q8-beg] + :end-before: [A-q8-end] + :dedent: 0 + | **A.9.** Run your programm to solve the problem. You should obtain this figure: .. figure:: img/result_rangebearing.png @@ -510,6 +558,14 @@ The ``fixpoint`` function will execute the content of the function ``contractors :end-before: [A-q9-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_A.m + :language: matlab + :start-after: [A-q9-beg] + :end-before: [A-q9-end] + :dedent: 0 + The black box :math:`[\mathbf{x}]` cumulates all the uncertainties of the problem: * uncertainties of the measurement