Feature/backup lqoc man ex

README.md

@@ -38,7 +38,7 @@ The CT was designed with the following features in mind:
  	- solve large scale optimal control problems in MPC fashion. 
 
  - **Robot Modelling, Rigid Body Kinematics and Dynamics**: 
-    - straight-forward interface to the state-of the art rigid body dynamics modelling tool RobCoGen.
+    - straight-forward interface to the state-of the art rigid body dynamics modelling tool [RobCoGen][robcogen-url].
 	- implementation of a basic nonlinear-programming inverse kinematics solver for fix-base robots.
 
  - **Automatic Differentiation**:
@@ -196,9 +196,10 @@ The four different main modules are detailed in the following.
  - Operational Space Controllers
  - Basic soft **auto-differentiable contact model** for arbitrary frames (ct::rbd::EEContactModel)
  - **Actuator dynamics** (ct::rbd::ActuatorDynamics)
- - Backend uses <a href="https://bitbucket.org/mfrigerio17/roboticscodegenerator/" target="_blank">RobCoGen</a> \cite frigerioCodeGen, 
+ - Backend uses [RobCoGen][robcogen-url], 
  a highly efficient Rigid Body Dynamics library
 
+[robcogen-url]: https://robcogenteam.bitbucket.io/
 
 ### ct_models
 

ct/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.14.7)
+cmake_minimum_required(VERSION 3.5)
 project(ct VERSION 3.0.2 LANGUAGES CXX)
 
 #Make sure metapackage does not fail when building documentation

ct_core/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
@@ -72,17 +72,30 @@ else()
     message(STATUS "COMPILING WITHOUT QWT")
 endif()
 
-find_package(Python COMPONENTS Development NumPy QUIET)
-if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3)
-   message(STATUS "Found python " ${Python_VERSION})
-   list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
-   message(STATUS "Python library path ... " ${Python_LIBRARIES})
-   list(APPEND ct_core_LIBS ${Python_LIBRARIES})
+if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+    find_package(PythonLibs 3 QUIET)
+    if (PYTHONLIBS_FOUND)
+        message(STATUS "Found python 3")
+        list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
+        set(PLOTTING_ENABLED ON)
+        message(STATUS "Python library path ... " ${PYTHON_LIBRARY})
+        list(APPEND ct_core_LIBS ${PYTHON_LIBRARY})
+    else()
+        message(STATUS "Python 3 not found, plotting will not be available")
+    endif()
 else()
-   message(STATUS "Python 3 not found, plotting will not be available")
+    find_package(Python COMPONENTS Development NumPy QUIET)
+    if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3)
+        message(STATUS "Found python " ${Python_VERSION})
+        list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
+        set(PLOTTING_ENABLED ON)
+        message(STATUS "Python library path ... " ${Python_LIBRARIES})
+        list(APPEND ct_core_LIBS ${Python_LIBRARIES})
+    else()
+        message(STATUS "Python 3 not found, plotting will not be available")
+    endif()
 endif()
 
-
 ## configure files required for code-generation
 set(CODEGEN_TEMPLATE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/templates")
 set(CODEGEN_OUTPUT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/generated")
@@ -95,8 +108,12 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ct/core/templateDir.h.in ${CM
 
 ## define the directories to be included in all ct_core targets
 list(APPEND ct_core_target_include_dirs ${EIGEN3_INCLUDE_DIR})
-list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS})
-list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS})
+if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+    list(APPEND ct_core_target_include_dirs ${PYTHON_INCLUDE_DIRS})
+else()
+    list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS})
+    list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS})
+endif()
 list(APPEND ct_core_target_include_dirs ${QWT_INCLUDE_DIR})
 list(APPEND ct_core_target_include_dirs $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
 list(APPEND ct_core_target_include_dirs $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/examples/include>)
@@ -140,7 +157,7 @@ target_link_libraries(ct_core INTERFACE
     dl # required for gcc compatibility
     )
 
-
+    
 ##################
 # BUILD EXAMPLES #
 ##################
@@ -208,4 +225,3 @@ endif()
 #################
 add_subdirectory(doc)
 
-

ct_core/examples/CMakeLists.txt

@@ -10,9 +10,11 @@ add_executable(ex_MasspointIntegration src/MasspointIntegration.cpp)
 target_link_libraries(ex_MasspointIntegration ct_core)
 list(APPEND core_ex_TARGETS ex_MasspointIntegration)
 
-add_executable(plotTest src/plot/plotTest.cpp)
-target_link_libraries(plotTest ct_core)
-list(APPEND core_ex_TARGETS plotTest)
+if(PLOTTING_ENABLED)
+    add_executable(plotTest src/plot/plotTest.cpp)
+    target_link_libraries(plotTest ct_core)
+    list(APPEND core_ex_TARGETS plotTest)
+endif()
 
 if(QWT_ENABLED)
     add_executable(plotQwtTest src/plot/plotQwtTest.cpp)
@@ -25,4 +27,4 @@ include(GNUInstallDirs)
 install(
     TARGETS ${core_ex_TARGETS}
     RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR}/ct_core
-    )
+    )

