Preparing Lesson E of tutorial

doc/manual/manual/tools/registration.rst

@@ -27,7 +27,7 @@ Inputs ``src`` and ``dst`` can be provided either as a list of 2d points, or wit
   .. code-tab:: py
 
     def affine_transformation(src, dst):
-    # with src/dst: two lists of Vector objects, or two SampledVectorTraj objects
+    # with src/dst: two lists of Vector objects, or two SampledTraj_Vector objects
 
   .. code-tab:: c++
 
@@ -70,7 +70,7 @@ The following example estimates a transformation between two sampled trajectorie
 
     # Reconstructing the dst trajectory using the estimated transformation
 
-    dst_estim = SampledVectorTraj()
+    dst_estim = SampledTraj_Vector()
     for ti,src_i in src:
       dst_estim.set(ti, tr*src_i)
 

doc/manual/tuto/cp_robotics/src/lesson_C.m

@@ -49,7 +49,7 @@
 
 s = RobotSimulator();
 s.w_max = 0.2; % maximum turning speed
-u = SampledVectorTraj(); % the simulator will return the inputs (not used)
+u = SampledTraj_Vector(); % 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]
 

doc/manual/tuto/cp_robotics/src/lesson_C.py

@@ -66,7 +66,7 @@ def g(t,x,M):
 
 s = RobotSimulator()
 s.w_max = 0.2 # maximum turning speed
-u = SampledVectorTraj() # the simulator will return the inputs (not used)
+u = SampledTraj_Vector() # the simulator will return the inputs (not used)
 x_truth = s.simulate(
   [0,0,0,0], # initial state (will be supposed unknown)
   1e-2, # simulation time step

examples/09_robot_simu/main.py

@@ -5,7 +5,7 @@
 ]
 
 s = RobotSimulator()
-u = SampledVectorTraj()
+u = SampledTraj_Vector()
 x = s.simulate([0,0,0,0], 1e-2, wpts, u)
 
 g = Figure2D("Robot simulation", GraphicOutput.VIBES | GraphicOutput.IPE)

python/codac/core/__init__.py

@@ -378,6 +378,13 @@ def tube_cart_prod(*x):
     return tube_cart_prod_list([*x])
 
 
+def traj_cart_prod(*x):
+  if not isinstance(x,tuple):
+    return traj_cart_prod_list([x])
+  else:
+    return traj_cart_prod_list([*x])
+
+
 class AnalyticTraj:
 
   def __init__(self, f, t):
@@ -445,11 +452,11 @@ def __init__(self, x, y=None):
     else:
       if isinstance(y, AnalyticFunction):
         self.__init__(x, y.f)
-      elif isinstance(y, (Interval,AnalyticFunction_Scalar,SampledScalarTraj)):
+      elif isinstance(y, (Interval,AnalyticFunction_Scalar,SampledTraj_Scalar)):
         self.tube = SlicedTube_Interval(x, y)
-      elif isinstance(y, (IntervalVector,AnalyticFunction_Vector,SampledVectorTraj)):
+      elif isinstance(y, (IntervalVector,AnalyticFunction_Vector,SampledTraj_Vector)):
         self.tube = SlicedTube_IntervalVector(x, y)
-      elif isinstance(y, (IntervalMatrix,AnalyticFunction_Matrix,SampledMatrixTraj)):
+      elif isinstance(y, (IntervalMatrix,AnalyticFunction_Matrix,SampledTraj_Matrix)):
         self.tube = SlicedTube_IntervalMatrix(x, y)
       else:
         codac_error("SlicedTube: can only build this tube from an AnalyticFunction_[Scalar/Vector/Matrix]")
@@ -536,8 +543,8 @@ def partial_integral(self,t1,t2=None):
     else:
       return self.tube.partial_integral(t1,t2)
 
-  def primitive(self):
-    return self.tube.primitive()
+  def primitive(self,*args):
+    return self.tube.primitive(*args)
 
   def as_function(self):
     return AnalyticFunction(self.tube.as_function())
@@ -551,6 +558,9 @@ def all_reals_value(self):
   def empty_value(self):
     return self.tube.empty_value()
 
+  def mid(self):
+    return self.tube.mid()
+
 
 def fixpoint(contract, *x):
   vol = -1.0
@@ -559,24 +569,26 @@ def fixpoint(contract, *x):
   while vol != prev_vol:
 
     prev_vol = vol
