[rob] added Redermor example + data classes + traj/tube updates

examples/cpp/09_redermor_trajectory/CMakeLists.txt

@@ -0,0 +1,28 @@
+# ==================================================================
+#  tubex-lib - cmake configuration file
+# ==================================================================
+
+
+# Sources
+
+  set(SRC
+    main.cpp
+  )
+
+  set(HEADERS
+
+  )
+
+  add_executable(ex_redermor_traj ${SRC})
+
+  target_link_libraries(ex_redermor_traj
+    ${LIBS}
+    tubex-core
+    tubex-robotics)
+
+
+# Tests
+
+  #if(BUILD_TESTS)
+  #  add_test(NAME ex_redermor_traj COMMAND ex_redermor_traj 0)
+  #endif(BUILD_TESTS)

examples/cpp/09_redermor_trajectory/main.cpp

@@ -0,0 +1,25 @@
+#include "ibex.h"
+#include "tubex.h"
+#include "tubex-robotics.h"
+
+using namespace std;
+using namespace ibex;
+using namespace tubex;
+
+int main()
+{
+  TubeVector *x;
+  TrajectoryVector *x_truth;
+  DataLoader_Redermor data_loader("./data/redermor/gesmi.txt");
+  data_loader.load_data(x, x_truth);
+
+  vibes::beginDrawing();
+  VibesFigure_Tube fig("Redermor", x, x_truth);
+  fig.set_properties(1550, 50, 600, 300);
+  fig.show();
+  vibes::endDrawing();
+
+  delete x;
+  delete x_truth;
+  return EXIT_SUCCESS;
+}

examples/cpp/CMakeLists.txt

@@ -3,15 +3,16 @@
 # ==================================================================
 
 
-add_subdirectory(01_arithmetic)
+#add_subdirectory(01_arithmetic)
 add_subdirectory(08_solver_01_picard)
-add_subdirectory(08_solver_02_xmsin_fwd)
-add_subdirectory(08_solver_03_xmsin_bwd)
+#add_subdirectory(08_solver_02_xmsin_fwd)
+#add_subdirectory(08_solver_03_xmsin_bwd)
 add_subdirectory(08_solver_04_bvp)
-add_subdirectory(08_solver_05_delay)
+#add_subdirectory(08_solver_05_delay)
 add_subdirectory(08_solver_06_bvp_delay)
-add_subdirectory(08_solver_07_path_planning)
-add_subdirectory(08_solver_08_integro_diff)
+#add_subdirectory(08_solver_07_path_planning)
+#add_subdirectory(08_solver_08_integro_diff)
+add_subdirectory(09_redermor_trajectory)
 
 #add_subdirectory(02_arithmetic_ctc)
 #add_subdirectory(03_integ_msinx)

src/core/dynamics/tubex_TrajectoryVector.cpp

@@ -215,11 +215,18 @@ namespace tubex
 
   const Vector& TrajectoryVector::set(double t, const Vector& y)
   {
-    if(!not_defined()) DimensionException::check(*this, y);
+    if(not_defined())
+      m_codomain = IntervalVector(y);
+
+    else
+    {
+      DimensionException::check(*this, y);
+      m_codomain |= y;
+    }
+
     m_map_values.erase(t);
     m_map_values.emplace(t, y);
     m_domain |= t;
-    m_codomain |= y;
     return m_map_values.at(t);
   }
 
@@ -262,7 +269,7 @@ namespace tubex
     if(x.m_function != NULL)
       str << " (Fnc object)";
 