ct_core/include/ct/core/Common

@@ -15,5 +15,6 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 #include "common/linspace.h"
 #include "common/activations/Activations.h"
 #include "common/EigenIsNan.h"
+#include "common/EigenFileExport.h"
 
 

ct_core/include/ct/core/common/EigenFileExport.h

@@ -0,0 +1,132 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
+Licensed under the BSD-2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <string>
+#include <fstream>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace ct::core {
+
+//
+// @brief Save Eigen-types or vectors of Eigen-types to text file.
+// Currently ordinary Eigen matrices and quaternion types are supported.
+//
+// Usage examples:
+// // writing an eigen matrix
+// Eigen::Matrix3d m = Eigen::Matrix3d::Random();
+// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat.csv", m, "test mat");
+//
+// // writing a vector of eigen matrices
+// std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> m_vec(3, Eigen::Matrix3d::Random());
+// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat_vec.csv", m_vec, "some title.");
+//
+// // writing a quaternion
+// Eigen::Quaterniond q(Eigen::Quaterniond::Identity());
+// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat.csv", q, "test quat");
+//
+// // writing a vector of quaternions
+// std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> q_vec(
+//     5, Eigen::Quaterniond::Identity());
+// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat_vec.csv", q_vec);
+//
+class EigenFileExport
+{
+public:
+    // Create CSV formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CSVFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", "\n");
+    }
+    // Create Octave formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat OctaveFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", ";\n", "", "", "[", "]");
+    }
+    // Create comma init formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CommaInitFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";");
+    }
+    // Create a clean formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CleanFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", "\n", "[", "]");
+    }
+    // create default full-info formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat HeavyFormat(int precision = Eigen::FullPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", ";\n", "[", "]", "[", "]");
+    }
+
+    //! Save a single matrix to single text file.
+    template <typename M>
+    static void mat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const Eigen::MatrixBase<M>& m,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+        if (!heading.empty())
+            file << heading << "\n";
+        file << m.format(fmt);
+
+        file.close();
+    }
+
+    //! Save a single quaternion to a single text file.
+    template <typename M>
+    static void quat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const Eigen::QuaternionBase<M>& q,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+        if (!heading.empty())
+            file << heading << "\n";
+        file << q.coeffs().transpose().format(fmt);
+
+        file.close();
+    }
+
+    // Save a vector of matrices to a single text file.
+    template <typename M, typename Alloc = Eigen::aligned_allocator<M>>
+    static void mat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const std::vector<M, Alloc>& m_vec,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+
+        if (!heading.empty())
+            file << heading << "\n";
+
+        for (size_t i = 0; i < m_vec.size(); i++)
+            file << m_vec[i].format(fmt) << "\n";
+
+        file.close();
+    }
+
+    // Save a vector of quaternions to a single text file.
+    template <typename Q, typename Alloc = Eigen::aligned_allocator<Eigen::Quaternion<Q>>>
+    static void quat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const std::vector<Eigen::Quaternion<Q>, Alloc>& q_vec,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+
+        if (!heading.empty())
+            file << heading << "\n";
+
+        for (size_t i = 0; i < q_vec.size(); i++)
+            file << q_vec[i].coeffs().transpose().format(fmt) << "\n";
+
+        file.close();
+    }
+};
+
+}  // namespace ct::core