-    if isinstance(x, tuple):
+    if type(x) is tuple:
       x = contract(*x)
     else: # prevent from unpacking
       x = contract(x)
 
-    if not isinstance(x,tuple):
-      vol = x.get_item_0(0).volume()
-    else:
-      vol = 0.0
-      for xi in x:
-        if xi.is_empty():
-          return x
-        w = xi.volume()
-        # As infinity is absorbent, this would not
-        # allow us to identify a contraction, so we
-        # exclude these cases:
-        if w != oo:
-          vol += w
+    # For computing the volume:
+    # only a real Python tuple is considered as a collection of objects.
+    # Otherwise, any iterable object (tube, boxes, etc.) will be divided in the for loop.
+    items = x if type(x) is tuple else (x,)
+
+    vol = 0.0
+    for xi in items:
+      if xi.is_empty():
+        return x
+      w = xi.volume()
+      # As infinity is absorbent, this would not
+      # allow us to identify a contraction, so we
+      # exclude these cases:
+      if w != oo:
+        vol += w
 
   return x
 

python/src/core/CMakeLists.txt

@@ -124,6 +124,7 @@
     trajectory/codac2_py_AnalyticTraj.cpp
     trajectory/codac2_py_SampledTraj.cpp
     trajectory/codac2_py_TrajBase.h
+    trajectory/codac2_py_traj_cart_prod.cpp
   )
 
   target_include_directories(_core

python/src/core/codac2_py_core.cpp

@@ -158,6 +158,7 @@ void export_trunc(py::module& m);
 // trajectory
 void export_AnalyticTraj(py::module& m);
 void export_SampledTraj(py::module& m);
+void export_traj_cart_prod(py::module& m);
 
 // Extension > sympy
 void export_sympy(py::module& m);
@@ -327,6 +328,7 @@ PYBIND11_MODULE(_core, m)
   // trajectory
   export_AnalyticTraj(m);
   export_SampledTraj(m);
+  export_traj_cart_prod(m);
 
   m.def("srand", []()
     {

python/src/core/domains/tube/codac2_py_SlicedTube.h

@@ -25,8 +25,8 @@ using namespace pybind11::literals;
 template<typename T>
 void export_SlicedTube(py::module& m, const std::string& name)
 {
-  py::class_<SlicedTube<T>,TubeBase> exported_slicedtubebase_class(m, name.c_str(), SLICEDTUBEBASE_MAIN);
-  exported_slicedtubebase_class
+  py::class_<SlicedTube<T>,TubeBase> exported_slicedtube_class(m, name.c_str(), SLICEDTUBEBASE_MAIN);
+  exported_slicedtube_class
 
     .def(py::init<const std::shared_ptr<TDomain>&,const T&>(),
       SLICEDTUBE_T_SLICEDTUBE_CONST_SHARED_PTR_TDOMAIN_REF_CONST_T_REF,
@@ -144,7 +144,7 @@ void export_SlicedTube(py::module& m, const std::string& name)
 
   if constexpr(std::is_same_v<T,IntervalVector>)
   {
-    exported_slicedtubebase_class
+    exported_slicedtube_class
 
     .def("inflate", (const SlicedTube<T>& (SlicedTube<T>::*)(const Vector&)) &SlicedTube<T>::inflate,
       CONST_SLICEDTUBE_T_REF_SLICEDTUBE_T_INFLATE_CONST_V_REF,
@@ -156,7 +156,7 @@ void export_SlicedTube(py::module& m, const std::string& name)
     ;
   }
 
-  exported_slicedtubebase_class
+  exported_slicedtube_class
 
     .def(py::self == py::self,
       BOOL_SLICEDTUBE_T_OPERATOREQ_CONST_SLICEDTUBE_REF_CONST,
@@ -193,7 +193,7 @@ void export_SlicedTube(py::module& m, const std::string& name)
 
   if constexpr(std::is_same_v<T,Interval> || std::is_same_v<T,IntervalVector>)
   {
-    exported_slicedtubebase_class
+    exported_slicedtube_class
 
       .def("__call__", [](const SlicedTube<T>& x, const Interval& t, const py::object& v)
           {
@@ -251,7 +251,7 @@ void export_SlicedTube(py::module& m, const std::string& name)
 
   if constexpr(std::is_same_v<T,IntervalVector>)
   {
-    exported_slicedtubebase_class
+    exported_slicedtube_class
 
       .def(
           #if FOR_MATLAB
@@ -296,4 +296,25 @@ void export_SlicedTube(py::module& m, const std::string& name)
         "i"_a, "j"_a)
     ;
   }
+
+  if constexpr(std::is_same_v<T,Interval>)
+  {
+    exported_slicedtube_class
+      .def("mid", &SlicedTube<Interval>::mid<double>,
+        AUTO_SLICEDTUBE_T_MID_CONST);
+  }
+
+  if constexpr(std::is_same_v<T,IntervalVector>)
+  {
+    exported_slicedtube_class
+      .def("mid", &SlicedTube<IntervalVector>::mid<Vector>,
+        AUTO_SLICEDTUBE_T_MID_CONST);
+  }
+
+  if constexpr(std::is_same_v<T,IntervalMatrix>)
+  {
+    exported_slicedtube_class
+      .def("mid", &SlicedTube<IntervalMatrix>::mid<Matrix>,
+        AUTO_SLICEDTUBE_T_MID_CONST);
+  }
 }

python/src/core/functions/analytic/codac2_py_AnalyticFunction.h

@@ -337,16 +337,65 @@ void export_AnalyticFunction(py::module& m, const std::string& export_name)
   {
     exported
 
+      // Mixed input types are not supported yet
+
+      // All scalar inputs
+
       .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1) {
+            if(!is_instance<SlicedTube<Interval>>(x1)) {
+              assert_release("tube_eval: invalid tube type");
+            }
+            return f.tube_eval(cast<SlicedTube<Interval>>(x1));
+          },
+        AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
+        "x1"_a)
 
-            if(!is_instance<SlicedTube<typename T::Domain>>(x1)) {
+      .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1, const py::object& x2) {
+            if(!is_instance<SlicedTube<Interval>>(x1) && !is_instance<SlicedTube<Interval>>(x2)) {
               assert_release("tube_eval: invalid tube type");
             }
+            return f.tube_eval(cast<SlicedTube<Interval>>(x1),cast<SlicedTube<Interval>>(x2));
+          },
+        AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
+        "x1"_a, "x2"_a)
+
+      .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1, const py::object& x2, const py::object& x3) {
+            if(!is_instance<SlicedTube<Interval>>(x1) && !is_instance<SlicedTube<Interval>>(x2) && !is_instance<SlicedTube<Interval>>(x3)) {
+              assert_release("tube_eval: invalid tube type");
+            }
+            return f.tube_eval(cast<SlicedTube<Interval>>(x1),cast<SlicedTube<Interval>>(x2),cast<SlicedTube<Interval>>(x3));
+          },
+        AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
+        "x1"_a, "x2"_a, "x3"_a)
 
-            return f.tube_eval(cast<SlicedTube<typename T::Domain>>(x1));
+      // All vector inputs
+
+      .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1) {
+            if(!is_instance<SlicedTube<IntervalVector>>(x1)) {
+              assert_release("tube_eval: invalid tube type");
+            }
+            return f.tube_eval(cast<SlicedTube<IntervalVector>>(x1));
           },
         AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
         "x1"_a)
+
+      .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1, const py::object& x2) {
+            if(!is_instance<SlicedTube<IntervalVector>>(x1) && !is_instance<SlicedTube<IntervalVector>>(x2)) {
+              assert_release("tube_eval: invalid tube type");
+            }
+            return f.tube_eval(cast<SlicedTube<IntervalVector>>(x1),cast<SlicedTube<IntervalVector>>(x2));
+          },
+        AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
+        "x1"_a, "x2"_a)
+
+      .def("tube_eval", [](const AnalyticFunction<T>& f, const py::object& x1, const py::object& x2, const py::object& x3) {
+            if(!is_instance<SlicedTube<IntervalVector>>(x1) && !is_instance<SlicedTube<IntervalVector>>(x2) && !is_instance<SlicedTube<IntervalVector>>(x3)) {
+              assert_release("tube_eval: invalid tube type");
+            }
+            return f.tube_eval(cast<SlicedTube<IntervalVector>>(x1),cast<SlicedTube<IntervalVector>>(x2),cast<SlicedTube<IntervalVector>>(x3));
+          },
+        AUTO_ANALYTICFUNCTION_T_TUBE_EVAL_CONST_SLICEDTUBE_ARGS_REF_VARIADIC_CONST,
+        "x1"_a, "x2"_a, "x3"_a)
     ;
   }
 

python/src/core/operators/codac2_py_operators.cpp

@@ -620,6 +620,10 @@ void export_operators(py::module& m)
       "y"_a, "x1"_a, "p"_a)
   ;
 
+  m.def("mod", [](const ScalarExpr& e1, const ScalarExpr& p) { return mod(e1,p); },
+    SCALAREXPR_MOD_CONST_SCALAREXPR_REF_CONST_SCALAREXPR_REF,
+    "x1"_a, "p"_a);
+
   py::class_<PowOp>(m, "PowOp")
     .def(py::init<>()) // for using static methods in Matlab
     .def_static("fwd", (Interval(*)(const Interval&,const Interval&)) &PowOp::fwd,

python/src/core/trajectory/codac2_py_SampledTraj.cpp

@@ -205,7 +205,7 @@ void add_operators(py::class_<SampledTraj<T>>& pyclass)
       SAMPLEDTRAJ_T_OPERATORPLUS_CONST_SAMPLEDTRAJ_T_REF_CONST_Q_REF,
       py::is_operator())
 
-    .def("__add__", [](const T& x1, const SampledTraj<T>& x2) { return x1+x2; },
+    .def("__radd__", [](const SampledTraj<T>& x2, const T& x1) { return x1+x2; },
       SAMPLEDTRAJ_T_OPERATORPLUS_CONST_Q_REF_CONST_SAMPLEDTRAJ_T_REF,
       py::is_operator())
 
@@ -221,22 +221,22 @@ void add_operators(py::class_<SampledTraj<T>>& pyclass)
       SAMPLEDTRAJ_T_OPERATORMINUS_CONST_SAMPLEDTRAJ_T_REF_CONST_Q_REF,
       py::is_operator())
 
-    .def("__sub__", [](const T& x1, const SampledTraj<T>& x2) { return x1-x2; },
+    .def("__rsub__", [](const SampledTraj<T>& x2, const T& x1) { return x1-x2; },
       SAMPLEDTRAJ_T_OPERATORMINUS_CONST_Q_REF_CONST_SAMPLEDTRAJ_T_REF,
       py::is_operator())
 
     .def("__mul__", [](const SampledTraj<T>& x1, const SampledTraj<T>& x2) { return x1*x2; },
       SAMPLEDTRAJ_T_OPERATORMUL_CONST_SAMPLEDTRAJ_T_REF_CONST_SAMPLEDTRAJ_T_REF,
       py::is_operator())
 
-    .def("__mul__", [](double x1, const SampledTraj<T>& x2) { return x1*x2; },
-      SAMPLEDTRAJ_T_OPERATORMUL_DOUBLE_CONST_SAMPLEDTRAJ_T_REF,
-      py::is_operator())
-
     .def("__mul__", [](const SampledTraj<T>& x1, double x2) { return x1*x2; },
       SAMPLEDTRAJ_T_OPERATORMUL_CONST_SAMPLEDTRAJ_T_REF_DOUBLE,
       py::is_operator())
 
+    .def("__rmul__", [](const SampledTraj<T>& x2, double x1) { return x1*x2; },
+      SAMPLEDTRAJ_T_OPERATORMUL_DOUBLE_CONST_SAMPLEDTRAJ_T_REF,
+      py::is_operator())
+
     .def("__mul__", [](const T& x1, const SampledTraj<T>& x2) { return x1*x2; },
       SAMPLEDTRAJ_T_OPERATORMUL_CONST_Q_REF_CONST_SAMPLEDTRAJ_T_REF,
       py::is_operator())
@@ -249,9 +249,9 @@ void add_operators(py::class_<SampledTraj<T>>& pyclass)
 
 void export_SampledTraj(py::module& m)
 {
-  auto py_SampledTraj_double = _export_SampledTraj<double>(m, "SampledScalarTraj");
-  auto py_SampledTraj_Vector = _export_SampledTraj<Vector>(m, "SampledVectorTraj");
-  auto py_SampledTraj_Matrix = _export_SampledTraj<Matrix>(m, "SampledMatrixTraj");
+  auto py_SampledTraj_double = _export_SampledTraj<double>(m, "SampledTraj_Scalar");
+  auto py_SampledTraj_Vector = _export_SampledTraj<Vector>(m, "SampledTraj_Vector");
+  auto py_SampledTraj_Matrix = _export_SampledTraj<Matrix>(m, "SampledTraj_Matrix");
 
   add_operators<double>(py_SampledTraj_double);