diff --git a/.gitignore b/.gitignore index 23d552616..c35bf0562 100644 --- a/.gitignore +++ b/.gitignore @@ -69,4 +69,7 @@ wheelhouse/ # Graphics files examples/**/*.xml -*.ipe \ No newline at end of file +*.ipe + +# Matlab files +*.asv diff --git a/doc/manual/manual/contractors/analytic/ctcinverse.rst b/doc/manual/manual/contractors/analytic/ctcinverse.rst index 600c3bf2a..cbc6aeef5 100644 --- a/doc/manual/manual/contractors/analytic/ctcinverse.rst +++ b/doc/manual/manual/contractors/analytic/ctcinverse.rst @@ -84,6 +84,14 @@ To represent the vectors :math:`\mathbf{x}\in\mathbb{R}^n` consistent with the c :end-before: [ctcinv-1-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-1-beg] + :end-before: [ctcinv-1-end] + :dedent: 0 + The contractor can be used as an operator to contract a 2d box :math:`[\mathbf{x}]`. It can also be involved in a paver in order to reveal the constraint: .. tabs:: @@ -104,6 +112,14 @@ The contractor can be used as an operator to contract a 2d box :math:`[\mathbf{x :end-before: [ctcinv-2-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-2-beg] + :end-before: [ctcinv-2-end] + :dedent: 0 + Which produces the following output: .. figure:: ./himmelblau_50.png @@ -131,6 +147,15 @@ We recall that for thick solution sets, one should prefer the use of the ``SepIn :end-before: [ctcinv-3-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-3-beg] + :end-before: [ctcinv-3-end] + :dedent: 0 + + .. figure:: ./himmelblau_50_inner.png :width: 400px @@ -163,6 +188,14 @@ can be easily approximated by the following union of contractors: :end-before: [ctcinv-4-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-4-beg] + :end-before: [ctcinv-4-end] + :dedent: 0 + .. figure:: ./himmelblau_50_150_250.png :width: 400px @@ -302,6 +335,14 @@ When the constraint is a complement constraint :math:`\mathbf{f}(\mathbf{x})\not :end-before: [ctcinv-5-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-5-beg] + :end-before: [ctcinv-5-end] + :dedent: 0 + Miscellaneous ------------- @@ -309,7 +350,7 @@ Miscellaneous Access to the underlying function ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The underlying analytic function can be accessed through ``.function()`` (useful for dimension checks or meta-programming). +The underlying analytic function can be accessed through ``.f()`` (useful for dimension checks or meta-programming). .. tabs:: @@ -329,6 +370,14 @@ The underlying analytic function can be accessed through ``.function()`` (useful :end-before: [ctcinv-6-end] :dedent: 4 + .. group-tab:: Matlab + + .. literalinclude:: src.m + :language: matlab + :start-after: [ctcinv-6-beg] + :end-before: [ctcinv-6-end] + :dedent: 0 + Centered form option ^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/manual/manual/contractors/analytic/src.cpp b/doc/manual/manual/contractors/analytic/src.cpp index 1d9d4cf02..ab3e7b17f 100644 --- a/doc/manual/manual/contractors/analytic/src.cpp +++ b/doc/manual/manual/contractors/analytic/src.cpp @@ -64,11 +64,11 @@ TEST_CASE("CtcInverse - manual") VectorVar x(2); AnalyticFunction f({x}, x[0]-x[1]); CtcInverse c(f, 0); - // c.function().input_size() == 2 - // c.function().output_size() == 1 + // c.f().input_size() == 2 + // c.f().output_size() == 1 // [ctcinv-6-end] - CHECK(c.function().input_size() == 2); - CHECK(c.function().output_size() == 1); + CHECK(c.f().input_size() == 2); + CHECK(c.f().output_size() == 1); } } \ No newline at end of file diff --git a/doc/manual/manual/contractors/analytic/src.m b/doc/manual/manual/contractors/analytic/src.m new file mode 100644 index 000000000..7c5526fe3 --- /dev/null +++ b/doc/manual/manual/contractors/analytic/src.m @@ -0,0 +1,53 @@ +% Codac tests +% ---------------------------------------------------------------------------- +% \date 2026 +% \author Simon Rohou +% \copyright Copyright 2026 Codac Team +% \license GNU Lesser General Public License (LGPL) + +import py.codac4matlab.* + +% [ctcinv-1-beg] +% Example of Himmelblau's function +a = 11.0; +b = 7.0; +x = VectorVar(2); +f = AnalyticFunction({x}, sqr(sqr(x(1))+x(2)-a)+sqr(x(1)+sqr(x(2))-b)); +c = CtcInverse(f,50.0); +% [ctcinv-1-end] + +% [ctcinv-2-beg] +DefaultFigure().pave(IntervalVector({{-6,6},{-6,6}}), c, 1e-2); +% [ctcinv-2-end] + +% [ctcinv-3-beg] +s = SepInverse(f, Interval(0,50)); +DefaultFigure().pave(IntervalVector({{-6,6},{-6,6}}), s, 1e-2); +% [ctcinv-3-end] + +% [ctcinv-4-beg] +cu = CtcUnion(CtcUnion(CtcInverse(f,50), CtcInverse(f,150)), CtcInverse(f,250)); +DefaultFigure().pave(IntervalVector({{-6,6},{-6,6}}), cu, 1e-2) +% [ctcinv-4-end] + +% [ctcinv-5-beg] +x = VectorVar(2); +f = AnalyticFunction({x}, x(1)); + +% Enforce the first component not in [0,1] +c = CtcInverseNotIn(f, Interval(0,1)); + +y = IntervalVector({{0.5,3},{-1,1}}); +c.contract(y); % [[1,3],[-1,1]] +% Only the first component is constrained by the not-in condition +% [ctcinv-5-end] + +assert(y==IntervalVector({{1,3},{-1,1}})); + +% [ctcinv-6-beg] +x = VectorVar(2); +f = AnalyticFunction({x}, x(1)-x(2)); +c = CtcInverse(f, 0); +assert(c.f().input_size()==2); +assert(c.f().output_size()==1); +% [ctcinv-6-end] \ No newline at end of file diff --git a/doc/manual/manual/contractors/analytic/src.py b/doc/manual/manual/contractors/analytic/src.py index 34fe2214a..edfefb444 100644 --- a/doc/manual/manual/contractors/analytic/src.py +++ b/doc/manual/manual/contractors/analytic/src.py @@ -56,8 +56,8 @@ def tests_CtcInverse_manual(test): x = VectorVar(2) f = AnalyticFunction([x], x[0]-x[1]) c = CtcInverse(f, 0) - assert c.function().input_size() == 2 - assert c.function().output_size() == 1 + assert c.f().input_size() == 2 + assert c.f().output_size() == 1 # [ctcinv-6-end] if __name__ == '__main__': diff --git a/doc/manual/tuto/cp_robotics/doc/Robot_simulation.pdf b/doc/manual/tuto/cp_robotics/doc/Robot_simulation.pdf new file mode 100644 index 000000000..0342591df Binary files /dev/null and b/doc/manual/tuto/cp_robotics/doc/Robot_simulation.pdf differ diff --git a/doc/manual/tuto/cp_robotics/img/CtcConstell_constell.png b/doc/manual/tuto/cp_robotics/img/CtcConstell_constell.png index 7b10a717d..96238d82b 100644 Binary files a/doc/manual/tuto/cp_robotics/img/CtcConstell_constell.png and b/doc/manual/tuto/cp_robotics/img/CtcConstell_constell.png differ diff --git a/doc/manual/tuto/cp_robotics/img/datasso_solved.png b/doc/manual/tuto/cp_robotics/img/datasso_solved.png index 0a4eac547..0c0c88de3 100644 Binary files a/doc/manual/tuto/cp_robotics/img/datasso_solved.png and b/doc/manual/tuto/cp_robotics/img/datasso_solved.png differ diff --git a/doc/manual/tuto/cp_robotics/img/first_result.gif b/doc/manual/tuto/cp_robotics/img/first_result.gif index d28545979..311baae78 100644 Binary files a/doc/manual/tuto/cp_robotics/img/first_result.gif and b/doc/manual/tuto/cp_robotics/img/first_result.gif differ diff --git a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks.png b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks.png index b181ca8c9..f9dea65b8 100644 Binary files a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks.png and b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks.png differ diff --git a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs.png b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs.png index 501a38a8a..649783669 100644 Binary files a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs.png and b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs.png differ diff --git a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box.png b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box.png index aaee8876b..8ef2d4d67 100644 Binary files a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box.png and b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box.png differ diff --git a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box_id.png b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box_id.png index 006ceb481..76378ba93 100644 Binary files a/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box_id.png and b/doc/manual/tuto/cp_robotics/img/loc_robot_landmarks_obs_box_id.png differ diff --git a/doc/manual/tuto/cp_robotics/img/result_rangebearing.png b/doc/manual/tuto/cp_robotics/img/result_rangebearing.png index 10aeb528a..4fcff6dd0 100644 Binary files a/doc/manual/tuto/cp_robotics/img/result_rangebearing.png and b/doc/manual/tuto/cp_robotics/img/result_rangebearing.png differ 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 671f06655..b07210d4c 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 @@ -128,6 +128,31 @@ where :math:`a,b,\dots,e` are intermediate variables used for the decomposition. Interval d(4.5,5); // interval for y ctc_g.contract(a,d,b); + .. code-tab:: matlab + + import py.codac4matlab.* + + % Symbolic variables: + y = ScalarVar(); + [x,m] = deal(VectorVar(2),VectorVar(2)); + + % Analytic scalar function g(x,m,y) involved in the constraint: + g = AnalyticFunction({x,y,m}, sqrt(((x(1)-m(1))^2)+((x(2)-m(2))^2))-y); + + % Contractor associated with the constraint g(x,m,y)\in[u], with [u]=[0,0] + ctc_g = CtcInverse(g, 0); + + % Now ctc_g can be called with the .contract(..) method to contract all domains: + % Example: + a = IntervalVector(2); % box for x + b = IntervalVector({{2,3},{5,6.2}}); % box for m + d = Interval(4.5,5); % interval for y + + res = ctc_g.contract(cart_prod(a,d,b)); + a = res.subvector(1,2); + b = res.subvector(3,4); + c = res(5); + Optimality of contractors ------------------------- @@ -176,6 +201,17 @@ could be implemented by // Contractor associated with the constraint g(x,m,y)\in[u] with [u]=[[0,0],[0,0]] CtcInverse ctc_g(g, {0,0}) // {0,0} is equivalent to Vector::zero(2) + .. code-tab:: matlab + + % Symbolic variables: + [x,m,y] = deal(VectorVar(3),VectorVar(2),VectorVar(2)); + + % Analytic vectorial function g(x,y,m) involved in the constraint: + g = AnalyticFunction({x,y,m}, vec(x(1)+y(1)*cos(x(3)+y(2))-m(1), x(2)+y(1)*sin(x(3)+y(2))-m(2))); + + % Contractor associated with the constraint g(x,m,y)\in[u] with [u]=[[0,0],[0,0]] + ctc_g = CtcInverse(g, Vector([0,0])); % [0,0] is equivalent to Vector.zero(2) + However, this involves a multi-occurrence of variables which leads to pessimism. For instance, the sum :math:`(x_3+y_2)` appears twice in functions :math:`\cos` and :math:`\sin`, which is hardly handled by a classical decomposition. @@ -266,6 +302,13 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal Vector x_truth = {2,1,PI/6}; // actual state vector (pose = position + bearing) + .. code-tab:: matlab + + % We recall that you can use the Vector class for + % representing mathematical vectors. For instance: + + x_truth = Vector([2,1,PI/6]); % actual state vector (pose = position + bearing) + .. container:: toggle, toggle-hidden .. tabs:: @@ -309,6 +352,10 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal x[2] &= x_truth[2]; // the heading is assumed to be known + .. code-tab:: matlab + + x(3).self_inter(x_truth(3)); % the heading is assumed to be known + .. container:: toggle, toggle-hidden .. tabs:: @@ -351,6 +398,11 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal DefaultFigure::draw_tank(x_truth, 1, {Color::black(),Color::yellow()}); // robot's size is 1 DefaultFigure::draw_box(m, Color::red()); + .. code-tab:: matlab + + DefaultFigure().draw_tank(x_truth, 1, StyleProperties({Color().black(),Color().yellow()})); + DefaultFigure().draw_box(m,Color().red()); + **A.5.** Display the range-and-bearing measurement with its uncertainties. For this, we will use the function ``.draw_pie(, <[rho]>, <[theta]>)`` to display a portion of a ring :math:`[\rho]\times[\theta]` centered on :math:`(x,y)^\intercal`. Here, we must add in :math:`[\theta]` the robot heading :math:`x_3` and the bounded bearing :math:`[y_2]`. You should obtain this figure: @@ -370,6 +422,10 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal DefaultFigure::draw_pie({, }, <[rho]> | 0, <[theta]>, Color::light_gray()); // with: <[rho]> | 0 + .. code-tab:: matlab + + DefaultFigure().draw_pie(Vector([, ]), <[rho]>.union(0), <[theta]>, Color().light_gray()); % with: <[rho]> | 0 + .. container:: toggle, toggle-hidden .. tabs:: @@ -499,6 +555,10 @@ The following code illustrates how to implement such fixpoint: etc. }, ); + .. code-tab:: matlab + + % Not supported yet + The ``fixpoint`` function will execute the content of the function ``contractors_list`` until there are no more contractions on the sets listed in ````. .. admonition:: Exercise diff --git a/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst b/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst index 5d145431b..e9ae10290 100644 --- a/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst +++ b/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst @@ -136,6 +136,25 @@ where :math:`\bigsqcup`, called *squared union*, returns the smallest box enclos const std::vector _M; }; + .. code-tab:: matlab + + classdef MyCtc < handle + + properties + M + end + + methods + function obj = MyCtc(M_) + obj.M = M_; + end + + function a = contract(obj, a) + % Insert contraction formula here + end + end + end + | Note that: * ``M`` represents the set :math:`\mathbb{M}`, made of 2d ``IntervalVector`` objects; @@ -169,6 +188,14 @@ where :math:`\bigsqcup`, called *squared union*, returns the smallest box enclos :end-before: [B-q2-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/MyCtc.m + :language: matlab + :start-after: [B-q2-beg] + :end-before: [B-q2-end] + :dedent: 0 + **B.3.** Test your contractor with: * a set :math:`\mathbb{M}=\big\{(1.5,2.5)^\intercal,(3,1)^\intercal,(2,2)^\intercal,(2.5,3)^\intercal,(3.5,2)^\intercal,(4,1)^\intercal,(1.5,0.5)^\intercal\big\}`, all boxes inflated by :math:`[-0.05,0.05]`; @@ -214,6 +241,24 @@ where :math:`\bigsqcup`, called *squared union*, returns the smallest box enclos ctc_constell.contract(a2); ctc_constell.contract(a3); + .. code-tab:: matlab + + M = {IntervalVector([...]),IntervalVector([...]),... + + for i = 1:numel(M) + M{i}.inflate(0.05); + end + + a1 = IntervalVector({... + a2 = IntervalVector({... + a3 = IntervalVector({... + + ctc_constell = MyCtc(M); + + a1 = ctc_constell.contract(a1) + a2 = ctc_constell.contract(a2) + a3 = ctc_constell.contract(a3) + .. container:: toggle, toggle-hidden .. tabs:: @@ -234,6 +279,14 @@ where :math:`\bigsqcup`, called *squared union*, returns the smallest box enclos :end-before: [B-q3-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q3-beg] + :end-before: [B-q3-end] + :dedent: 0 + .. figure:: img/CtcConstell_constell.png :width: 600px @@ -280,6 +333,14 @@ We will localize the robot in the map :math:`\mathbb{M}` created in Question **B :end-before: [B-q4-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q4-beg] + :end-before: [B-q4-end] + :dedent: 0 + **B.5.** Display the robot and the landmarks in a graphical view with: .. tabs:: @@ -300,6 +361,14 @@ We will localize the robot in the map :math:`\mathbb{M}` created in Question **B :end-before: [B-q5-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q5-beg] + :end-before: [B-q5-end] + :dedent: 0 + You should obtain this result. .. figure:: img/loc_robot_landmarks.png @@ -325,6 +394,14 @@ We will localize the robot in the map :math:`\mathbb{M}` created in Question **B :end-before: [B-q6-end] :dedent: 0 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q6-beg] + :end-before: [B-q6-end] + :dedent: 0 + The returned value is a vector of [range]×[bearing] interval values. Compute and display the measurements in the view with ``.draw_pie()``, as we did in the previous lesson. @@ -348,6 +425,14 @@ We will localize the robot in the map :math:`\mathbb{M}` created in Question **B :end-before: [B-q7-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q7-beg] + :end-before: [B-q7-end] + :dedent: 0 + You should obtain a figure similar to this one: .. figure:: img/loc_robot_landmarks_obs.png @@ -391,6 +476,14 @@ We will first reuse the fixpoint method of the previous Lesson, and apply it on :start-after: [B-q7b-beg] :end-before: [B-q7b-end] :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q7b-beg] + :end-before: [B-q7b-end] + :dedent: 0 **B.8.** Display the resulted contracted box :math:`[\mathbf{x}]` with: @@ -414,6 +507,14 @@ We will first reuse the fixpoint method of the previous Lesson, and apply it on :end-before: [B-q8-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q8-beg] + :end-before: [B-q8-end] + :dedent: 0 + You should obtain this result in black (considering uncertainties proposed in Question **B.6**): .. figure:: img/loc_robot_landmarks_obs_box.png @@ -453,6 +554,14 @@ We now assume that the identities of the landmarks are not known. This means tha :end-before: [B-q9-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q9-beg] + :end-before: [B-q9-end] + :dedent: 0 + **B.11.** *Same result* means that the data association worked: each measurement has been automatically associated to the correct landmark without ambiguity. @@ -480,6 +589,14 @@ We now assume that the identities of the landmarks are not known. This means tha :end-before: [B-q10-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_B.m + :language: matlab + :start-after: [B-q10-beg] + :end-before: [B-q10-end] + :dedent: 4 + Expected result: .. figure:: img/loc_robot_landmarks_obs_box_id.png diff --git a/doc/manual/tuto/cp_robotics/lesson_c_dynamic_localization.rst b/doc/manual/tuto/cp_robotics/lesson_c_dynamic_localization.rst index da89f4710..6a988cfd3 100644 --- a/doc/manual/tuto/cp_robotics/lesson_c_dynamic_localization.rst +++ b/doc/manual/tuto/cp_robotics/lesson_c_dynamic_localization.rst @@ -105,6 +105,14 @@ Before going into a set-membership state estimation, we set up a simulation envi :end-before: [C-q2-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q2-beg] + :end-before: [C-q2-end] + :dedent: 0 + **C.3.** Now, we simulate a mobile robot moving in the middle of the field of landmarks. The trajectory is defined by a set of random waypoints. The class ``RobotSimulator`` is used to simplify the simulation: .. tabs:: @@ -125,6 +133,14 @@ Before going into a set-membership state estimation, we set up a simulation envi :end-before: [C-q3-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q3-beg] + :end-before: [C-q3-end] + :dedent: 0 + **C.4.** We update the observation function ``g`` of the previous lessons for the dynamical case. To ease the code, the returned observation vector will contain the observation time :math:`t_i`. .. tabs:: @@ -145,6 +161,14 @@ Before going into a set-membership state estimation, we set up a simulation envi :end-before: [C-q4-end] :dedent: 0 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q4-beg] + :end-before: [C-q4-end] + :dedent: 0 + **C.5.** Finally, we run the simulation for generating and drawing the observation vectors. ``obs`` contains all the observation vectors. .. tabs:: @@ -165,6 +189,14 @@ Before going into a set-membership state estimation, we set up a simulation envi :end-before: [C-q5-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q5-beg] + :end-before: [C-q5-end] + :dedent: 0 + State estimation with constraint programming -------------------------------------------- @@ -191,6 +223,14 @@ State estimation with constraint programming :end-before: [C-q6-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q6-beg] + :end-before: [C-q6-end] + :dedent: 0 + In the above code, the intermediate variable :math:`\mathbf{v}(\cdot)` has also its own domain ``v``. It is initialized to :math:`[-\infty,\infty]` for each dimension. @@ -216,6 +256,14 @@ State estimation with constraint programming :end-before: [C-q7-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q7-beg] + :end-before: [C-q7-end] + :dedent: 0 + **C.8.** **Fixpoint resolution.** Finally, the propagation loops need to be updated to incorporate the dynamic constraints. Note that the contractors :math:`\mathcal{C}_\mathbf{f}` and :math:`\mathcal{C}_{\mathrm{deriv}}` apply to the whole tubes :math:`[\mathbf{x}](\cdot)` and :math:`[\mathbf{v}](\cdot)`. Furthermore, the class ``CtcInverse`` can contract tubes using the ``.contract_tube(..)`` method, exactly as we would do for boxes. Finally, a *restriction* on a tube (*i.e.* setting a value to a slice) can be done using the ``.set(y,t)`` method, for setting the interval vector value ``y`` at time ``t``. @@ -239,6 +287,14 @@ State estimation with constraint programming :end-before: [C-q8-end] :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_C.m + :language: matlab + :start-after: [C-q8-beg] + :end-before: [C-q8-end] + :dedent: 0 + @@ -248,6 +304,21 @@ You should obtain a result similar to: Localization by solving data association: the state trajectory :math:`\mathbf{x}(\cdot)` (in white) has been estimated (in blue) together with the identification of the perceived landmarks. +Note that to obtain this exact result it is possible to manually set the random seed to ``287``. + + .. tabs:: + + .. code-tab:: py + + srand(287) + + .. code-tab:: cpp + + srand(287); + + .. code-tab:: matlab + + srand(int32(287)); .. rubric:: Why is this problem of localization and data association difficult? diff --git a/doc/manual/tuto/cp_robotics/src/MyCtc.m b/doc/manual/tuto/cp_robotics/src/MyCtc.m new file mode 100644 index 000000000..866cfa1ad --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/MyCtc.m @@ -0,0 +1,26 @@ +classdef MyCtc < handle + + properties + M + end + + methods + function obj = MyCtc(M_) + obj.M = M_; + end + + function a = contract(obj, a) + % [B-q2-beg] + u = py.codac4matlab.IntervalVector().empty(int32(2)); + for i = 1:numel(obj.M) + mi = obj.M{i}; + u.self_union(a.inter(mi)); + end + a = u; + % [B-q2-end] + + % Insert contraction formula here (question B.2) + + end + end +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 0982505d3..fdf4e3885 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.m +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.m @@ -16,19 +16,19 @@ 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), 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))); +[x1,x2,x3] = deal(VectorVar(2), VectorVar(3), VectorVar(2)); +f_minus = AnalyticFunction({x1,x2,x3},vec(x1(1)-x2(1)-x3(1), x1(2)-x2(2)-x3(2))); ctc_minus = CtcInverse(f_minus, Vector([0,0])); -s =VectorVar(3); -f_plus = AnalyticFunction({s}, s(1)+s(2)-s(3)); +[s1,s2,s3] = deal(ScalarVar(),ScalarVar(),ScalarVar()); +f_plus = AnalyticFunction({s1,s2,s3}, s1+s2-s3); ctc_plus = CtcInverse(f_plus, Interval(0,0)); % [A-q6-end] @@ -39,6 +39,7 @@ % [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)); @@ -54,9 +55,41 @@ m = res_ctc_minus.subvector(1,2); x = res_ctc_minus.subvector(3,5); d = res_ctc_minus.subvector(6,7); + +% Or using a fixpoint method: +function [x,y,m,a,d] = contract(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus) + res_ctc_plus = ctc_plus.contract(py.codac4matlab.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(py.codac4matlab.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(py.codac4matlab.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); +end + +function [x,y,m,a,d] = fixpoint(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + [x,y,m,a,d] = contract(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus); + vol = x.volume() + y.volume() + m.volume() + a.volume() + d.volume(); + end +end + +[x,y,m,a,d] = fixpoint(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus); % [A-q8-end] % [A-q9-beg] x -DefaultFigure().draw_box(x) % does not display anything if unbounded +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 b1d4479b5..183d474e5 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_A.py +++ b/doc/manual/tuto/cp_robotics/src/lesson_A.py @@ -35,8 +35,8 @@ ]) ctc_minus = CtcInverse(f_minus, [0,0]) -x1,x2,x3 = ScalarVar(), ScalarVar(), ScalarVar() -f_plus = AnalyticFunction([x1,x2,x3], x1+x2-x3) +s1,s2,s3 = ScalarVar(), ScalarVar(), ScalarVar() +f_plus = AnalyticFunction([s1,s2,s3], s1+s2-s3) ctc_plus = CtcInverse(f_plus, 0) # [A-q6-end] @@ -54,13 +54,14 @@ # Or using a fixpoint method: -def constraints(x,y,m,a,d): +def contract(x,y,m,a,d): x[2],y[1],a = ctc_plus.contract(x[2],y[1],a) d[0],d[1],y[0],a = ctc_polar.contract(d[0],d[1],y[0],a) m,x,d = ctc_minus.contract(m,x,d) return x,y,m,a,d -x,y,m,a,d = fixpoint(constraints, x,y,m,a,d) + +x,y,m,a,d = fixpoint(contract, x,y,m,a,d) # [A-q8-end] # [A-q9-beg] diff --git a/doc/manual/tuto/cp_robotics/src/lesson_B.cpp b/doc/manual/tuto/cp_robotics/src/lesson_B.cpp index 295d5166f..1acf14de9 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_B.cpp +++ b/doc/manual/tuto/cp_robotics/src/lesson_B.cpp @@ -190,7 +190,7 @@ int main() }, x,yi,mi,ai,di); // [B-q10-beg] - if(mi.max_diam() <= 1) + if(mi.max_diam() < 0.11) DefaultFigure::draw_point(mi.mid()); // [B-q10-end] } diff --git a/doc/manual/tuto/cp_robotics/src/lesson_B.m b/doc/manual/tuto/cp_robotics/src/lesson_B.m new file mode 100644 index 000000000..2d6dc784d --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_B.m @@ -0,0 +1,211 @@ +import py.codac4matlab.* + +% [B-q3-beg] +M = {IntervalVector([1.5,2.5]),IntervalVector([3,1]), IntervalVector([2,2]), IntervalVector([2.5,3]), IntervalVector([3.5,2]), IntervalVector([4,1]), IntervalVector([1.5,0.5])}; + +for i = 1:numel(M) + M{i}.inflate(0.05); +end + +a1 = IntervalVector({{1.25,3.},{1.6,2.75}}); +a2 = IntervalVector({{2,3.5},{0.6,1.2}}); +a3 = IntervalVector({{1.1,3.25},{0.2,1.4}}); + +ctc_constell = MyCtc(M); + +a1 = ctc_constell.contract(a1) +a2 = ctc_constell.contract(a2) +a3 = ctc_constell.contract(a3) +% [B-q3-end] + +% [B-q4-beg] +x_truth = Vector([2,1,PI/6]); +% [B-q4-end] + +% [B-q5-beg] +DefaultFigure().draw_tank(x_truth, 0.4, StyleProperties({Color().black(),Color().yellow()})) +for i = 1:numel(M) + DefaultFigure().draw_box(M{i}, StyleProperties({Color().dark_green(),Color().green()})) +end + +DefaultFigure().set_axes(axis(1,Interval([1.,4.5])), axis(2,Interval([0.,3.5]))); +% [B-q5-end] + +% [B-q6-beg] +function p = g(x, mi) + r = py.codac4matlab.sqrt(py.codac4matlab.sqr(mi(1)-x(1))+py.codac4matlab.sqr(mi(2)-x(2))); + b = py.codac4matlab.atan2(mi(2)-x(2),mi(1)-x(1)) - x(3); + p = py.codac4matlab.IntervalVector({r,b}).inflate(0.02); +end +% [B-q6-end] + +% [B-q7-beg] +obs = {}; +for i = 1:numel(M) + obs{end+1} = cart_prod(g(x_truth, M{i}), M{i}); + % We append the position of the landmark to the measurement + % yi = {range}×{bearing}×{2d position} +end + +for i = 1:numel(obs) + DefaultFigure().draw_pie(x_truth.subvector(1,2),obs{i}(1), x_truth(3)+obs{i}(2),Color().red()); + DefaultFigure().draw_pie(x_truth.subvector(1,2),obs{i}(1).union(0),x_truth(3)+obs{i}(2),Color().gray()); +end +% [B-q7-end] + +ctc_polar = CtcPolar(); + +[x1,x2,x3] = deal(VectorVar(2), VectorVar(3), VectorVar(2)); +f_minus = AnalyticFunction({x1,x2,x3},vec(x1(1)-x2(1)-x3(1), x1(2)-x2(2)-x3(2))); +ctc_minus = CtcInverse(f_minus, Vector([0,0])); + +[s1,s2,s3] = deal(ScalarVar(),ScalarVar(),ScalarVar()); +f_plus = AnalyticFunction({s1,s2,s3}, s1+s2-s3); +ctc_plus = CtcInverse(f_plus, Interval(0,0)); + +% [B-q7b-beg] +function [x,yi,mi,ai,di] = ctc_one_obs(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus) + res_ctc_minus = ctc_minus.contract(py.codac4matlab.cart_prod(mi,x,di)); % The result is a 7D IntervalVector + [mi,x,di] = deal(res_ctc_minus.subvector(1,2),res_ctc_minus.subvector(3,5),res_ctc_minus.subvector(6,7)); + + res_ctc_plus = ctc_plus.contract(py.codac4matlab.cart_prod(x(3), yi(2), ai)); % The result is a 3D IntervalVector + x.setitem(3,res_ctc_plus(1)); + yi.setitem(2,res_ctc_plus(2)); + ai = res_ctc_plus(3); + + res_ctc_polar = ctc_polar.contract(py.codac4matlab.cart_prod(di(1),di(2),yi(1),ai)); % The result is a 4D IntervalVector + di.setitem(1,res_ctc_polar(1)); + di.setitem(2,res_ctc_polar(2)); + yi.setitem(1,res_ctc_polar(3)); + ai = res_ctc_polar(4); +end + +function [x,y,m,a,d] = fixpoint_ctc_one_obs(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + [x,y,m,a,d] = ctc_one_obs(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus); + vol = x.volume() + y.volume() + m.volume() + a.volume() + d.volume(); + end +end + +function x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus) + for i = 1:numel(obs) + yi = obs{i}; + ai = py.codac4matlab.Interval(); + di = py.codac4matlab.IntervalVector(2); + mi = yi.subvector(3,4); + [x,yi,mi,ai,di] = fixpoint_ctc_one_obs(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus); + end +end + +function x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus); + vol = x.volume(); + end +end + +x = cart_prod(IntervalVector(2),x_truth(3)); +x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus); +% [B-q7b-end] + +% [B-q8-beg] +x +DefaultFigure().draw_box(x) +% [B-q8-end] + +% [B-q9-beg] +function [x,yi,mi,ai,di] = ctc_one_obs_datasso(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + res_ctc_minus = ctc_minus.contract(py.codac4matlab.cart_prod(mi,x,di)); % The result is a 7D IntervalVector + [mi,x,di] = deal(res_ctc_minus.subvector(1,2),res_ctc_minus.subvector(3,5),res_ctc_minus.subvector(6,7)); + + res_ctc_plus = ctc_plus.contract(py.codac4matlab.cart_prod(x(3), yi(2), ai)); % The result is a 3D IntervalVector + x.setitem(3,res_ctc_plus(1)); + yi.setitem(2,res_ctc_plus(2)); + ai = res_ctc_plus(3); + + res_ctc_polar = ctc_polar.contract(py.codac4matlab.cart_prod(di(1),di(2),yi(1),ai)); % The result is a 4D IntervalVector + di.setitem(1,res_ctc_polar(1)); + di.setitem(2,res_ctc_polar(2)); + yi.setitem(1,res_ctc_polar(3)); + ai = res_ctc_polar(4); + % ==== Added contractor ==== + mi = ctc_constell.contract(mi); + % ========================== +end + +function [x,y,m,a,d] = fixpoint_ctc_one_obs_datasso(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + [x,y,m,a,d] = ctc_one_obs_datasso(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + vol = x.volume() + y.volume() + m.volume() + a.volume() + d.volume(); + end +end + +function x = ctc_all_obs_datasso(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + for i = 1:numel(obs) + yi = obs{i}; + ai = py.codac4matlab.Interval(); + di = py.codac4matlab.IntervalVector(2); + % ==== Changed domain ==== + mi = py.codac4matlab.IntervalVector(2); % the identity (position) of the landmark is not known + % ======================== + [x,yi,mi,ai,di] = fixpoint_ctc_one_obs_datasso(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + end +end + +function x = fixpoint_ctc_all_obs_datasso(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + x = ctc_all_obs_datasso(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + vol = x.volume(); + end +end + +x = cart_prod(IntervalVector(2),x_truth(3)); +x = fixpoint_ctc_all_obs_datasso(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell); +% [B-q9-end] + +function x = ctc_all_obs_datasso_mi(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + for i = 1:numel(obs) + yi = obs{i}; + ai = py.codac4matlab.Interval(); + di = py.codac4matlab.IntervalVector(2); + % ==== Changed domain ==== + mi = py.codac4matlab.IntervalVector(2); % the identity (position) of the landmark is not known + % ======================== + [x,yi,mi,ai,di] = fixpoint_ctc_one_obs_datasso(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + % [B-q10-beg] + if mi.max_diam() < 0.11 + py.codac4matlab.DefaultFigure().draw_point(mi.mid()); + % [B-q10-end] + end + end +end + +function x = fixpoint_ctc_all_obs_datasso_mi(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + x = ctc_all_obs_datasso_mi(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + vol = x.volume(); + end +end + +x = cart_prod(IntervalVector(2),x_truth(3)); +x = fixpoint_ctc_all_obs_datasso_mi(x,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell); diff --git a/doc/manual/tuto/cp_robotics/src/lesson_B.py b/doc/manual/tuto/cp_robotics/src/lesson_B.py index 0bb821751..986238f14 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_B.py +++ b/doc/manual/tuto/cp_robotics/src/lesson_B.py @@ -86,8 +86,8 @@ def g(x, mi): ]) ctc_minus = CtcInverse(f_minus, [0,0]) -x1,x2,x3 = ScalarVar(), ScalarVar(), ScalarVar() -f_plus = AnalyticFunction([x1,x2,x3], x1+x2-x3) +s1,s2,s3 = ScalarVar(), ScalarVar(), ScalarVar() +f_plus = AnalyticFunction([s1,s2,s3], s1+s2-s3) ctc_plus = CtcInverse(f_plus, 0) @@ -153,7 +153,7 @@ def ctc_all_obs_datasso(x): # ======================== x,yi,mi,ai,di = fixpoint(ctc_one_obs_datasso, x,yi,mi,ai,di) # [B-q10-beg] - if mi.max_diam() <= 1: + if mi.max_diam() < 0.11: DefaultFigure.draw_point(mi.mid()) # [B-q10-end] return x diff --git a/doc/manual/tuto/cp_robotics/src/lesson_C.m b/doc/manual/tuto/cp_robotics/src/lesson_C.m new file mode 100644 index 000000000..3857d4cdc --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_C.m @@ -0,0 +1,214 @@ +clear all; + +import py.codac4matlab.* + +% [C-q4-beg] +function obs = g(t,x,M) + obs = {}; % several landmarks can be seen at some ti + scope_range = py.codac4matlab.Interval(0,10); + scope_angle = py.codac4matlab.Interval(-py.codac4matlab.PI/4,py.codac4matlab.PI/4); + + for i = 1:numel(M) + mi = M{i}; + r = py.codac4matlab.sqrt(py.codac4matlab.sqr(mi(1)-x(1)) + py.codac4matlab.sqr(mi(2)-x(2))); + a = py.codac4matlab.atan2(mi(2)-x(2),mi(1)-x(1)) - x(3); + + % If the landmark is seen by the robot: + if scope_range.is_superset(r) && scope_angle.is_superset(a) + obs{end+1} =py.codac4matlab.cart_prod(t,r,a); + end + end +end +% [C-q4-end] + +% [C-q2-beg] +srand(); % initialize the random seed (from C++) +N = 50; % number of landmarks +X = IntervalVector({{-40,40},{-40,40}}); % landmarks distribution zone + +M={}; % creating the landmarks +for i = 1:N + M{i} = IntervalVector(X.rand()).inflate(0.2); +end + +fig = Figure2D("Robot simulation", GraphicOutput().VIBES); +fig.set_axes(axis(1, X(1).inflate(10),"x_1"),axis(2, X(2).inflate(10),"x_2")).auto_scale(); + +for i = 1:numel(M) % displaying the landmarks + fig.draw_box(M{i}, StyleProperties({Color().dark_green(),Color().green()})) +end +% [C-q2-end] + + +% [C-q3-beg] +wpts={}; % creating random waypoints for simulating the robot trajectory +X = IntervalVector({{-35,35},{-35,35}}); % robot evolution zone +for i = 1:5 % 5 waypoints + wpts{i} = X.rand(); +end + +s = RobotSimulator(); +s.w_max = 0.2; % maximum turning speed +u = SampledVectorTraj(); % the simulator will return the inputs (not used) +x_truth = s.simulate(Vector({0,0,0,0}), 0.01, wpts, u); % initial state (will be supposed unknown) and simulation time step +% [C-q3-end] + + +% [C-q5-beg] +prev_t = 0.; +time_between_obs = 3.; +obs = {}; + +t = double(x_truth.tdomain().lb()); + +tend = x_truth.tdomain().ub(); +while t < tend + if t-prev_t > time_between_obs + x_truth_t = x_truth(t); + obs_ti = g(t,x_truth_t,M); % computing the observation vector + + for i = 1:numel(obs_ti) + yi = obs_ti{i}; + prev_t = yi(1).mid(); + fig.draw_pie(x_truth_t.subvector(1,2), yi(2).union(0), x_truth_t(3)+yi(3), Color().light_gray()); + fig.draw_pie(x_truth_t.subvector(1,2), yi(2), x_truth_t(3)+yi(3), Color().red()); + + obs{end+1} = yi; + end + end + t = t + 0.01; +end +% [C-q5-end] + + +% [C-q6-beg] +x_ = VectorVar(4); +% Positions are not known, but headings (x3) and velocities (x4) are bounded.. +h = AnalyticFunction({x_},vec(Interval(-oo,oo),Interval(-oo,oo), x_(3) + 0.02*Interval(-1,1), x_(4) + 0.02*Interval(-1,1))); + +tdomain = create_tdomain(x_truth.tdomain(),0.05, true); +% The tube x is created from the interval evaluation of the actual trajectory +x = h.tube_eval(SlicedTube(tdomain,x_truth)); +% The tube v is created as a four-dimensional tube of infinite values +v = SlicedTube(tdomain, IntervalVector(4)); +% [C-q6-end] + + + +[x1,x2,x3] = deal(VectorVar(2), VectorVar(4), VectorVar(2)); +f_minus = AnalyticFunction({x1,x2,x3},vec(x1(1)-x2(1)-x3(1), x1(2)-x2(2)-x3(2))); +ctc_minus = CtcInverse(f_minus, Vector([0,0])); + +[s1,s2,s3] = deal(ScalarVar(),ScalarVar(),ScalarVar()); +f_plus = AnalyticFunction({s1,s2,s3}, s1+s2-s3); +ctc_plus = CtcInverse(f_plus, Interval(0,0)); + +% [C-q7-beg] +ctc_deriv = CtcDeriv(); + +[x_f,v_f] = deal(VectorVar(4),VectorVar(4)); +f = AnalyticFunction({x_f,v_f},vec(v_f(1)-x_f(4)*cos(x_f(3)), v_f(2)-x_f(4)*sin(x_f(3)))); + +ctc_f = CtcInverse(f, Vector([0,0])); +% + other contractors from previous lessons: +% ctc_plus, ctc_minus, ctc_polar, ctc_constell +% [C-q7-end] + +ctc_polar = CtcPolar(); +ctc_constell = MyCtc(M); + +% [C-q8-beg] +function [xi,yi,mi,ai,si] = ctc_one_obs(xi,yi,mi,ai,si,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + + res_ctc_minus = ctc_minus.contract(py.codac4matlab.cart_prod(mi,xi,si)); % The result is a 8D IntervalVector + [mi,xi,si] = deal(res_ctc_minus.subvector(1,2),res_ctc_minus.subvector(3,6),res_ctc_minus.subvector(7,8)); + + res_ctc_plus = ctc_plus.contract(py.codac4matlab.cart_prod(xi(3), yi(3), ai)); % The result is a 3D IntervalVector + xi.setitem(3,res_ctc_plus(1)); + yi.setitem(3,res_ctc_plus(2)); + ai = res_ctc_plus(3); + + res_ctc_polar = ctc_polar.contract(py.codac4matlab.cart_prod(si(1),si(2),yi(2),ai)); % The result is a 4D IntervalVector + si.setitem(1,res_ctc_polar(1)); + si.setitem(2,res_ctc_polar(2)); + yi.setitem(2,res_ctc_polar(3)); + ai = res_ctc_polar(4); + + mi = ctc_constell.contract(mi); +end + +function [x,y,m,a,s] = fixpoint_ctc_one_obs(x,y,m,a,s,ctc_plus,ctc_polar,ctc_minus,ctc_constell) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + [x,y,m,a,s] = ctc_one_obs(x,y,m,a,s,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + vol = 0.; + [x_vol,y_vol,m_vol,a_vol,s_vol] = deal(x.volume(), y.volume(), m.volume(), a.volume(), s.volume()); + if x_vol~=inf + vol = vol + x_vol; + end + if y_vol~=inf + vol = vol + y_vol; + end + if m_vol~=inf + vol = vol + m_vol; + end + if a_vol~=inf + vol = vol + a_vol; + end + if s_vol~=inf + vol = vol + s_vol; + end + if x.is_empty() || y.is_empty() || m.is_empty() || a.is_empty() || s.is_empty() + break + end + end +end + +function [x,v] = ctc_all_obs(x,v,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell,ctc_f,ctc_deriv) + for i = 1:numel(obs) + yi = obs{i}; + xi = x(yi(1)); + ai = py.codac4matlab.Interval(); + si = py.codac4matlab.IntervalVector(2); + mi = py.codac4matlab.IntervalVector(2); + [xi,yi,mi,ai,si] = fixpoint_ctc_one_obs(xi,yi,mi,ai,si,ctc_plus,ctc_polar,ctc_minus,ctc_constell); + x.set(xi,yi(1)); + end + + res_ctc_f = ctc_f.contract_tube(x,v); + x = res_ctc_f{1}; + v = res_ctc_f{2}; + + ctc_deriv.contract(x,v); + +end + +function x = fixpoint_ctc_all_obs(x,v,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell,ctc_f,ctc_deriv) + vol = -1.; + prev_vol = -2.; + + while vol ~= prev_vol + prev_vol = vol; + [x,v] = ctc_all_obs(x,v,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell,ctc_f,ctc_deriv); + vol = x.volume(); + if x.is_empty() + break + end + end +end + +fixpoint_ctc_all_obs(x,v,obs,ctc_plus,ctc_polar,ctc_minus,ctc_constell,ctc_f,ctc_deriv); + %[C-q8-end] + +x + +% [C-q9-beg] +fig.draw_tube(x); +fig.draw_trajectory(x_truth); +xf = x_truth(tend); +fig.draw_tank(xf, 2., StyleProperties({Color().black(),Color().yellow()})); +fig.draw_pie(xf, Interval(0,10), xf(3)+Interval(-PI/4.,PI/4), Color().dark_gray()); +% [C-q9-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_C.py b/doc/manual/tuto/cp_robotics/src/lesson_C.py index 37b4cae92..86e88658c 100644 --- a/doc/manual/tuto/cp_robotics/src/lesson_C.py +++ b/doc/manual/tuto/cp_robotics/src/lesson_C.py @@ -123,8 +123,8 @@ def g(t,x,M): ]) ctc_minus = CtcInverse(f_minus, [0,0]) -x1,x2,x3 = ScalarVar(), ScalarVar(), ScalarVar() -f_plus = AnalyticFunction([x1,x2,x3], x1+x2-x3) +s1,s2,s3 = ScalarVar(), ScalarVar(), ScalarVar() +f_plus = AnalyticFunction([s1,s2,s3], s1+s2-s3) ctc_plus = CtcInverse(f_plus, 0) @@ -169,7 +169,7 @@ def ctc_all_obs(x): x.set(xi,yi[0]) # restriction on the tube x at time ti=yi[0] x,v = ctc_f.contract_tube(x,v) - x = ctc_deriv.contract(x,v) + ctc_deriv.contract(x,v) return x diff --git a/examples/02_centered_form/main.m b/examples/02_centered_form/main.m index 391cd1db6..2186f9024 100644 --- a/examples/02_centered_form/main.m +++ b/examples/02_centered_form/main.m @@ -7,4 +7,4 @@ )); ctc = CtcInverse(f, IntervalVector({0,0})); -DefaultFigure.pave(IntervalVector({{0,2},{2,4},{0,10}}), ctc, 0.004); \ No newline at end of file +DefaultFigure().pave(IntervalVector({{0,2},{2,4},{0,10}}), ctc, 0.004); \ No newline at end of file diff --git a/python/codac/core/__init__.py b/python/codac/core/__init__.py index 0468a4c32..34fb43a9c 100644 --- a/python/codac/core/__init__.py +++ b/python/codac/core/__init__.py @@ -183,8 +183,8 @@ def contract_tube(self,*x): def copy(self): return self.c.copy() - def function(self): - return self.c.function() + def f(self): + return self.c.f() class CtcInverseNotIn(Ctc_IntervalVector): diff --git a/python/src/core/codac2_py_core.cpp b/python/src/core/codac2_py_core.cpp index 35884a859..79c1adb82 100644 --- a/python/src/core/codac2_py_core.cpp +++ b/python/src/core/codac2_py_core.cpp @@ -331,4 +331,11 @@ PYBIND11_MODULE(_core, m) srand(time(NULL)); }, DOC_TO_BE_DEFINED); + + m.def("srand", [](unsigned int seed) + { + srand(seed); + }, + DOC_TO_BE_DEFINED, + "seed"_a); } diff --git a/python/src/core/contractors/codac2_py_CtcDeriv.cpp b/python/src/core/contractors/codac2_py_CtcDeriv.cpp index 41f20086f..f061a0b0b 100644 --- a/python/src/core/contractors/codac2_py_CtcDeriv.cpp +++ b/python/src/core/contractors/codac2_py_CtcDeriv.cpp @@ -45,10 +45,10 @@ void export_CtcDeriv(py::module& m) .def("contract", [](const CtcDeriv& ctc, py::object& x, const py::object& v, const std::vector& ctc_indices) { if(is_instance>(x) && is_instance>(v)) - ctc.contract(cast>(x), cast>(v)); + return ctc.contract(cast>(x), cast>(v)); else if(is_instance>(x) && is_instance>(v)) - ctc.contract(cast>(x), cast>(v), + return ctc.contract(cast>(x), cast>(v), matlab::convert_indices(ctc_indices)); else { diff --git a/python/src/core/contractors/codac2_py_CtcInverse.h b/python/src/core/contractors/codac2_py_CtcInverse.h index 07b529589..a8af68124 100644 --- a/python/src/core/contractors/codac2_py_CtcInverse.h +++ b/python/src/core/contractors/codac2_py_CtcInverse.h @@ -65,8 +65,8 @@ void export_CtcInverse(py::module& m, const std::string& export_name, py::class_ VOID_CTCBASE_X_CONTRACT_TUBE_SLICEDTUBE_X_REF_VARIADIC_CONST, "x"_a) - .def("function", &C::function, - CONST_ANALYTICFUNCTION_TYPENAME_EXPRTYPE_Y_TYPE_REF_CTCINVERSE_YX_FUNCTION_CONST) + .def("f", &C::f, + CONST_ANALYTICFUNCTION_TYPENAME_EXPRTYPE_Y_TYPE_REF_CTCINVERSE_YX_F_CONST) ; } \ No newline at end of file diff --git a/python/src/core/domains/interval/codac2_py_Interval.cpp b/python/src/core/domains/interval/codac2_py_Interval.cpp index 1f519feba..09f9c8a0e 100644 --- a/python/src/core/domains/interval/codac2_py_Interval.cpp +++ b/python/src/core/domains/interval/codac2_py_Interval.cpp @@ -193,6 +193,11 @@ py::class_ export_Interval(py::module& m) .def("diff", &Interval::diff, VECTOR_INTERVAL_INTERVAL_DIFF_CONST_INTERVAL_REF_BOOL_CONST, "y"_a, "compactness"_a = true) + ; + + if constexpr(!FOR_MATLAB) + { + exported_interval_class .def(py::self |= py::self, INTERVAL_REF_INTERVAL_OPERATORUNIONEQ_CONST_INTERVAL_REF, @@ -201,7 +206,9 @@ py::class_ export_Interval(py::module& m) .def(py::self &= py::self, INTERVAL_REF_INTERVAL_OPERATORINTEREQ_CONST_INTERVAL_REF, "x"_a) - ; + + ; + } if constexpr(FOR_MATLAB) { diff --git a/python/src/core/domains/interval/codac2_py_IntervalMatrixBase.h b/python/src/core/domains/interval/codac2_py_IntervalMatrixBase.h index 242ab8218..622d22db5 100644 --- a/python/src/core/domains/interval/codac2_py_IntervalMatrixBase.h +++ b/python/src/core/domains/interval/codac2_py_IntervalMatrixBase.h @@ -171,23 +171,52 @@ void export_IntervalMatrixBase(py::module& m, py::class_& pyclass) .def("bisect_largest", [](const S& x, double ratio = 0.49) { return x.bisect_largest(ratio); }, MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_BISECT_LARGEST_FLOAT_CONST, "ratio"_a = 0.49) - - .def(py::self &= py::self, - MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORINTEREQ_CONST_MATRIXBASE_OTHERDERIVED_REF - "x"_a) - - .def(py::self |= py::self, - MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORUNIONEQ_CONST_MATRIXBASE_OTHERDERIVED_REF - "x"_a) - - .def(py::self & py::self, - MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORINTER_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST - "x"_a) - - .def(py::self | py::self, - MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORUNION_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST, - "x"_a) - ; + ; + + if constexpr(!FOR_MATLAB) + { + pyclass + + .def(py::self &= py::self, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORINTEREQ_CONST_MATRIXBASE_OTHERDERIVED_REF + "x"_a) + + .def(py::self |= py::self, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORUNIONEQ_CONST_MATRIXBASE_OTHERDERIVED_REF + "x"_a) + + .def(py::self & py::self, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORINTER_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST + "x"_a) + + .def(py::self | py::self, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORUNION_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST, + "x"_a) + ; + } + + if constexpr(FOR_MATLAB) + { + // For MATLAB compatibility + pyclass + + .def("self_inter", [](S& x, const S& y) { x &= y; return x; }, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORINTEREQ_CONST_MATRIXBASE_OTHERDERIVED_REF, + "x"_a) + + .def("self_union", [](S& x, const S& y) { x |= y; return x; }, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_REF_OPERATORUNIONEQ_CONST_MATRIXBASE_OTHERDERIVED_REF + "x"_a) + + .def("inter", [](const S& x,const S& y) { return x & y; }, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORINTER_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST + "x"_a) + + .def("union", [](const S& x,const S& y) { return x | y; }, + MATRIX_ADDONS_INTERVALMATRIXBASE_AUTO_OPERATORUNION_CONST_MATRIXBASE_OTHERDERIVED_REF_CONST + "x"_a) + ; + } py::implicitly_convertible(); } diff --git a/python/src/core/domains/interval/codac2_py_Interval_operations.cpp b/python/src/core/domains/interval/codac2_py_Interval_operations.cpp index 4be36e699..3d276d446 100644 --- a/python/src/core/domains/interval/codac2_py_Interval_operations.cpp +++ b/python/src/core/domains/interval/codac2_py_Interval_operations.cpp @@ -27,17 +27,19 @@ void export_Interval_operations(py::module& m, py::class_& py_Interval { // Members functions - py_Interval - - .def(py::self & py::self, - INTERVAL_OPERATORINTER_CONST_INTERVAL_REF_CONST_INTERVAL_REF, - "x"_a) + if constexpr(!FOR_MATLAB) + { + py_Interval - .def(py::self | py::self, - INTERVAL_OPERATORUNION_CONST_INTERVAL_REF_CONST_INTERVAL_REF, - "x"_a) + .def(py::self & py::self, + INTERVAL_OPERATORINTER_CONST_INTERVAL_REF_CONST_INTERVAL_REF, + "x"_a) - ; + .def(py::self | py::self, + INTERVAL_OPERATORUNION_CONST_INTERVAL_REF_CONST_INTERVAL_REF, + "x"_a) + ; + } if constexpr(FOR_MATLAB) { diff --git a/python/src/core/domains/tube/codac2_py_SlicedTube.h b/python/src/core/domains/tube/codac2_py_SlicedTube.h index 4c12cc9b9..e72d31cc9 100644 --- a/python/src/core/domains/tube/codac2_py_SlicedTube.h +++ b/python/src/core/domains/tube/codac2_py_SlicedTube.h @@ -255,7 +255,7 @@ void export_SlicedTube(py::module& m, const std::string& name) .def( #if FOR_MATLAB - "__call__" + "getitem" #else "__getitem__" #endif @@ -267,16 +267,33 @@ void export_SlicedTube(py::module& m, const std::string& name) }, SLICEDTUBE_INTERVAL_SLICEDTUBE_T_OPERATORCOMPO_INDEX_CONST, "i"_a) + + .def("get_item_0", + [](const SlicedTube& x, Index_type i) -> SlicedTube + { + matlab::test_integer(i); + return x[i]; + }, + SLICEDTUBE_INTERVAL_SLICEDTUBE_T_OPERATORCOMPO_INDEX_CONST, + "i"_a) .def("subvector", [](const SlicedTube& x, Index_type i, Index_type j) -> SlicedTube { - matlab::test_integer(i); - matlab::test_integer(j); + matlab::test_integer(i,j); return x.subvector(matlab::input_index(i),matlab::input_index(j)); }, SLICEDTUBE_INTERVALVECTOR_SLICEDTUBE_T_SUBVECTOR_INDEX_INDEX_CONST, "i"_a, "j"_a) + + .def("subvector_0", + [](const SlicedTube& x, Index_type i, Index_type j) -> SlicedTube + { + matlab::test_integer(i,j); + return x.subvector(i,j); + }, + SLICEDTUBE_INTERVALVECTOR_SLICEDTUBE_T_SUBVECTOR_INDEX_INDEX_CONST, + "i"_a, "j"_a) ; } -} \ No newline at end of file +} diff --git a/python/src/core/trajectory/codac2_py_SampledTraj.cpp b/python/src/core/trajectory/codac2_py_SampledTraj.cpp index d5dc6d4e2..6bf1d4844 100644 --- a/python/src/core/trajectory/codac2_py_SampledTraj.cpp +++ b/python/src/core/trajectory/codac2_py_SampledTraj.cpp @@ -89,7 +89,7 @@ py::class_> _export_SampledTraj(py::module& m, const string& clas .def( #if FOR_MATLAB - "__call__" + "getitem" #else "__getitem__" #endif diff --git a/src/core/contractors/codac2_CtcInverse.h b/src/core/contractors/codac2_CtcInverse.h index 89d8870fc..e82f4cdf9 100644 --- a/src/core/contractors/codac2_CtcInverse.h +++ b/src/core/contractors/codac2_CtcInverse.h @@ -117,7 +117,7 @@ namespace codac2 _f.intersect_from_args(v, x...); // updating input values } - const AnalyticFunction::Type>& function() const + const AnalyticFunction::Type>& f() const { return _f; } diff --git a/src/graphics/3rd/ipe/codac2_Figure2D_IPE.cpp b/src/graphics/3rd/ipe/codac2_Figure2D_IPE.cpp index 814d5c749..38c465bbf 100644 --- a/src/graphics/3rd/ipe/codac2_Figure2D_IPE.cpp +++ b/src/graphics/3rd/ipe/codac2_Figure2D_IPE.cpp @@ -431,21 +431,25 @@ void Figure2D_IPE::draw_pie(const Vector& c, const Interval& r, const Interval& { assert(_fig.size() <= c.size()); assert(r.lb() >= 0.); + + // IPE doesn't support arcs with a radius equal to zero + double r_lb = r.lb() < 1e-5 ? 1e-5 : r.lb(); + double r_ub = r.ub(); begin_path(style); - Vector point1 ({r.lb() * std::cos(theta.lb()), r.lb() * std::sin(theta.lb())}); - Vector point2 ({r.ub() * std::cos(theta.lb()), r.ub() * std::sin(theta.lb())}); - Vector point3 ({r.ub() * std::cos(theta.ub()), r.ub() * std::sin(theta.ub())}); - Vector point4 ({r.lb() * std::cos(theta.ub()), r.lb() * std::sin(theta.ub())}); + Vector point1 ({r_lb * std::cos(theta.lb()), r_lb * std::sin(theta.lb())}); + Vector point2 ({r_ub * std::cos(theta.lb()), r_ub * std::sin(theta.lb())}); + Vector point3 ({r_ub * std::cos(theta.ub()), r_ub * std::sin(theta.ub())}); + Vector point4 ({r_lb * std::cos(theta.ub()), r_lb * std::sin(theta.ub())}); _working_item += to_string(scale_x(c[0] + point1[0])) + " " + to_string(scale_y(c[1] + point1[1])) + " m \n"; _working_item += to_string(scale_x(c[0] + point2[0])) + " " + to_string(scale_y(c[1] + point2[1])) + " l \n"; - _working_item += to_string(scale_length(r.ub())) + " 0 0 " + to_string(scale_length(r.ub())) + " " + _working_item += to_string(scale_length(r_ub)) + " 0 0 " + to_string(scale_length(r_ub)) + " " + to_string(scale_x(c[i()])) + " " + to_string(scale_y(c[j()])) + " " + to_string(scale_x(c[0] + point3[0])) + " " + to_string(scale_y(c[1] + point3[1])) + " a \n"; _working_item += to_string(scale_x(c[0] + point4[0])) + " " + to_string(scale_y(c[1] + point4[1])) + " l \n"; - _working_item += to_string(scale_length(r.lb())) + " 0 0 " + to_string(- scale_length(r.lb())) + " " + _working_item += to_string(scale_length(r_lb)) + " 0 0 " + to_string(- scale_length(r_lb)) + " " + to_string(scale_x(c[i()])) + " " + to_string(scale_y(c[j()])) + " " + to_string(scale_x(c[0] + point1[0])) + " " + to_string(scale_y(c[1] + point1[1])) + " a \n"; diff --git a/tests/core/contractors/codac2_tests_CtcInverse.cpp b/tests/core/contractors/codac2_tests_CtcInverse.cpp index c8f7ec491..ce4880f2b 100644 --- a/tests/core/contractors/codac2_tests_CtcInverse.cpp +++ b/tests/core/contractors/codac2_tests_CtcInverse.cpp @@ -43,7 +43,7 @@ TEST_CASE("CtcInverse") ScalarVar x,y; AnalyticFunction f { {x,y}, x-y }; CtcInverse c(f, Interval(0.)); - CHECK(c.function().input_size() == 2); + CHECK(c.f().input_size() == 2); Interval a,b; diff --git a/tests/core/contractors/codac2_tests_CtcInverse.py b/tests/core/contractors/codac2_tests_CtcInverse.py index df2f44ebb..5cc4114a6 100644 --- a/tests/core/contractors/codac2_tests_CtcInverse.py +++ b/tests/core/contractors/codac2_tests_CtcInverse.py @@ -30,7 +30,7 @@ def test_CtcInverse_1(self): x = VectorVar(2) f = AnalyticFunction([x], x[0]-x[1]) c = CtcInverse(f, 0) - self.assertTrue(c.function().input_size() == 2) + self.assertTrue(c.f().input_size() == 2) b = IntervalVector(2)