-    else if(!x.m_map_values.empty())
+    else if(!x.m_map_values.empty() && x.m_map_values.size() < 10)
     {
       str << ", " << x.m_map_values.size() << " pts: { ";
       for(map<double,Vector>::const_iterator it = x.m_map_values.begin() ; it != x.m_map_values.end() ; it++)

src/core/dynamics/tubex_TubeSlice.cpp

@@ -434,6 +434,13 @@ namespace tubex
 
   // Protected methods
 
+    void TubeSlice::resize(int n)
+    {
+      m_codomain.resize(n);
+      m_input_gate->resize(n);
+      m_output_gate->resize(n);
+    }
+
     void TubeSlice::set_tube_ref(TubeVector *tube_ref)
     {
       m_tube_ref = tube_ref;

src/core/dynamics/tubex_TubeSlice.h

@@ -98,6 +98,7 @@ namespace tubex
 
     protected:
 
+      void resize(int n);
       void set_tube_ref(TubeVector *tube_ref);
       void set_domain(const ibex::Interval& domain);
       const ibex::IntervalVector codomain_box() const;

src/core/dynamics/tubex_TubeVector.cpp

@@ -115,7 +115,7 @@ namespace tubex
     TubeVector::TubeVector(const TubeVector& x, const IntervalVector& codomain)
       : TubeVector(x)
     {
-      DimensionException::check(x, codomain);
+      resize(codomain.size());
       set(codomain);
     }
 
@@ -215,6 +215,16 @@ namespace tubex
     {
       return m_v_slices[0]->dim();
     }
+
+    void TubeVector::resize(int n)
+    {
+      TubeSlice *slice = get_first_slice();
+      while(slice != NULL)
+      {
+        slice->resize(n);
+        slice = slice->next_slice();
+      }
+    }
 
     // Slices structure
 
@@ -790,6 +800,33 @@ namespace tubex
       return *this;
     }
 
+    const TubeVector& TubeVector::inflate(const TrajectoryVector& rad)
+    {
+      DimensionException::check(*this, rad);
+      DomainException::check(*this, rad);
+      TubeSlice *slice = get_first_slice();
+      TubeSlice *first_slice = slice;
+
+      // Setting envelopes before gates' inflation
+      while(slice != NULL)
+      {
+        slice->set_envelope(slice->codomain() + Interval(-1.,1.) * rad[slice->domain()]);
+        slice = slice->next_slice();
+      }
+
+      slice = first_slice;
+
+      while(slice != NULL)
+      {
+        if(slice == first_slice)
+          slice->set_input_gate(slice->input_gate() + Interval(-1.,1.) * rad[slice->domain().lb()]);
+        slice->set_output_gate(slice->output_gate() + Interval(-1.,1.) * rad[slice->domain().ub()]);
+        slice = slice->next_slice();
+      }
+
+      return *this;
+    }
+
     // Bisection
 
     const pair<TubeVector,TubeVector> TubeVector::bisect(double t, float ratio) const

src/core/dynamics/tubex_TubeVector.h

@@ -52,6 +52,7 @@ namespace tubex
       const TubeVector& operator=(const TubeVector& x);
       const ibex::Interval domain() const;
       int dim() const;
+      void resize(int n);
 
       // Slices structure
       int nb_slices() const;
@@ -104,6 +105,7 @@ namespace tubex
       void set(const ibex::IntervalVector& y, const ibex::Interval& t);
       void set_empty();
       const TubeVector& inflate(double rad);
+      const TubeVector& inflate(const TrajectoryVector& rad);
 
       // Bisection
       const std::pair<TubeVector,TubeVector> bisect(double t, float ratio = 0.55) const;

src/core/graphics/tubex_VibesFigure_Tube.cpp

@@ -376,7 +376,7 @@ namespace tubex
     const Polygon VibesFigure_Tube::polygon_envelope(const TubeVector *tube) const
     {
       if(tube->is_empty())
-        cout << "Tube graphics: warning, tube " << m_map_tubes.at(tube).name << " is empty" << endl;
+        cout << "Tube graphics: warning, empty tube" << endl;
 
       vector<Point> v_pts;
 

src/robotics/data/tubex_DataLoader.h

@@ -16,6 +16,7 @@
 #include <string>
 #include <fstream>
 #include "tubex_TubeVector.h"
+#include "tubex_TrajectoryVector.h"
 
 namespace tubex
 {
@@ -26,7 +27,9 @@ namespace tubex
       DataLoader(const std::string& file_path);
       ~DataLoader();
 
-      virtual bool loadData(TubeVector& x, const ibex::Interval& domain = ibex::Interval::ALL_REALS) = 0;
+      virtual void load_data(TubeVector *&x,
+                             TrajectoryVector *&truth,
+                             const ibex::Interval& domain = ibex::Interval::ALL_REALS) = 0;
 
     protected:
 

src/robotics/data/tubex_DataLoader_Redermor.cpp

@@ -10,7 +10,9 @@
  *  Created   : 2018
  * ---------------------------------------------------------------------------- */
 
+#include "tubex_Exception.h"
 #include "tubex_DataLoader_Redermor.h"
+#include "tubex_Function.h"
 
 using namespace std;
 using namespace ibex;
@@ -23,8 +25,58 @@ namespace tubex
 
   }
 
-  bool DataLoader_Redermor::loadData(TubeVector& x, const Interval& domain)
+  void DataLoader_Redermor::load_data(TubeVector *&x, TrajectoryVector *&truth, const Interval& domain)
   {
-
+    // todo: truncate domain
+    if(!m_datafile->is_open())
+      throw Exception("DataLoader_Redermor::load_data", "data file not already opened");
+
+    int i = 0;
+    string line;
+    TrajectoryVector traj_data_x, traj_data_dx;
+    truth = new TrajectoryVector();
+
+    while(getline(*m_datafile, line))
+    {
+      i ++;
+      if(i < 46) continue; // accessing data
+      if(i == 60000) break; // end of data
+
+      double t;
+      Vector y(10), dy(10);
+
+      istringstream iss(line);
+      if(!(iss >> t
+               >> y[0] >> dy[0]   // phi
+               >> y[1] >> dy[1]   // theta
+               >> y[2] >> dy[2]   // psi
+               >> y[3] >> dy[3]   // vx
+               >> y[4] >> dy[4]   // vy
+               >> y[5] >> dy[5]   // vz
+               >> y[6] >> dy[6]   // depth
+               >> y[7] >> dy[7]   // alt
+               >> y[8] >> dy[8]   // x
+               >> y[9] >> dy[9])) // y
+        throw Exception("DataLoader_Redermor::load_data", "fail loading data");
+
+      traj_data_x.set(t, y.subvector(0,5));
+      traj_data_dx.set(t, dy.subvector(0,5));
+      truth->set(t, y.subvector(8,9));
+    }
+
+    // Data from sensors with uncertainties
+    TubeVector data_x(traj_data_x, 1.);
+    data_x.inflate(traj_data_dx);
+
+    // Computing robot's velocities
+    tubex::Function f("phi", "theta", "psi", "vxr", "vyr", "vzr", 
+                      "(vxr * cos(theta) * cos(psi) \
+                        - vyr * (cos(phi) * sin(psi) - sin(theta) * cos(psi) * sin(phi)) \
+                        + vzr * (sin(phi) * sin(psi) + sin(theta) * cos(psi) * cos(phi)) ; \
+                        vxr * cos(theta) * sin(psi) \
+                        + vyr * (cos(psi) * cos(phi) + sin(theta) * sin(psi) * sin(phi)) \
+                        - vzr * (cos(psi) * sin(phi) - sin(theta) * cos(phi) * sin(psi)))");
+    TubeVector tt = f.eval(data_x).primitive();
+    x = new TubeVector(tt);
   }
 }

src/robotics/data/tubex_DataLoader_Redermor.h

@@ -23,7 +23,9 @@ namespace tubex
     public:
 
       DataLoader_Redermor(const std::string& file_path);
-      bool loadData(TubeVector& x, const ibex::Interval& domain = ibex::Interval::ALL_REALS);
+      void load_data(TubeVector *&x,
+                     TrajectoryVector *&truth,
+                     const ibex::Interval& domain = ibex::Interval::ALL_REALS);
 
     protected: