Merge branch 'master' of https://github.com/lebarsfa/codac into lebar…

…sfa-master

doc/doc/install/04-start-matlab-project.rst

@@ -18,8 +18,15 @@ Start a MATLAB project
 
    | Integers in function calls from the Codac library should be cast to `int32`, e.g. `4` would be `int32(4)` (however do not change those that are in strings), otherwise remember that all numbers are `double` by default in MATLAB. 
    | The operator `[index]` for a Codac object should be replaced with `.getitem(int32(index))` when getting its value while it should be replaced with `.setitem(int32(index))` when setting its value (please note that indices of Codac objects start at `0` contrary to typical MATLAB objects such as MATLAB arrays or cell arrays). 
+   | The operators/functions `x&y` (intersection) for a Codac object should be replaced with `x.inter(y)`, `x|y` (union) with `x.union(y)`, `x&=y` (intersection and update of x) with `x.inter_self(y)`, `x|=y` (union and update of x) with `x.union_self(y)`, `x**y` (power) with `x.pow(y)`, `abs(x)` (absolute) with `x.abs()`. 
    | Python lists of objects should be converted to MATLAB cell arrays. 
-   | Also, when a Codac function needs a Python list parameter, the corresponding MATLAB cell array should be given as `py.list(...)`. 
+   | Also, when a Codac function needs a `py.list` or `Vector` parameter, the corresponding MATLAB cell array should be given as `py.list(...)` (however when the Codac function do not need a `py.list` or `Vector` parameter but just an element of a MATLAB cell array, do not convert with `py.list(...)` and be sure to get the cell array element with `{}` operator). 
+   | Be sure that Python multiline strings are correctly converted to multiline MATLAB strings between `'`. Remember that multiline statements in MATLAB need `...` before next line.
+   | Please also convert Python `for` loops to typical MATLAB `for` loops, same for `if-else`, `+=` statements. 
+
+.. admonition:: Automating Python to MATLAB conversions
+
+   | `Bing Chat with GPT-4 <https://www.bing.com/>`_ in `Precise` conversation style has been used successfully to help convert Python examples to MATLAB, using the description above and by providing `01_getting_started.py` and the corresponding `a01_getting_started.m` as example (about 80% of the code for `03_static_rangebearing.m` and `05_dyn_rangebearing.m` was correctly converted this way).
 
 .. code-block:: matlab
   

doc/doc/tutorial/08-rangeonly-slam/solution.m

@@ -0,0 +1,125 @@
+% Codac - Examples
+% Dynamic range-bearing localization
+% ----------------------------------------------------------------------------
+
+import py.codac.*
+
+% =========== CREATING DATA ===========
+
+dt = 0.05;
+iteration_dt = 0.2;
+tdomain = Interval(0,15); % [t0,tf]
+
+% Initial pose x0=(0,0,2)
+x0 = [0, 0, 2];
+
+% System input
+u = Trajectory(tdomain, TFunction('3*(sin(t)^2)+t/100'), dt);
+
+% Noise
+i_n = Interval(-0.03,0.03); % the noises are known to be bounded by i_n
+
+n_u = RandTrajectory(tdomain, dt, i_n); % input noise
+n_theta = RandTrajectory(tdomain, dt, i_n); % heading noise
+
+% Actual trajectories (state + derivative)
+v_truth = TrajectoryVector(int32(3));
+x_truth = TrajectoryVector(int32(3));
+v_truth.setitem(int32(2), u + n_u);
+x_truth.setitem(int32(2), v_truth.getitem(int32(2)).primitive() + x0(3));
+v_truth.setitem(int32(0), 10*cos(x_truth.getitem(int32(2))));
+v_truth.setitem(int32(1), 10*sin(x_truth.getitem(int32(2))));
+x_truth.setitem(int32(0), v_truth.getitem(int32(0)).primitive() + x0(1));
+x_truth.setitem(int32(1), v_truth.getitem(int32(1)).primitive() + x0(2));
+
+% Bounded trajectories (dead reckoning)
+v = TubeVector(tdomain, dt, int32(3));
+x = TubeVector(tdomain, dt, int32(3));
+v.setitem(int32(2), Tube(u, dt).inflate(i_n.rad())); % command u with bounded uncertainties
+x.setitem(int32(2), Tube(x_truth.getitem(int32(2))+n_theta, dt).inflate(i_n.rad())); % heading measurement with bounded uncertainties
+v.setitem(int32(0), 10*cos(x.getitem(int32(2))));
+v.setitem(int32(1), 10*sin(x.getitem(int32(2))));
+x = v.primitive()+IntervalVector(x0); % dead reckoning
+
+% Set of landmarks
+v_m = { py.list([6,12]), py.list([-2,-5]), py.list([-3,20]), py.list([3,4]) };
+
+% =========== GRAPHICS ===========
+
+beginDrawing();
+
+fig_map = VIBesFigMap('slam');
+fig_map.set_properties(int32(50), int32(50), int32(1200), int32(600));
+fig_map.add_tube(x, 'x', int32(0), int32(1));
+fig_map.add_trajectory(x_truth, 'truth', int32(0), int32(1), 'white');
+fig_map.smooth_tube_drawing(true);
+fig_map.add_landmarks(py.list(v_m), single(0.4));
+fig_map.show(double(1));
+
+% =========== CONTRACTOR NETWORK ===========
+
+v_m_boxes = cell(size(v_m));
+for i=1:length(v_m)
+  v_m_boxes(i) = {IntervalVector(int32(2))};
+end
+
+% Contractor Network:
+
+cn = ContractorNetwork();
+
+t = tdomain.lb();
+prev_t_obs = t;
+
+while t < tdomain.ub()
+
+  if t-prev_t_obs > 2*dt % new observation each 2*delta
+
+    % Creating new observation to a random landmark
+
+    landmark_id = randi([1 length(v_m)]); % a random landmark is perceived
+
+    xt = double(x_truth(t));
+    pos_x = [xt(1), xt(2)];
+    pos_b = double(v_m{landmark_id});
+
+    yi = Interval(sqrt((pos_x(1)-pos_b(1))^2+(pos_x(2)-pos_b(2))^2));
+    yi.inflate(0.03); % adding range bounded uncertainty
+
+    prev_t_obs = t;
+
+    % Adding related observation constraints to the network
+
+    % Alias (for ease of reading)
+    b = v_m_boxes{landmark_id};
+
+    % Intermediate variables
+    ti = Interval(t);
+    xi = IntervalVector(int32(3));
+
+    % Contractors
+    cn.add(CtcEval(), py.list({ti, xi, x, v}));
+    cn.add(CtcDist(), py.list({xi.getitem(int32(0)), xi.getitem(int32(1)), b.getitem(int32(0)), b.getitem(int32(1)), yi}));
+
+  end
+
+  contraction_dt = cn.contract_during(iteration_dt);  
+  if iteration_dt>contraction_dt
+    pause(iteration_dt-contraction_dt); % iteration delay
+  end
+
+  % Display the current slice x
+  fig_map.draw_box(x(t).subvector(int32(0),int32(1)));
+
+  t = t + dt;
+
+end
+
+cn.contract(true); % lets the solver run the remaining contractions
+
+fig_map.show();
+for i=1:length(v_m_boxes)
+  b = v_m_boxes{i};
+  fig_map.draw_box(b);
+end
+
+endDrawing();

examples/tuto/01_getting_started/01_getting_started.py

@@ -1,5 +1,5 @@
 # Codac - Examples
-# Dynamic range-only localization
+# Getting started: 2 minutes to Codac
 # ----------------------------------------------------------------------------
 
 from codac import *

examples/tuto/01_getting_started/a01_getting_started.m

@@ -1,5 +1,5 @@
 % Codac - Examples
-% Dynamic range-only localization
+% Getting started: 2 minutes to Codac
 % ----------------------------------------------------------------------------
 
 import py.codac.*
@@ -87,5 +87,4 @@
 
 
 % Checking if this example still works:
-if x.volume() < 5; check = true; else; check = false; end % todo: x.contains(x_truth)
-assert(check)
+assert(x.volume() < 5) % todo: x.contains(x_truth)

examples/tuto/03_static_rangebearing/a03_static_rangebearing.m

