diff --git a/doc/manual/index.rst b/doc/manual/index.rst index e4f545767..d4c6b7c04 100644 --- a/doc/manual/index.rst +++ b/doc/manual/index.rst @@ -52,8 +52,7 @@ The solution set is approximated from an initial box :math:`[\mathbf{x}_0]=[0,2] int main() { VectorVar x(3); - AnalyticFunction f { {x}, - { + AnalyticFunction f { {x}, { -(x[2]^2)+2*x[2]*sin(x[2]*x[0])+cos(x[2]*x[1]), 2*x[2]*cos(x[2]*x[0])-sin(x[2]*x[1]) } @@ -191,7 +190,8 @@ User manual * :ref:`sec-intervals` * What is an interval? * :ref:`sec-intervals-class` - * Boolean intervals + * :ref:`sec-intervals-intervalvector-class` + * :ref:`sec-intervals-boolinterval-class` * :ref:`sec-linear` * :ref:`sec-linear-vecmat` @@ -207,11 +207,11 @@ User manual * :ref:`sec-functions-analytic-operators` * Extension to custom expressions * Temporal functions + * :ref:`sec-functions-parallelepiped-eval` + * :ref:`sec-functions-peibos` * Set-membership functions * The class SetMembershipFunction * Extension to custom expressions - * :ref:`sec-functions-parallelepiped-eval` - * :ref:`sec-functions-peibos` * Tubes * What is a tube? @@ -338,10 +338,10 @@ User manual * :ref:`sec-tools-registration` * :ref:`sec-tools-octasym` -* Codac extensions +* :ref:`sec-extensions` * :ref:`sec-extensions-capd` + * :ref:`sec-extensions-sympy` * Interface with the IBEX library - * Sympy (symbolic computation) * Frequently Asked Questions @@ -356,9 +356,7 @@ How-to guides ------------- * Robotics - * Non-linear state estimation - * State estimation by solving data association - * Range-only SLAM + * :ref:`sec-tuto-cprob` * Explored area * Loop detections and verifications diff --git a/doc/manual/manual/functions/analytic/analytic_operators.rst b/doc/manual/manual/functions/analytic/analytic_operators.rst index b7910a55c..4f8722fad 100644 --- a/doc/manual/manual/functions/analytic/analytic_operators.rst +++ b/doc/manual/manual/functions/analytic/analytic_operators.rst @@ -245,7 +245,7 @@ If you notice any mathematical operators missing from the list below, feel free +-----------------------------------------------------+----------------------+---------------+-------------------------------------+--------+--------+-------+------------+ | :math:`\min(x_1,x_2)` | ``min(x1,x2)`` | |MinOp| | ``x1``, ``x2``: scalar ||okk| ||okk| ||okk| ||okk| | +-----------------------------------------------------+----------------------+---------------+-------------------------------------+--------+--------+-------+------------+ - | :math:`x_1\bmod x_2` | -- | |ModOp| | -- ||nok| ||nok| ||nok| ||bok| | + | :math:`x_1\mod p` | ``mod(x1,p)`` | |ModOp| | ``x1``, ``p``: scalar ||okk| ||nok| ||nok| ||okk| | +-----------------------------------------------------+----------------------+---------------+-------------------------------------+--------+--------+-------+------------+ | :math:`(x_1)^{x_2}` | | ``pow(x1,x2)`` | |PowOp| | ``x1``, ``x2``: scalar ||okk| ||okk| ||okk| ||okk| | | | | ``x1^x2`` | | | | | | | diff --git a/doc/manual/tuto/cp_robotics/img/slam_0.png b/doc/manual/tuto/cp_robotics/img/slam_0.png index 43c2c4751..c936ff593 100644 Binary files a/doc/manual/tuto/cp_robotics/img/slam_0.png and b/doc/manual/tuto/cp_robotics/img/slam_0.png differ 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 4713fae36..27d59a83f 100644 --- a/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst +++ b/doc/manual/tuto/cp_robotics/lesson_b_data_association.rst @@ -565,7 +565,7 @@ We now assume that the identities of the landmarks are not known. This means tha :dedent: 0 - **B.11.** *Same result* means that the data association worked: each measurement has been automatically associated to the correct landmark without ambiguity. + **B.10.** *Same result* means that the data association worked: each measurement has been automatically associated to the correct landmark without ambiguity. We can now look at the set ``m`` containing the associations. If one :math:`[\mathbf{m}^i]` contains only one item of :math:`\mathbb{M}`, then it means that the landmark of the measurement :math:`\mathbf{y}^i` has been identified. diff --git a/doc/manual/tuto/cp_robotics/lesson_d_slam.rst b/doc/manual/tuto/cp_robotics/lesson_d_slam.rst index 0a07985fa..b18ceee3e 100644 --- a/doc/manual/tuto/cp_robotics/lesson_d_slam.rst +++ b/doc/manual/tuto/cp_robotics/lesson_d_slam.rst @@ -1,17 +1,17 @@ .. _sec-tuto-cprob-lesson-d: -[new] Lesson D: SLAM -==================== +Lesson D: SLAM +============== Main authors: `Simon Rohou `_ -We propose to apply interval tools for solving a classical state estimation problem of Simultaneous Localization And Mapping (SLAM). +We propose to apply interval tools to solve a classical state estimation problem of Simultaneous Localization And Mapping (SLAM). -A tank robot :math:`\mathcal{R}`, described by the state vector -:math:`\mathbf{x}\in\mathbb{R}^3` depicting its position +A tank robot :math:`\mathcal{R}`, whose state vector +:math:`\mathbf{x}\in\mathbb{R}^3` describes its position :math:`(x_1,x_2)^\intercal` and its heading :math:`x_3`, is evolving among a set of landmarks :math:`\mathbf{b}^k\in\mathbb{R}^2` with a constant speed :math:`v=10`. It is equipped with a compass for -measuring its heading :math:`x_3` with some uncertainties. +measuring its heading :math:`x_3` with bounded uncertainty. Formalism --------- @@ -53,45 +53,57 @@ The observation function :math:`g` is the distance function between a position .. admonition:: Exercise - **D.1.** Update your current version of Codac: + **D.1.** Update your current version of Codac. In Python, for instance: - .. code-block:: bash + .. code-block:: bash pip install codac --upgrade --pre # Option --pre has to be set because Codac v2 is only available in pre-release + To verify that you have the correct version, you can use: + + .. code-block:: bash + + python -m pip show codac + + > Name: codac + Version: 2.0.0.dev27 + + The required version for this tutorial is 2.0.0.dev27 or later. + The following instructions are given in Python, but feel free to use C++ or Matlab if you prefer. Simulation ---------- The following code provides a simulation of the actual but unknown trajectory :math:`\mathbf{x}^*(\cdot)`, without uncertainties. -This code can be used as a starting point of your project. +This code can be used as a starting point for your project. -.. code-block:: python +.. tabs:: - from codac import * - import random - import numpy as np + .. group-tab:: Python - dt = 0.02 # temporal discretization - t0tf = Interval(0,15) # [t0,tf] + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q1-beg] + :end-before: [D-q1-end] + :dedent: 0 - # System input - t = ScalarVar() - u = AnalyticTraj(AnalyticFunction([t],3*(sin(t)^2)+t/100), t0tf).sampled(dt) + .. group-tab:: C++ - truth_heading = u.primitive() - truth_px = (cos(truth_heading)*10.).primitive() - truth_py = (sin(truth_heading)*10.).primitive() + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q1-beg] + :end-before: [D-q1-end] + :dedent: 2 - # Actual state trajectory - # Note that this trajectory is unknown of the resolution - var_s1,var_s2,var_s3 = ScalarVar(),ScalarVar(),ScalarVar() - f_concat = AnalyticFunction([var_s1,var_s2,var_s3], [var_s1,var_s2,var_s3]) - truth_x = f_concat.traj_eval(truth_px,truth_py,truth_heading) + .. group-tab:: Matlab - DefaultFigure.draw_trajectory(truth_x) + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q1-beg] + :end-before: [D-q1-end] + :dedent: 0 Using VIBes, you should visualize these states: @@ -104,7 +116,7 @@ Deadreckoning ------------- The robot knows its initial state: :math:`\mathbf{x}(0)=(0,0,0)^\intercal`. -Deadreckoning consists in estimating the following positions of the robot without +Deadreckoning consists in estimating the robot’s subsequent positions without exteroceptive measurements (*i.e.* without distances from the landmarks). In this section, we will compute the set of feasible positions of :math:`\mathcal{R}`, considering only heading measurements and the evolution function :math:`\mathbf{f}`. @@ -113,12 +125,33 @@ considering only heading measurements and the evolution function :math:`\mathbf{ **D.2. Domains.** The set of feasible positions along time is a *tube* (interval of trajectories). - We create a tube :math:`[\mathbf{x}](t)` using: + We create a tube :math:`[\mathbf{x}](\cdot)` using: + + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q2-beg] + :end-before: [D-q2-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q2-beg] + :end-before: [D-q2-end] + :dedent: 2 - .. code-block:: python + .. group-tab:: Matlab - tdomain = create_tdomain(t0tf, dt) - x = SlicedTube(tdomain, IntervalVector(3)) + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q2-beg] + :end-before: [D-q2-end] + :dedent: 0 where ``tdomain`` is the temporal discretization over :math:`[t_0,t_f]`, and ``dt`` is the discretization parameter. Here, ``x`` is a 3d tube. @@ -128,24 +161,64 @@ considering only heading measurements and the evolution function :math:`\mathbf{ This can be verified, for instance at :math:`t=0`, with: - .. code-block:: python + .. tabs:: - print(x(0.)) + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q2b-beg] + :end-before: [D-q2b-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q2b-beg] + :end-before: [D-q2b-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q2b-beg] + :end-before: [D-q2b-end] + :dedent: 0 .. admonition:: Exercise **D.3. Heading measurements.** - In practice, the headings :math:`x_3(t)` are measured with some uncertainties known to be + In practice, the headings :math:`x_3(\cdot)` are measured with some uncertainties known to be bounded in :math:`[-0.03,0.03]`. We set these bounded measurements in the last - component of the tube vector :math:`[\mathbf{x}](t)` as: + component of the tube vector :math:`[\mathbf{x}](\cdot)` with: + + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q3-beg] + :end-before: [D-q3-end] + :dedent: 0 + + .. group-tab:: C++ - .. code-block:: python + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q3-beg] + :end-before: [D-q3-end] + :dedent: 2 - x = tube_cart_prod( - SlicedTube(tdomain, IntervalVector(2)), - # Heading measurement with bounded uncertainties: - SlicedTube(tdomain, truth_heading).inflate(0.03) - ) + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q3-beg] + :end-before: [D-q3-end] + :dedent: 0 .. admonition:: Exercise @@ -153,65 +226,168 @@ considering only heading measurements and the evolution function :math:`\mathbf{ The initial state :math:`\mathbf{x}(0)` (position and heading) is known, which can be implemented in the tube with: - .. code-block:: python + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q4-beg] + :end-before: [D-q4-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q4-beg] + :end-before: [D-q4-end] + :dedent: 2 - x.set([0,0,0], 0.) # setting a vector value at t=0 - print(x(0.)) + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q4-beg] + :end-before: [D-q4-end] + :dedent: 0 Now that a domain (a tube) has been defined for enclosing the estimates together with their uncertainties, it remains to define contractors for narrowing their bounds. In deadreckoning, only Eq. :eq:`eq-f` is considered: -:math:`\dot{\mathbf{x}}(t)=\mathbf{f}(\mathbf{x}(t))`. This can be processed with -two contractors, one for dealing with :math:`\mathbf{v}(t)=\mathbf{f}(\mathbf{x}(t))`, -and one for :math:`\dot{\mathbf{x}}(t)=\mathbf{v}(t)`. +:math:`\dot{\mathbf{x}}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))`. This can be processed with +two contractors, one for dealing with :math:`\mathbf{v}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))`, +and one for :math:`\dot{\mathbf{x}}(\cdot)=\mathbf{v}(\cdot)`. + +The new tube :math:`[\mathbf{v}](\cdot)` will enclose the feasible derivatives of the +possible states in :math:`[\mathbf{x}](\cdot)`. + +.. admonition:: Exercise -The new tube :math:`[\mathbf{v}](t)` will enclose the feasible derivatives of the -possible states in :math:`[\mathbf{x}](t)`. + **D.5. Derivatives of the state.** As for :math:`[\mathbf{x}](\cdot)`, create another 3D ``SlicedTube`` for + :math:`[\mathbf{v}](\cdot)`, called ``v``. .. admonition:: Exercise - **D.5. Contractors.** As for :math:`[\mathbf{x}](t)`, create another 3d ``SlicedTube`` for - :math:`[\mathbf{v}](t)`, called ``v``. + **D.6. Contractors.** Then, create a contractor for :math:`\mathbf{v}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))`. The associated constraint is expressed as an implicit form :math:`\mathbf{f}(\mathbf{x}(\cdot),\mathbf{v}(\cdot))=\mathbf{0}`. - Then, create a contractor for :math:`\mathbf{v}(t)=\mathbf{f}(\mathbf{x}(t))`. The associated constraint is expressed as an implicit form :math:`\mathbf{f}(\mathbf{x}(t),\mathbf{v}(t))=\mathbf{0}`. + .. tabs:: - .. code-block:: python + .. group-tab:: Python - var_x,var_v = VectorVar(3),VectorVar(3) - f_evol = AnalyticFunction([var_x,var_v], [ - var_v[0]-10*cos(var_x[2]), - var_v[1]-10*sin(var_x[2]), - ]) + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q6-beg] + :end-before: [D-q6-end] + :dedent: 0 - ctc_f = CtcInverse(f_evol, [0,0]) # [0,0] stands for the implicit form f(..)=0 + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q6-beg] + :end-before: [D-q6-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q6-beg] + :end-before: [D-q6-end] + :dedent: 0 Create a contractor for :math:`\dot{\mathbf{x}}(t)=\mathbf{v}(t)`: - .. code-block:: python + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q6b-beg] + :end-before: [D-q6b-end] + :dedent: 0 - ctc_deriv = CtcDeriv() + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q6b-beg] + :end-before: [D-q6b-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q6b-beg] + :end-before: [D-q6b-end] + :dedent: 0 We recall that contractors are algorithms for reducing sets of feasible values (intervals, boxes, tubes..). .. admonition:: Exercise - **D.6. Deadreckoning estimation.** At this point, you can implemented an algorithm for deadreckoning, by calling the previously defined contractors. + **D.7. Deadreckoning estimation.** At this point, you can implement an algorithm for deadreckoning, by calling the previously defined contractors. + + .. tabs:: - .. code-block:: python + .. group-tab:: Python - ctc_f.contract_tube(x,v) - ctc_deriv.contract(x,v) + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q7-beg] + :end-before: [D-q7-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q7-beg] + :end-before: [D-q7-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q7-beg] + :end-before: [D-q7-end] + :dedent: 0 .. admonition:: Exercise - **D.7. Display.** Eventually, the contracted tube :math:`[\mathbf{x}](t)` can be displayed with: + **D.8. Display.** Finally, the contracted tube :math:`[\mathbf{x}](t)` can be displayed with: + + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q8-beg] + :end-before: [D-q8-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q8-beg] + :end-before: [D-q8-end] + :dedent: 2 - .. code-block:: python + .. group-tab:: Matlab - DefaultFigure.draw_tube(x, [Color.light_gray(),Color.light_gray()]) - DefaultFigure.draw_trajectory(truth_x) + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q8-beg] + :end-before: [D-q8-end] + :dedent: 0 You should obtain the following result: @@ -221,98 +397,203 @@ You should obtain the following result: In gray: tube :math:`[\mathbf{x}](t)` enclosing the estimated trajectories of the robot. The actual but unknown trajectory is depicted in black. With interval methods, the computations are guaranteed: the actual trajectory cannot be outside the tube. - However, the tube may be large in case of poor localization, as it is the case up - to now without state observations. + However, the tube may be large in case of poor localization, as is the case so far without state observations. Range-only localization ----------------------- The obtained tube grows with time, illustrating a significant drift of the robot. -We now rely on distances measured from known landmarks for reducing this drift. -This amounts to a non-linear state estimation problem: function :math:`g` of +We now rely on distances measured from known landmarks to reduce this drift. +This amounts to a non-linear state estimation problem: function :math:`g` in System :eq:`eq-state` is a distance function. Non-linearities can be difficult to solve with conventional methods. .. admonition:: Exercise - **D.8. Landmarks.** Define five landmarks with: + **D.9. Landmarks.** Define five landmarks with: - .. code-block:: python + .. tabs:: - b = [(6,12),(-2,-5),(-3,20),(3,4),(-10,0)] + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q9-beg] + :end-before: [D-q9-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q9-beg] + :end-before: [D-q9-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q9-beg] + :end-before: [D-q9-end] + :dedent: 0 .. admonition:: Exercise - **D.9. Range observations.** In a loop, for each :math:`t_i\in\{0,1,\dots,15\}`, compute the distance measured + **D.10. Range observations.** In a loop, for each :math:`t_i\in\{0,1,\dots,15\}`, compute the distance measured from a random landmark: - .. code-block:: python + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q10-beg] + :end-before: [D-q10-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q10-beg] + :end-before: [D-q10-end] + :dedent: 2 - Y = [] - for ti in np.arange(0,15): - k = random.randint(0,len(b)-1) # a random landmark is perceived - d = sqrt(sqr(truth_x(ti)[0]-b[k][0])+sqr(truth_x(ti)[1]-b[k][1])) - Y.append([k,ti,d]) + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q10-beg] + :end-before: [D-q10-end] + :dedent: 0 .. admonition:: Exercise - **D.10. Measurement uncertainty.** The measurements come with some errors (not computed here) that are known to be + **D.11. Measurement uncertainty.** The measurements come with some errors (not computed here) that are known to be bounded within :math:`[-0.03,0.03]`. We use intervals for enclosing the observations: - .. code-block:: python + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q11-beg] + :end-before: [D-q11-end] + :dedent: 0 + + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q11-beg] + :end-before: [D-q11-end] + :dedent: 2 - ... - d += Interval(-0.03,0.03) - # or equivalently: d.inflate(3e-2) + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q11-beg] + :end-before: [D-q11-end] + :dedent: 0 As in the previous lessons, a fixpoint function can be used to manage the contractors automatically. -In another ``for`` loop, we will combine contractors using a fixpoint method, in order to improve the localisation of the robot. +In another ``for`` loop, we will combine contractors using a fixpoint method, in order to improve the localization of the robot. .. admonition:: Exercise - **D.11. Observation contractors.** The state observations are added to the + **D.12. Observation contractors.** The state observations are added to the set of constraints by means of a new distance contractor linking the state to the position of a landmark: - .. code-block:: python + .. tabs:: + + .. group-tab:: Python + + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q12-beg] + :end-before: [D-q12-end] + :dedent: 0 + + .. group-tab:: C++ - var_b = VectorVar(2) - var_d = ScalarVar() - f_dist = AnalyticFunction([var_x,var_b,var_d], sqrt(sqr(var_x[0]-var_b[0])+sqr(var_x[1]-var_b[1]))-var_d) - ctc_dist = CtcInverse(f_dist, 0) # also expressed in a implicit form g(x,b,d)=0 + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q12-beg] + :end-before: [D-q12-end] + :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q12-beg] + :end-before: [D-q12-end] + :dedent: 0 .. admonition:: Exercise - **D.12. Range-only localization.** - The network of contractors that you can implement is now the following: + **D.13. Range-only localization.** + The contractor network is now as follows: + + .. tabs:: + + .. group-tab:: Python - .. code-block:: python + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q13-beg] + :end-before: [D-q13-end] + :dedent: 0 - def contractors_list(x,v): - ctc_deriv.contract(x,v) - ctc_f.contract_tube(x,v) - for yi in Y: # for each range-only measurement - ti = yi[1] - pi = x(ti) - bi = IntervalVector(b[yi[0]]) - di = yi[2] - pi,bi,di = ctc_dist.contract(pi,bi,di) - x.set(pi, ti) - return x,v + .. group-tab:: C++ - x,v = fixpoint(contractors_list, x,v) + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q13-beg] + :end-before: [D-q13-end] + :dedent: 2 + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q13-beg] + :end-before: [D-q13-end] + :dedent: 0 .. admonition:: Exercise - **D.13. Reveal the contracted tube.** + **D.14. Display the contracted tube.** + + .. tabs:: - .. code-block:: python + .. group-tab:: Python - DefaultFigure.draw_tube(x, ColorMap.blue_tube()) - DefaultFigure.draw_trajectory(truth_x) - DefaultFigure.draw_tank(truth_x(t0tf.ub()), 1., [Color.dark_gray(),Color.yellow()]) + .. literalinclude:: src/lesson_D.py + :language: py + :start-after: [D-q14-beg] + :end-before: [D-q14-end] + :dedent: 0 + .. group-tab:: C++ + + .. literalinclude:: src/lesson_D.cpp + :language: c++ + :start-after: [D-q14-beg] + :end-before: [D-q14-end] + :dedent: 2 + + .. group-tab:: Matlab + + .. literalinclude:: src/lesson_D.m + :language: matlab + :start-after: [D-q14-beg] + :end-before: [D-q14-end] + :dedent: 0 You should obtain the following result: @@ -326,7 +607,7 @@ You should obtain the following result: .. admonition:: Exercise - | **D.14. Unknown initial condition?** + | **D.15. Unknown initial condition?** | *What if we have no knowledge about the initial position of the robot?* Try to remove the condition set in Exercise **D.4.**, and zoom towards the initial position. @@ -337,9 +618,11 @@ Challenge: Simultaneous Localization and Mapping (SLAM) .. admonition:: Exercise - **D.15. Range only SLAM.** + **D.16. Range-only SLAM.** Adapt the script in order to deal with a SLAM problem, in which the positions of the - landmarks are priorly unknown, but then estimated together with the positions of the robot. + landmarks are a priori unknown, but then estimated together with the positions of the robot. + + For this estimation, we will need more observations: we now assume that distance measurements are obtained every 0.1s. Note that in SLAM, the initial condition set in Exercise **D.4.** is mandatory. \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_D.cpp b/doc/manual/tuto/cp_robotics/src/lesson_D.cpp new file mode 100644 index 000000000..11e5db0f5 --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_D.cpp @@ -0,0 +1,82 @@ +#include +using namespace codac2; + + +// [D-q1-beg] + +// [D-q1-end] + + +// [D-q2-beg] + +// [D-q2-end] + + +// [D-q2b-beg] + +// [D-q2b-end] + + +// [D-q3-beg] + +// [D-q3-end] + + +// [D-q4-beg] + +// [D-q4-end] + + +// [D-q5-beg] + +// [D-q5-end] + + +// [D-q6-beg] + +// [D-q6-end] + + +// [D-q6b-beg] + +// [D-q6b-end] + + +// [D-q7-beg] + +// [D-q7-end] + + +// [D-q8-beg] + +// [D-q8-end] + + +// [D-q9-beg] + +// [D-q9-end] + + +// [D-q10-beg] + +// [D-q10-end] + + +// [D-q11-beg] + +// [D-q11-end] + + +// [D-q12-beg] + +// [D-q12-end] + + +// [D-q13-beg] + +// [D-q13-end] + + +// [D-q14-beg] + +// [D-q14-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_D.m b/doc/manual/tuto/cp_robotics/src/lesson_D.m new file mode 100644 index 000000000..cdf4cd853 --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_D.m @@ -0,0 +1,82 @@ +clear all; + +import py.codac4matlab.* + +% [D-q1-beg] + +% [D-q1-end] + + +% [D-q2-beg] + +% [D-q2-end] + + +% [D-q2b-beg] + +% [D-q2b-end] + + +% [D-q3-beg] + +% [D-q3-end] + + +% [D-q4-beg] + +% [D-q4-end] + + +% [D-q5-beg] + +% [D-q5-end] + + +% [D-q6-beg] + +% [D-q6-end] + + +% [D-q6b-beg] + +% [D-q6b-end] + + +% [D-q7-beg] + +% [D-q7-end] + + +% [D-q8-beg] + +% [D-q8-end] + + +% [D-q9-beg] + +% [D-q9-end] + + +% [D-q10-beg] + +% [D-q10-end] + + +% [D-q11-beg] + +% [D-q11-end] + + +% [D-q12-beg] + +% [D-q12-end] + + +% [D-q13-beg] + +% [D-q13-end] + + +% [D-q14-beg] + +% [D-q14-end] \ No newline at end of file diff --git a/doc/manual/tuto/cp_robotics/src/lesson_D.py b/doc/manual/tuto/cp_robotics/src/lesson_D.py new file mode 100644 index 000000000..69b2c761f --- /dev/null +++ b/doc/manual/tuto/cp_robotics/src/lesson_D.py @@ -0,0 +1,140 @@ +# [D-q1-beg] +from codac import * +import random +import numpy as np + +dt = 0.02 # temporal discretization +t0tf = Interval(0,15) # temporal domain [t0,tf] + +# System input +t = ScalarVar() +# Input u(.) is given as an analytic trajectory +u = AnalyticTraj(AnalyticFunction([t],3*(sin(t)^2)+t/100), t0tf).sampled(dt) + +# Implementing manually the evolution function (Eq. (2)) +truth_heading = u.primitive() +truth_px = (cos(truth_heading)*10.).primitive() +truth_py = (sin(truth_heading)*10.).primitive() + +# Actual state trajectory +# Note that this trajectory is unknown to the estimation process +s1,s2,s3 = ScalarVar(),ScalarVar(),ScalarVar() +f_concat = AnalyticFunction([s1,s2,s3], [s1,s2,s3]) +truth_x = f_concat.traj_eval(truth_px,truth_py,truth_heading) + +DefaultFigure.draw_trajectory(truth_x) +DefaultFigure.draw_tank(truth_x(t0tf.ub()), 1., [Color.dark_gray(),Color.yellow()]) +# [D-q1-end] + +# [D-q2-beg] +tdomain = create_tdomain(t0tf, dt) # temporal discretization over [t0,tf] +x = SlicedTube(tdomain, IntervalVector(3)) +# [D-q2-end] + + +# [D-q2b-beg] +print(x(0.)) +# [D-q2b-end] + + +# [D-q3-beg] +x = tube_cart_prod( + SlicedTube(tdomain, IntervalVector(2)), + # Heading measurement with bounded uncertainties: + SlicedTube(tdomain, truth_heading).inflate(0.03) +) +# [D-q3-end] + + +# [D-q4-beg] +x.set([0,0,0], 0.) # setting a vector value (0,0,0) at t=0 +print(x(0.)) +# [D-q4-end] + + +# [D-q5-beg] +v = SlicedTube(tdomain, IntervalVector(3)) +# [D-q5-end] + + +# [D-q6-beg] +vx,vv = VectorVar(3),VectorVar(3) +f_evol = AnalyticFunction([vx,vv], [ + vv[0]-10*cos(vx[2]), + vv[1]-10*sin(vx[2]), + ]) + +ctc_f = CtcInverse(f_evol, [0,0]) # [0,0] stands for the implicit form f(..)=0 +# [D-q6-end] + + +# [D-q6b-beg] +ctc_deriv = CtcDeriv() +# [D-q6b-end] + + +# [D-q7-beg] +ctc_f.contract_tube(x,v) +ctc_deriv.contract(x,v) +# [D-q7-end] + + +# [D-q8-beg] +DefaultFigure.draw_tube(x, [Color.light_gray(),Color.light_gray()]) +DefaultFigure.draw_trajectory(truth_x) +# [D-q8-end] + + +# [D-q9-beg] +b = [(6,12),(-2,-5),(-3,20),(3,4),(-10,0)] +# [D-q9-end] + + +# [D-q10-beg] +Y = [] +for ti in np.arange(0,15): + k = random.randint(0,len(b)-1) # a random landmark is perceived + d = sqrt(sqr(truth_x(ti)[0]-b[k][0])+sqr(truth_x(ti)[1]-b[k][1])) + Y.append([k,ti,d]) +# [D-q10-end] + +Y = [] +for ti in np.arange(0,15): + k = random.randint(0,len(b)-1) # a random landmark is perceived + d = sqrt(sqr(truth_x(ti)[0]-b[k][0])+sqr(truth_x(ti)[1]-b[k][1])) + # [D-q11-beg] + d += Interval(-0.03,0.03) + # or equivalently: d.inflate(3e-2) + # [D-q11-end] + Y.append([k,ti,d]) + + +# [D-q12-beg] +vb = VectorVar(2) +vd = ScalarVar() +f_dist = AnalyticFunction([vx,vb,vd], sqrt(sqr(vx[0]-vb[0])+sqr(vx[1]-vb[1]))-vd) +ctc_dist = CtcInverse(f_dist, 0) # also expressed in a implicit form g(x,b,d)=0 +# [D-q12-end] + + +# [D-q13-beg] +def contractors_list(x,v): + ctc_deriv.contract(x,v) + ctc_f.contract_tube(x,v) + for yi in Y: # for each range-only measurement + ti = yi[1] + pi = x(ti) + bi = IntervalVector(b[yi[0]]) + di = yi[2] + pi,bi,di = ctc_dist.contract(pi,bi,di) + x.set(pi, ti) + return x,v + +x,v = fixpoint(contractors_list, x,v) +# [D-q13-end] + +# [D-q14-beg] +DefaultFigure.draw_tube(x, ColorMap.blue_tube()) +DefaultFigure.draw_trajectory(truth_x) +DefaultFigure.draw_tank(truth_x(t0tf.ub()), 1., [Color.dark_gray(),Color.yellow()]) +# [D-q14-end] \ No newline at end of file diff --git a/python/src/core/operators/codac2_py_operators.cpp b/python/src/core/operators/codac2_py_operators.cpp index e3c23ee79..c7c4deaf9 100644 --- a/python/src/core/operators/codac2_py_operators.cpp +++ b/python/src/core/operators/codac2_py_operators.cpp @@ -606,12 +606,18 @@ void export_operators(py::module& m) py::class_(m, "ModOp") .def(py::init<>()) // for using static methods in Matlab - .def_static("bwd", (void(*)(Interval&,Interval&,double)) &ModOp::bwd, - STATIC_VOID_MODOP_BWD_INTERVAL_REF_INTERVAL_REF_DOUBLE, - "x1"_a, "x2"_a, "p"_a) - .def_static("bwd", (void(*)(Interval&,Interval&,Interval&)) &ModOp::bwd, - STATIC_VOID_MODOP_BWD_INTERVAL_REF_INTERVAL_REF_INTERVAL_REF, - "x1"_a, "x2"_a, "p"_a) + .def_static("fwd", (Interval(*)(const Interval&,const Interval&)) &ModOp::fwd, + STATIC_INTERVAL_MODOP_FWD_CONST_INTERVAL_REF_CONST_INTERVAL_REF, + "x1"_a, "p"_a) + .def_static("bwd", (void(*)(const Interval&,Interval&,Interval&)) &ModOp::bwd, + STATIC_VOID_MODOP_BWD_CONST_INTERVAL_REF_INTERVAL_REF_INTERVAL_REF, + "y"_a, "x1"_a, "p"_a) + .def_static("fwd_bwd", (void(*)(Interval&,Interval&,double)) &ModOp::fwd_bwd, + STATIC_VOID_MODOP_FWD_BWD_INTERVAL_REF_INTERVAL_REF_DOUBLE, + "y"_a, "x1"_a, "p"_a) + .def_static("fwd_bwd", (void(*)(Interval&,Interval&,Interval&)) &ModOp::fwd_bwd, + STATIC_VOID_MODOP_FWD_BWD_INTERVAL_REF_INTERVAL_REF_INTERVAL_REF, + "y"_a, "x1"_a, "p"_a) ; py::class_(m, "PowOp") diff --git a/src/core/operators/codac2_mod.h b/src/core/operators/codac2_mod.h index 4b643241e..02aefb661 100644 --- a/src/core/operators/codac2_mod.h +++ b/src/core/operators/codac2_mod.h @@ -16,71 +16,198 @@ namespace codac2 { + static inline double canonical_mod(double x, double p) + { + double r = std::fmod(x, p); + if(r < 0.0) r += p; + return r; + } + struct ModOp { - template - static std::string str(const X1& x1, const X2& x2, const P& p) + template + static std::string str(const X1& x1, const P& p) { - return "mod(" + x1->str() + "," + x2->str() + "," + p->str() + ")"; + return "mod(" + x1->str() + "," + p->str() + ")"; } - template - static std::pair output_shape([[maybe_unused]] const X1& s1, [[maybe_unused]] const X2& s2) + template + static std::pair output_shape([[maybe_unused]] const X1& s1, [[maybe_unused]] const P& p) { return {1,1}; } - static void bwd(Interval& x1, Interval& x2, double p); - static void bwd(Interval& x1, Interval& x2, Interval& p); + static Interval fwd(const Interval& x1, const Interval& p); + static ScalarType fwd_natural(const ScalarType& x1, const ScalarType& p); + static ScalarType fwd_centered(const ScalarType& x1, const ScalarType& p); + static void bwd(const Interval& y, Interval& x1, Interval& p); + + static void fwd_bwd(Interval& y, Interval& x1, double p); + static void fwd_bwd(Interval& y, Interval& x1, Interval& p); }; + // Analytic operator + // The following function can be used to build analytic expressions. + + inline ScalarExpr + mod(const ScalarExpr& x1, const ScalarExpr& p) + { + return { std::make_shared>(x1,p) }; + } + // Inline functions - inline void ModOp::bwd(Interval& x1, Interval& x2, double p) // x1 = x2 mod(p) + inline Interval ModOp::fwd(const Interval& x1, const Interval& p) { - // The content of this function comes from the IBEX library. - // See ibex::Interval (IBEX lib, main author: Gilles Chabert) - // https://ibex-lib.readthedocs.io + if(x1.is_empty() || p.is_empty()) + return Interval::empty(); + + Interval pp = p & Interval(next_float(0),oo); + if(pp.is_empty()) + return Interval::empty(); - assert_release(p > 0. && "Modulo needs a strictly positive period p."); + // Exact case: singleton / singleton + if(x1.is_degenerated() && pp.is_degenerated()) + return { canonical_mod(x1.lb(), pp.lb()) }; - if(!(x2.diam() > p || x1.diam() > p)) + // Exact case: for every admissible p, x1 is already in the canonical band [0,p) + if(x1.lb() >= 0.0 && x1.ub() < pp.lb()) + return x1; + + // Exact case: scalar period and constant quotient over the whole x1 + if(pp.lb() == pp.ub()) { - Interval r = (x1-x2)/p; - Interval ir = integer(r); + const double q = pp.lb(); + const double k1 = std::floor(x1.lb() / q); + const double k2 = std::floor(x1.ub() / q); - if(ir.is_empty()) // additional protection because an empty interval is considered degenerated. + if(k1 == k2) { - x1.set_empty(); - x2.set_empty(); + // y = x1 - k*q + return x1 - (Interval(k1) * q); } + } + + // Safe but intentionally simple enclosure + double yub = pp.ub(); + if(x1.lb() >= 0.0) + yub = std::min(yub, x1.ub()); + + return { 0.0, yub }; + } + + inline ScalarType ModOp::fwd_natural(const ScalarType& x1, const ScalarType& p) + { + return { + fwd(x1.a, p.a), + x1.def_domain && p.def_domain + }; + } + + inline ScalarType ModOp::fwd_centered(const ScalarType& x1, const ScalarType& p) + { + if(centered_form_not_available_for_args(x1,p)) + return fwd_natural(x1,p); + + return { + fwd(x1.m,p.m), + fwd(x1.a,p.a), + IntervalMatrix(0,0), // not supported yet for auto diff + x1.def_domain && p.def_domain + }; + } + + inline void ModOp::bwd(const Interval& y, Interval& x1, Interval& p) + { + auto set_empty = [&]() + { + x1.set_empty(); + p.set_empty(); + }; + + if(y.is_empty() || x1.is_empty() || p.is_empty()) + { + set_empty(); + return; + } - else + // Modulo is defined only for p > 0 + p &= Interval(next_float(0),oo); + if(p.is_empty()) + { + set_empty(); + return; + } + + // Since y is not contracted here (const), we only keep the part + // potentially compatible with a canonical remainder: 0 <= y <= p.ub() + Interval y0 = y & Interval(0.0, p.ub()); + if(y0.is_empty()) + { + set_empty(); + return; + } + + // Candidate integer quotients: + // x1 = y + k p <=> k = (x1 - y)/p + Interval r = (x1-y0)/p; + + const double kmin_d = std::ceil(r.lb()); + const double kmax_d = std::floor(r.ub()); + + if(!(kmin_d <= kmax_d)) + { + set_empty(); + return; + } + + Interval K(kmin_d, kmax_d); + + // Coarse hull contraction + x1 &= y0+K*p; + if(x1.is_empty()) + { + set_empty(); + return; + } + + // If 0 is not in K, we can also contract p approximately + if(!K.contains(0.)) + { + p &= (x1-y0)/K; + p &= Interval(next_float(0),oo); + + if(p.is_empty()) + { + set_empty(); + return; + } + + y0 &= Interval(0.0, p.ub()); + if(y0.is_empty()) { - if(ir.is_degenerated()) - SubOp::bwd(ir*p,x1,x2); - - else if(ir.diam() == 1.) - { - Interval x1_1 = x1; Interval x1_2 = x1; - Interval x2_1 = x2; Interval x2_2 = x2; - SubOp::bwd(Interval(ir.lb()*p),x1_1,x2_1); - SubOp::bwd(Interval(ir.ub()*p),x1_2,x2_2); - x1 = x1_1 | x1_2; - x2 = x2_1 | x2_2; - } - - else - { - assert_release_constexpr(false && "Modulo diameter error."); - } + set_empty(); + return; + } + + x1 &= y0+K*p; + if(x1.is_empty()) + { + set_empty(); + return; } } } - inline void ModOp::bwd(Interval& x1, Interval& x2, Interval& p) // x = y mod(p) + inline void ModOp::fwd_bwd(Interval& y, Interval& x1, double p) + { + Interval ip(p); + ModOp::fwd_bwd(y,x1,ip); + } + + inline void ModOp::fwd_bwd(Interval& y, Interval& x1, Interval& p) { - assert_release(p.is_degenerated() && "ModOp::bwd(y,x1,x2) (with x1 and x2 intervals) not implemented yet"); - ModOp::bwd(x1, x2, p.mid()); + y &= ModOp::fwd(x1,p); + ModOp::bwd(y,x1,p); } } diff --git a/src/extensions/sympy/codac2_sympy.cpp b/src/extensions/sympy/codac2_sympy.cpp index 5226ca02b..281a335de 100644 --- a/src/extensions/sympy/codac2_sympy.cpp +++ b/src/extensions/sympy/codac2_sympy.cpp @@ -40,7 +40,11 @@ namespace codac2 Index flat_input_index = -1; if(layout.flat_index_of(x, flat_input_index)) return flat_input_index; - assert(false && "flat_input_index_of: expected a scalar input variable or a direct component of a vector/matrix input variable"); + else + { + assert(false && "flat_input_index_of: expected a scalar input variable or a direct component of a vector/matrix input variable"); + return 0; + } } pybind11::object remap_to_reference_symbols( diff --git a/tests/core/operators/codac2_tests_operators.cpp b/tests/core/operators/codac2_tests_operators.cpp index ab171964d..029463b82 100644 --- a/tests/core/operators/codac2_tests_operators.cpp +++ b/tests/core/operators/codac2_tests_operators.cpp @@ -161,15 +161,6 @@ void CHECK_bwd_sub(const Interval& y, CHECK(_x2 == -expected_x2); } -void CHECK_bwd_imod(double p, - const Interval& x1, const Interval& x2, const Interval& expected_x1, const Interval& expected_x2) -{ - Interval _x1, _x2; - _x1 = x1; _x2 = x2; ModOp::bwd(_x1,_x2,p); - CHECK(Approx(_x1) == expected_x1); - CHECK(Approx(_x2) == expected_x2); -} - TEST_CASE("Interval bwd operations") { const double pi_lb = Interval::pi().lb(); @@ -314,15 +305,6 @@ TEST_CASE("Interval bwd operations") CHECK_bwd_sub(Interval(0,0),Interval(-1,1),Interval(-1,1),Interval(-1,1),Interval(-1,1)); CHECK_bwd_sub(Interval(-1,1),Interval(1,2),Interval(-10,5),Interval(1,2),Interval(0,3)); - CHECK_bwd_imod(3.,Interval(3.,5.),Interval(1.,2.),Interval(4.,5.),Interval(1.,2.)); - CHECK_bwd_imod(2.,Interval(7.,8.),Interval(.5,2.),Interval(7.,8.),Interval(1.,2.)); - CHECK_bwd_imod(2.,Interval(7.,8.),Interval(0.,2.),Interval(7.,8.),Interval(0.,2.)); - CHECK_bwd_imod(2.*PI,Interval(2.*PI,3.*PI),Interval(PI/6,PI/2.),Interval(13.*PI/6.,5.*PI/2.),Interval(PI/6,PI/2.)); - CHECK_bwd_imod(2.*PI,Interval(3.*PI,4.*PI),Interval(PI/3,PI/2.),Interval::empty(),Interval::empty()); - CHECK_bwd_imod(2.*PI,Interval(3.*PI,4.*PI),Interval(0.,PI/2.),Interval(4*PI),Interval(0.)); - CHECK_bwd_imod(2.*PI,Interval(2.*PI,4.*PI),Interval(-PI/6,PI/2.),Interval(2.*PI,4.*PI),Interval(-PI/6,PI/2.)); - CHECK_bwd_imod(2.*PI,Interval(7.*PI/4.,8.*PI/3),Interval(-PI/2,PI/2.),Interval(7.*PI/4.,5.*PI/2.),Interval(-PI/4,PI/2.)); - x = Interval(-oo,oo); FloorOp::bwd(Interval::empty(),x); CHECK(x == Interval::empty()); x = Interval(-oo,-0.000001); FloorOp::bwd(Interval(-oo,-1),x); CHECK(x == Interval(-oo,-0.000001)); x = Interval(-oo, 0.000001); FloorOp::bwd(Interval(-oo,-1),x); CHECK(x == Interval(-oo,0)); @@ -438,3 +420,59 @@ TEST_CASE("test flatten operator") FlattenOp::bwd(N,M); CHECK(M == IntervalMatrix({ {{1,1.2},{2,2.2},{3,3.2}} , {4.0,{5,5.2},{6,6.2}} })); } + +TEST_CASE("Interval fwd modulo") +{ + CHECK(ModOp::fwd(Interval::empty(), {2}) == Interval::empty()); + CHECK(ModOp::fwd({-oo,oo}, {-oo,0}) == Interval::empty()); + + CHECK(ModOp::fwd({5}, {3}) == Interval(2)); + CHECK(ModOp::fwd({-1}, {3}) == Interval(2)); + + CHECK(ModOp::fwd({0.25,0.75}, {1,2}) == Interval(0.25,0.75)); + CHECK(ModOp::fwd({3.25,3.75}, {2}) == Interval(1.25,1.75)); + + CHECK(ModOp::fwd({-1,5}, {2}) == Interval(0,2)); + CHECK(ModOp::fwd({1,5}, {2,4}) == Interval(0,4)); + CHECK(ModOp::fwd({-1,5}, {-2,4}) == Interval(0,4)); +} + +TEST_CASE("Interval bwd modulo") +{ + Interval x, p; + + x = {-oo,oo}; p = {-oo,oo}; + ModOp::bwd(Interval::empty(), x, p); + CHECK(x == Interval::empty()); + CHECK(p == Interval::empty()); + + x = {-oo,oo}; p = {-oo,0}; + ModOp::bwd({0,1}, x, p); + CHECK(x == Interval::empty()); + CHECK(p == Interval::empty()); + + x = {-oo,oo}; p = {2,3}; + ModOp::bwd({5,6}, x, p); + CHECK(x == Interval::empty()); + CHECK(p == Interval::empty()); + + x = {0,3}; p = {3,4}; + ModOp::bwd({1,2}, x, p); + CHECK(x == Interval(1,2)); + CHECK(p == Interval(3,4)); + + x = {2.5,4}; p = {2,3}; + ModOp::bwd({1}, x, p); + CHECK(x == Interval(3,4)); + CHECK(p == Interval(2,3)); + + x = {5,5}; p = {1.9,2.1}; + ModOp::bwd({1}, x, p); + CHECK(Approx(x) == Interval(5)); + CHECK(Approx(p) == Interval(2)); + + x = {0.3,0.4}; p = {10,11}; + ModOp::bwd({0.1,0.2}, x, p); + CHECK(x == Interval::empty()); + CHECK(p == Interval::empty()); +} \ No newline at end of file diff --git a/tests/core/operators/codac2_tests_operators.py b/tests/core/operators/codac2_tests_operators.py index 977812bd9..719e752cd 100644 --- a/tests/core/operators/codac2_tests_operators.py +++ b/tests/core/operators/codac2_tests_operators.py @@ -140,12 +140,6 @@ def CHECK_bwd_sub(self, y, x1, x2, expected_x1, expected_x2): self.assertTrue(_x1 == -expected_x1) self.assertTrue(_x2 == -expected_x2) - def CHECK_bwd_imod(self, p, x1, x2, expected_x1, expected_x2): - _x1 = Interval(); _x2 = Interval() - _x1 = x1; _x2 = x2; ModOp.bwd(_x1,_x2,p) - self.assertTrue(Approx(_x1) == expected_x1) - self.assertTrue(Approx(_x2) == expected_x2) - def test_interval_bwd(self): pi_lb = Interval.pi().lb() @@ -291,15 +285,6 @@ def test_interval_bwd(self): self.CHECK_bwd_sub(Interval(0,0),Interval(-1,1),Interval(-1,1),Interval(-1,1),Interval(-1,1)) self.CHECK_bwd_sub(Interval(-1,1),Interval(1,2),Interval(-10,5),Interval(1,2),Interval(0,3)) - self.CHECK_bwd_imod(3.,Interval(3.,5.),Interval(1.,2.),Interval(4.,5.),Interval(1.,2.)) - self.CHECK_bwd_imod(2.,Interval(7.,8.),Interval(.5,2.),Interval(7.,8.),Interval(1.,2.)) - self.CHECK_bwd_imod(2.,Interval(7.,8.),Interval(0.,2.),Interval(7.,8.),Interval(0.,2.)) - self.CHECK_bwd_imod(2.*math.pi,Interval(2.*math.pi,3.*math.pi),Interval(math.pi/6,math.pi/2.),Interval(13.*math.pi/6.,5.*math.pi/2.),Interval(math.pi/6,math.pi/2.)) - self.CHECK_bwd_imod(2.*math.pi,Interval(3.*math.pi,4.*math.pi),Interval(math.pi/3,math.pi/2.),Interval.empty(),Interval.empty()) - self.CHECK_bwd_imod(2.*math.pi,Interval(3.*math.pi,4.*math.pi),Interval(0.,math.pi/2.),Interval(4*math.pi),Interval(0.)) - self.CHECK_bwd_imod(2.*math.pi,Interval(2.*math.pi,4.*math.pi),Interval(-math.pi/6,math.pi/2.),Interval(2.*math.pi,4.*math.pi),Interval(-math.pi/6,math.pi/2.)) - self.CHECK_bwd_imod(2.*math.pi,Interval(7.*math.pi/4.,8.*math.pi/3),Interval(-math.pi/2,math.pi/2.),Interval(7.*math.pi/4.,5.*math.pi/2.),Interval(-math.pi/4,math.pi/2.)) - x = Interval(-oo,oo); FloorOp.bwd(Interval.empty(),x); self.assertTrue(x == Interval.empty()) x = Interval(-oo,-0.000001); FloorOp.bwd(Interval(-oo,-1),x); self.assertTrue(x == Interval(-oo,-0.000001)) x = Interval(-oo, 0.000001); FloorOp.bwd(Interval(-oo,-1),x); self.assertTrue(x == Interval(-oo,0)) @@ -372,6 +357,58 @@ def test_flatten_operator(self): FlattenOp.bwd(N,M) self.assertTrue(M == IntervalMatrix([[[1,1.2],[2,2.2],[3,3.2]],[4.0,[5,5.2],[6,6.2]]])) + def test_interval_modulo(self): + + self.assertTrue(ModOp.fwd(Interval.empty(), Interval(2)) == Interval.empty()) + self.assertTrue(ModOp.fwd(Interval(-oo,oo), Interval(-oo,0)) == Interval.empty()) + + self.assertTrue(ModOp.fwd(Interval(5), Interval(3)) == Interval(2)) + self.assertTrue(ModOp.fwd(Interval(-1), Interval(3)) == Interval(2)) + + self.assertTrue(ModOp.fwd(Interval(0.25,0.75), Interval(1,2)) == Interval(0.25,0.75)) + self.assertTrue(ModOp.fwd(Interval(3.25,3.75), Interval(2)) == Interval(1.25,1.75)) + + self.assertTrue(ModOp.fwd(Interval(-1,5), Interval(2)) == Interval(0,2)) + self.assertTrue(ModOp.fwd(Interval(1,5), Interval(2,4)) == Interval(0,4)) + self.assertTrue(ModOp.fwd(Interval(-1,5), Interval(-2,4)) == Interval(0,4)) + + x = Interval() + p = Interval() + + x = Interval(-oo,oo); p = Interval(-oo,oo) + ModOp.bwd(Interval.empty(), x, p) + self.assertTrue(x == Interval.empty()) + self.assertTrue(p == Interval.empty()) + + x = Interval(-oo,oo); p = Interval(-oo,0) + ModOp.bwd(Interval(0,1), x, p) + self.assertTrue(x == Interval.empty()) + self.assertTrue(p == Interval.empty()) + + x = Interval(-oo,oo); p = Interval(2,3) + ModOp.bwd(Interval(5,6), x, p) + self.assertTrue(x == Interval.empty()) + self.assertTrue(p == Interval.empty()) + + x = Interval(0,3); p = Interval(3,4) + ModOp.bwd(Interval(1,2), x, p) + self.assertTrue(x == Interval(1,2)) + self.assertTrue(p == Interval(3,4)) + + x = Interval(2.5,4); p = Interval(2,3) + ModOp.bwd(Interval(1), x, p) + self.assertTrue(x == Interval(3,4)) + self.assertTrue(p == Interval(2,3)) + + x = Interval(5,5); p = Interval(1.9,2.1) + ModOp.bwd(Interval(1), x, p) + self.assertTrue(Approx(x) == Interval(5)) + self.assertTrue(Approx(p) == Interval(2)) + + x = Interval(0.3,0.4); p = Interval(10,11) + ModOp.bwd(Interval(0.1,0.2), x, p) + self.assertTrue(x == Interval.empty()) + self.assertTrue(p == Interval.empty()) if __name__ == '__main__': unittest.main()