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)) 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 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..0982505d3 --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.m @@ -0,0 +1,62 @@ +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.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.3,0.1])); +% [A-q3-end] + +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).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/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] 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