@@ -0,0 +1,86 @@
+% Codac - Examples
+% Static range-bearing localization
+% ----------------------------------------------------------------------------
+
+import py.codac.*
+
+% =================== 0. Parameters, truth and data ====================
+
+% Truth (unknown pose)
+x_truth = [0,0,pi/6]; % (x,y,heading)
+
+% Creating random map of landmarks
+map_area = IntervalVector(int32(2), [-8,8]);
+v_map = DataLoader().generate_landmarks_boxes(map_area, int32(1));
+
+% The following function generates a set of [range]x[bearing] values
+v_obs = DataLoader().generate_static_observations(py.list(x_truth), v_map, false);
+
+% Adding uncertainties on the measurements
+for i=1:length(v_obs) % for each observation:
+  v_obs{i}.getitem(int32(0)).inflate(0.3); % range
+  v_obs{i}.getitem(int32(1)).inflate(0.1); % bearing
+end
+
+
+% =============== 1. Defining domains for our variables ================
+
+x = IntervalVector(int32(2)); % unknown position
+heading = Interval(x_truth(3)).inflate(0.01); % measured heading
+
+
+% =========== 2. Defining contractors to deal with equations ===========
+
+ctc_plus = CtcFunction(Function('a', 'b', 'c', 'a+b-c')); % a+b=c
+ctc_minus = CtcFunction(Function('a', 'b', 'c', 'a-b-c')); % a-b=c
+% We also use the predefined contractor CtcPolar(), no need to build it
+
+
+% =============== 3. Adding the contractors to a network ===============
+
+cn = ContractorNetwork();
+
+for i=1:length(v_obs)
+
+  % Intermediate variables
+  alpha = cn.create_interm_var(Interval());
+  d = cn.create_interm_var(IntervalVector(int32(2)));
+
+  cn.add(ctc_plus, py.list({v_obs{i}.getitem(int32(1)), heading, alpha}));
+  cn.add(ctc_minus, py.list({v_map{i}, x, d}));
+  cn.add(CtcPolar(), py.list({d, v_obs{i}.getitem(int32(0)), alpha}));
+end
+
+
+% ======================= 4. Solving the problem =======================
+
+cn.contract();
+
+
+% ============================ 5. Graphics =============================
+
+beginDrawing();
+
+fig = VIBesFigMap('Map');
+fig.set_properties(int32(50), int32(50), int32(600), int32(600));
+
+for i=1:length(v_map)
+  iv = v_map{i};
+  fig.add_beacon(iv.mid(), 0.2);
+end
+
+for i=1:length(v_obs)
+  y = v_obs{i};
+  fig.draw_pie(x_truth(1), x_truth(2), y.getitem(int32(0)).union(Interval(0)), heading+y.getitem(int32(1)), 'lightGray');
+  fig.draw_pie(x_truth(1), x_truth(2), y.getitem(int32(0)), heading+y.getitem(int32(1)), 'gray');
+end
+
+fig.draw_vehicle(py.list(x_truth),0.5);
+fig.draw_box(x); % estimated position
+fig.show();
+
+endDrawing();
+
+
+% Checking if this example still works:
+assert(x.contains(py.list(x_truth(1:2))))

examples/tuto/05_dyn_rangebearing/05_dyn_rangebearing.py

@@ -1,5 +1,5 @@
 # Codac - Examples
-# Dynamic range-only localization
+# Dynamic range-bearing localization
 # ----------------------------------------------------------------------------
 
 from codac import *

examples/tuto/05_dyn_rangebearing/a05_dyn_rangebearing.m

