Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion doc/manual/manual/intervals/Interval_class.rst
Original file line number Diff line number Diff line change
Expand Up @@ -248,10 +248,11 @@ Mathematical functions such as :math:`\sin`, :math:`\cos`, :math:`\exp`, :math:`

.. code-tab:: matlab

x = Interval.half_pi();
x = Interval().half_pi();
x.self_union(0); % x = [0, π/2]
y = sin(x); % y = [0, 1]
z = exp(x); % z = [1, exp(π/2)]
w = y.inter(z); % w = [1, 1]

For a complete list of additional operations, see the page :ref:`sec-functions-analytic-operators`.

Expand Down
1 change: 1 addition & 0 deletions doc/manual/manual/intervals/src.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ TEST_CASE("Interval class - manual")
x |= 0; // x = [0, π/2]
Interval y = sin(x); // y = [0, 1]
Interval z = exp(x); // z = [1, e^(π/2)]
Interval w = y & z; // w = [1, 1]
// [interval-class-6-end]
CHECK(Approx(x) == Interval(0,PI/2));
CHECK(Approx(y) == Interval(0,1));
Expand Down
1 change: 1 addition & 0 deletions doc/manual/manual/intervals/src.py
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ def tests_Interval_manual(test):
x |= 0 # x = [0, π/2]
y = sin(x) # y = [0, 1]
z = exp(x) # z = [1, e^(π/2)]
w = y & z # w = [1, 1]
# [interval-class-6-end]
test.assertTrue(Approx(x) == Interval(0,PI/2))
test.assertTrue(Approx(y) == Interval(0,1))
Expand Down
56 changes: 56 additions & 0 deletions doc/manual/tuto/cp_robotics/lesson_a_static_range_and_bearing.rst
Original file line number Diff line number Diff line change
Expand Up @@ -286,6 +286,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal
:end-before: [A-q2-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q2-beg]
:end-before: [A-q2-end]
:dedent: 0


**A.3.** Create the bounded sets related to the state, the measurement and the landmark position: :math:`[\mathbf{x}]\in\mathbb{IR}^3`, :math:`[\mathbf{y}]\in\mathbb{IR}^2`, :math:`[\mathbf{m}]\in\mathbb{IR}^2`. We can for instance use the ``.inflate(float radius)`` method on intervals or boxes.

Expand Down Expand Up @@ -321,6 +329,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal
:end-before: [A-q3-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q3-beg]
:end-before: [A-q3-end]
:dedent: 0

**A.4.** Display the vehicle and the landmark with:

.. tabs::
Expand Down Expand Up @@ -374,6 +390,14 @@ A robot depicted by the state :math:`\mathbf{x}=\left(2,1,\pi/6\right)^\intercal
:end-before: [A-q5-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q5-beg]
:end-before: [A-q5-end]
:dedent: 0

As one can see, intervals are not limited to axis-aligned boxes: we sometimes perform rotational mapping to better fit the set to represent. This polar constraint is a case in point.


Expand Down Expand Up @@ -411,6 +435,14 @@ We will implement the decomposition of Question **A.1** using contractors and a
:end-before: [A-q6-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q6-beg]
:end-before: [A-q6-end]
:dedent: 0

| **A.7.** Create the intermediate variables introduced in Question **A.1**. They are ``Interval`` and ``IntervalVector`` objects, as for the other variables.
| Note that the intermediate variables do not have to be initialized with prior values. For ``IntervalVector`` objects, you only have to define their size.

Expand All @@ -434,6 +466,14 @@ We will implement the decomposition of Question **A.1** using contractors and a
:end-before: [A-q7-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q7-beg]
:end-before: [A-q7-end]
:dedent: 0

The current problem can be solved in one step with a smart order of contractors. You may find this order and run the contractors without iterative loop. However, this order may not always exist (due to dependencies in the constraints) or, in a pure declarative paradigm, we may not want to think about it. Using fixed-point iteration can then simplify the algorithm. In Codac, this is allowed using the ``fixpoint(..)`` function that will execute some ``contractors_list`` function until a fixpoint is reached. The `fixpoint` corresponds to a point where the involved domains are not contracted anymore.

The following code illustrates how to implement such fixpoint:
Expand Down Expand Up @@ -485,6 +525,14 @@ The ``fixpoint`` function will execute the content of the function ``contractors
:end-before: [A-q8-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q8-beg]
:end-before: [A-q8-end]
:dedent: 0

| **A.9.** Run your programm to solve the problem. You should obtain this figure:

.. figure:: img/result_rangebearing.png
Expand All @@ -510,6 +558,14 @@ The ``fixpoint`` function will execute the content of the function ``contractors
:end-before: [A-q9-end]
:dedent: 2

.. group-tab:: Matlab

.. literalinclude:: src/lesson_A.m
:language: matlab
:start-after: [A-q9-beg]
:end-before: [A-q9-end]
:dedent: 0

The black box :math:`[\mathbf{x}]` cumulates all the uncertainties of the problem:

* uncertainties of the measurement
Expand Down
2 changes: 1 addition & 1 deletion doc/manual/tuto/cp_robotics/src/lesson_A.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ int main()
// [A-q2-beg]
Vector x_truth({2,1,PI/6});
Vector y_truth({6,PI/6});
Vector m_truth({5,6});
Vector m_truth({5,6.2});
// [A-q2-end]

// [A-q3-beg]
Expand Down
62 changes: 62 additions & 0 deletions doc/manual/tuto/cp_robotics/src/lesson_A.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
import py.codac4matlab.*

% [A-q2-beg]
x_truth = Vector([2.,1.,PI/6.0]);
y_truth = Vector([6.,PI/6.0]);
m_truth = Vector([5.,6.2]);
% [A-q2-end]

% [A-q3-beg]
x = cart_prod(Interval(-oo,oo),Interval(-oo,oo),x_truth(3));
m = IntervalVector(m_truth).inflate(0.2);
y = IntervalVector(y_truth).inflate(Vector([0.3,0.1]));
% [A-q3-end]

DefaultFigure().draw_box(m,Color().red());
DefaultFigure().draw_tank(x_truth, 1, StyleProperties({Color().black(),Color().yellow()}));

% [A-q5-beg]
DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1), x_truth(3)+y(2), Color().red());
DefaultFigure().draw_pie(x_truth.subvector(1,2), y(1).union(0), x_truth(3)+y(2), Color().light_gray());
% [A-q5-end]

% [A-q6-beg]
ctc_polar = CtcPolar();

x123 = VectorVar(7);
f_minus = AnalyticFunction({x123},vec(x123(1)-x123(3)-x123(6), x123(2)-x123(4)-x123(7)));
ctc_minus = CtcInverse(f_minus, Vector([0,0]));

s =VectorVar(3);
f_plus = AnalyticFunction({s}, s(1)+s(2)-s(3));
ctc_plus = CtcInverse(f_plus, Interval(0,0));
% [A-q6-end]

% [A-q7-beg]
a = Interval();
d = IntervalVector(2);
% [A-q7-end]

% [A-q8-beg]
% Either with a smart order of contractor calls:
res_ctc_plus = ctc_plus.contract(cart_prod(x(3), y(2), a)); % The result is a 3D IntervalVector
x.setitem(3,res_ctc_plus(1));
y.setitem(2,res_ctc_plus(2));
a = res_ctc_plus(3);

res_ctc_polar = ctc_polar.contract(cart_prod(d(1),d(2),y(1),a)); % The result is a 4D IntervalVector
d.setitem(1,res_ctc_polar(1));
d.setitem(2,res_ctc_polar(2));
y.setitem(1,res_ctc_polar(3));
a = res_ctc_polar(4);

res_ctc_minus = ctc_minus.contract(cart_prod(m,x,d)); % The result is a 7D IntervalVector
m = res_ctc_minus.subvector(1,2);
x = res_ctc_minus.subvector(3,5);
d = res_ctc_minus.subvector(6,7);
% [A-q8-end]

% [A-q9-beg]
x
DefaultFigure().draw_box(x) % does not display anything if unbounded
% [A-q9-end]
2 changes: 1 addition & 1 deletion doc/manual/tuto/cp_robotics/src/lesson_A.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
# [A-q2-beg]
x_truth = Vector([2,1,PI/6])
y_truth = Vector([6,PI/6])
m_truth = Vector([5,6])
m_truth = Vector([5,6.2])
# [A-q2-end]

# [A-q3-beg]
Expand Down
16 changes: 8 additions & 8 deletions examples/11_peibos/main.m
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
X_2d = VectorVar(1);
psi0_2d = AnalyticFunction({X_2d},vec(sin(X_2d(1)*PI/4.),cos(X_2d(1)*PI/4.)));

id_2d = OctaSym(int64([1,2]));
s = OctaSym(int64([-2,1]));
id_2d = OctaSym([1,2]);
s = OctaSym([-2,1]);

v_par_2d = PEIBOS(f_2d,psi0_2d,{id_2d,s,s*s,s.invert()},0.2,Vector([-0.2,0.]),true);

Expand Down Expand Up @@ -45,9 +45,9 @@
X_3d = VectorVar(2);
psi0_3d = AnalyticFunction({X_3d},vec(1/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2))),X_3d(1)/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2))),X_3d(2)/sqrt(1+sqr(X_3d(1))+sqr(X_3d(2)))));

id_3d = OctaSym(int64([1,2,3]));
s1 = OctaSym(int64([-2, 1, 3]));
s2 = OctaSym(int64([3, 2, -1]));
id_3d = OctaSym([1, 2, 3]);
s1 = OctaSym([-2, 1, 3]);
s2 = OctaSym([3, 2, -1]);

v_par_3d = PEIBOS(f_3d,psi0_3d,{id_3d,s1,s1*s1,s1.invert(),s2,s2.invert()},0.2,true);

Expand Down Expand Up @@ -76,9 +76,9 @@
X_nd = VectorVar(1);
psi0_nd = AnalyticFunction({X_nd},vec(X_nd(1),1,1));

id_nd = OctaSym(int64([1, 2, 3]));
s1_nd = OctaSym(int64([-2, 1, 3]));
s2_nd = OctaSym(int64([3, 2, -1]));
id_nd = OctaSym([1, 2, 3]);
s1_nd = OctaSym([-2, 1, 3]);
s2_nd = OctaSym([3, 2, -1]);

figure_3d_nd = Figure3D("Cube on Sphere matlab");
figure_3d_nd.draw_axes(0.5);
Expand Down
12 changes: 5 additions & 7 deletions python/src/core/matrices/codac2_py_MatrixBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -119,15 +119,13 @@ void export_MatrixBase(py::module& m, py::class_<S>& pyclass)
#else
"__setitem__"
#endif
, [](S& x, const py::tuple& ij, const T& a)
, [](S& x, std::vector<Index_type> ij, const T& a)
{
if constexpr(FOR_MATLAB)
assert_release(py::isinstance<py::int_>(ij[0]) && py::isinstance<py::int_>(ij[1]));

int i = ij[0].cast<int>();
int j = ij[1].cast<int>();
auto ij_conv = matlab::convert_indices(ij);
int i = ij_conv[0];
int j = ij_conv[1];

x(matlab::input_index(i), matlab::input_index(j)) = a;
x(i, j) = a;
},
MATRIX_ADDONS_BASE_SCALAR_REF_OPERATORCALL_INDEX_INDEX)

Expand Down
2 changes: 1 addition & 1 deletion python/src/core/matrices/codac2_py_VectorBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void export_VectorBase([[maybe_unused]] py::module& m, py::class_<S>& pyclass)

.def(
#if FOR_MATLAB
"__call__"
"setitem"
#else
"__setitem__"
#endif
Expand Down