@@ -0,0 +1,111 @@
+% Codac - Examples
+% Dynamic range-bearing localization
+% ----------------------------------------------------------------------------
+
+import py.codac.*
+
+
+% =================== 0. Parameters, truth and data ====================
+
+dt = 0.05;                                      % timestep for tubes accuracy
+tdomain = Interval(0,3);                        % temporal limits [t_0,t_f]=[0,3]
+
+x_truth = TrajectoryVector(tdomain, TFunction(['(' ...
+  '10*cos(t)+t ;' ...
+  '5*sin(2*t)+t ;' ...
+  'atan2((10*cos(2*t)+1),(-10*sin(t)+1)) ;' ...
+  'sqrt((-10*sin(t)+1)^2+(10*cos(2*t)+1)^2))'])); % actual trajectory
+
+% Continuous measurements coming from the truth
+measured_psi = x_truth.getitem(int32(2)).sample(dt).make_continuous();
+measured_psi = measured_psi + RandTrajectory(tdomain, dt, Interval(-0.01,0.01)); % adding some noise
+measured_speed = x_truth.getitem(int32(3)).sample(dt);
+measured_speed = measured_speed + RandTrajectory(tdomain, dt, Interval(-0.01,0.01)); % adding some noise
+
+% Creating random map of landmarks
+map_area = IntervalVector(int32(2), [-8,8]);
+v_map = DataLoader().generate_landmarks_boxes(map_area, int32(30));
+
+% The following function generates a set of [range]x[bearing] values
+v_obs = DataLoader().generate_observations(x_truth, v_map, int32(10));
+
+% Adding uncertainties on the measurements
+for i=1:length(v_obs) % for each observation:
+  obs = v_obs{i};
+  obs.getitem(int32(1)).inflate(0.3); % range
+  obs.getitem(int32(2)).inflate(0.1); % bearing
+end
+
+% =============== 1. Defining domains for our variables ================
+
+x = TubeVector(tdomain, dt, int32(4));                    % 4d tube for state vectors
+v = TubeVector(tdomain, dt, int32(4));                    % 4d tube for derivatives of the states
+u = TubeVector(tdomain, dt, int32(2));                    % 2d tube for inputs of the system
+
+x.setitem(int32(2), Tube(measured_psi, dt).inflate(0.01));       % measured_psi is a set of measurements
+x.setitem(int32(3), Tube(measured_speed, dt).inflate(0.01));
+
+
+% =========== 2. Defining contractors to deal with equations ===========
+
+ctc_f = CtcFunction(Function('v[4]', 'x[4]', 'u[2]', ...
+                             '(v[0]-x[3]*cos(x[2]) ; v[1]-x[3]*sin(x[2]) ; v[2]-u[0] ; v[3]-u[1])'));
+ctc_plus = CtcFunction(Function('a', 'b', 'c', 'a+b-c')); % a+b=c
+ctc_minus = CtcFunction(Function('a', 'b', 'c', 'a-b-c')); % a-b=c
+% We also use the predefined contractors CtcPolar(), CtcEval(), no need to build them
+
+
+% =============== 3. Adding the contractors to a network ===============
+
+cn = ContractorNetwork();   % creating a network
+cn.add(ctc_f, py.list({v, x, u}));   % adding the f constraint
+
+for i=1:length(v_obs) % we add the observ. constraint for each range-only measurement
+  y = v_obs{i};
+
+  % Intermediate variables
+  alpha = cn.create_interm_var(Interval()); % absolute angle robot-landmark
+  d = cn.create_interm_var(IntervalVector(int32(2))); % dist robot-landmark
+  p = cn.create_interm_var(IntervalVector(int32(4))); % state at t_i
+
+  cn.add(ctc_plus, py.list({y.getitem(int32(2)), p.getitem(int32(2)), alpha}));
+  cn.add(ctc_minus, py.list({cn.subvector(y,int32(3),int32(4)), cn.subvector(p,int32(0),int32(1)), d}));
+  cn.add(CtcPolar(), py.list({d, y.getitem(int32(1)), alpha}));
+  cn.add(CtcEval(), py.list({y.getitem(int32(0)), p, x, v}));
+end
+
+
+% ======================= 4. Solving the problem =======================
+
+cn.contract(true);
+
+
+% ============================ 5. Graphics =============================
+
+beginDrawing();
+fig = VIBesFigMap('fig');
+fig.set_properties(int32(50), int32(50), int32(900), int32(550));
+fig.add_trajectory(x_truth, 'xtruth', int32(0), int32(1), int32(2));
+fig.add_tube(x, 'x', int32(0), int32(1));
+fig.smooth_tube_drawing(true);
+
+for i=1:length(v_map)
+  b = v_map{i};
+  fig.add_beacon(b.mid(), 0.2); % drawing beacons
+end
+
+for i=1:length(v_obs)
+  y = v_obs{i};
+  t_obs = y.getitem(int32(0)).mid();
+  t_state = x_truth(t_obs);
+  fig.draw_pie(t_state{1}, t_state{2}, y.getitem(int32(1)).union(Interval(0.01)), t_state{3} + y.getitem(int32(2)), 'lightGray'); % drawing range-bearing measurements
+  fig.draw_pie(t_state{1}, t_state{2}, y.getitem(int32(1)), t_state{3} + y.getitem(int32(2)), 'darkGray'); % drawing range-bearing measurements
+  fig.draw_vehicle(t_obs, x_truth, 0.7);
+end
+
+fig.show(double(0));
+endDrawing();
+
+
+% Checking if this example still works:
+assert(x.contains(x_truth) == py.codac.core.BoolInterval(int32(2)));