full upload

CMakeLists.txt

+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
+INCLUDE(cmake/python.cmake)
+INCLUDE(cmake/ide.cmake)
+INCLUDE(cmake/apple.cmake)
+
+SET(PROJECT_NAME timeopt)
+SET(PROJECT_DESCRIPTION "COM Generator")
+SET(PROJECT_URL "http://github.com/ggory15")
+OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF)
+
+SET(DOXYGEN_USE_MATHJAX YES)
+
+# Handle APPLE Cmake policy
+IF(APPLE)
+  APPLY_DEFAULT_APPLE_CONFIGURATION()
+ENDIF(APPLE)
+
+# Disable -Werror on Unix for now.
+SET(CXX_DISABLE_WERROR True)
+SET(CMAKE_VERBOSE_MAKEFILE True)
+
+SETUP_PROJECT()
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -g -Wall -Wwrite-strings -pedantic -O3 -funroll-loops -fPIC")
+
+IF(WIN32)
+  SET(LINK copy_if_different)
+ELSE(WIN32)
+  SET(LINK create_symlink)
+ENDIF(WIN32)
+
+# --- OPTIONS ----------------------------------------
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
+OPTION (BUILD_UNIT_TESTS "Build the unitary tests" ON)
+OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+OPTION (EIGEN_RUNTIME_NO_MALLOC "If ON, it can assert in case of runtime allocation" ON)
+OPTION (EIGEN_NO_AUTOMATIC_RESIZING "If ON, it forbids automatic resizing of dynamics arrays and matrices" OFF)
+
+IF(INITIALIZE_WITH_NAN)
+  MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
+  ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
+ENDIF(INITIALIZE_WITH_NAN)
+
+IF(EIGEN_RUNTIME_NO_MALLOC)
+  MESSAGE(STATUS "Option EIGEN_RUNTIME_NO_MALLOC on.")
+  ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC)
+ENDIF(EIGEN_RUNTIME_NO_MALLOC)
+
+IF(EIGEN_NO_AUTOMATIC_RESIZING)
+  MESSAGE(STATUS "Option EIGEN_NO_AUTOMATIC_RESIZING on.")
+  ADD_DEFINITIONS(-DEIGEN_NO_AUTOMATIC_RESIZING)
+ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
+
+# ----------------------------------------------------
+# --- DEPENDANCIES -----------------------------------
+# ----------------------------------------------------
+ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.2.0") # Eigen::Ref appeared from 3.2.0
+ADD_REQUIRED_DEPENDENCY("momentumopt")
+ADD_REQUIRED_DEPENDENCY("yaml-cpp")
+
+
+SET(BOOST_REQUIERED_COMPONENTS filesystem system)
+SET(BOOST_BUILD_COMPONENTS unit_test_framework)
+SET(BOOST_OPTIONAL_COMPONENTS "")
+
+IF(BUILD_PYTHON_INTERFACE)
+  SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
+  FINDPYTHON()
+  INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})
+SEARCH_FOR_BOOST()
+
+# Path to boost headers
+INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
+# ----------------------------------------------------
+# --- INCLUDE ----------------------------------------
+# ----------------------------------------------------
+
+SET(${PROJECT_NAME}_TIMEOPT_HEADERS
+    include/timeopt/fwd.hpp
+    include/timeopt/phase.hpp
+    include/timeopt/problem.hpp
+
+    include/timeopt/interface/contactplanner.hpp
+  )
+SET(${PROJECT_NAME}_SERIALIZATION_HEADERS
+  include/serialization/eigen-matrix.hpp
+  include/serialization/spatial.hpp
+  include/serialization/fwd.hpp
+  include/serialization/xml.hpp
+  include/serialization/archive.hpp
+)
+
+
+SET(HEADERS
+    ${${PROJECT_NAME}_SERIALIZATION_HEADERS}
+    ${${PROJECT_NAME}_TIMEOPT_HEADERS}
+ )
+
+LIST(REMOVE_DUPLICATES HEADERS)
+
+SET(HEADERS_FULL_PATH "")
+FOREACH(header ${HEADERS})
+  LIST(APPEND HEADERS_FULL_PATH "${CMAKE_SOURCE_DIR}/${header}")
+  GET_FILENAME_COMPONENT(headerName ${header} NAME)
+  GET_FILENAME_COMPONENT(headerPath ${header} PATH)
+  INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/${header}
+    DESTINATION ${CMAKE_INSTALL_PREFIX}/${headerPath}
+          PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+ENDFOREACH(header ${HEADERS})
+
+# --- MAIN LIBRARY -------------------------------------------------------------
+ADD_SUBDIRECTORY(src)
+
+#--- BINDINGS ----------------------------------------------------------------
+IF(BUILD_PYTHON_INTERFACE)
+  SET(PYWRAP ${PROJECT_NAME}_pywrap)
+  ADD_SUBDIRECTORY(bindings)
+ENDIF(BUILD_PYTHON_INTERFACE)
+# --- UNIT TESTS ---------------------------------------------------------------
+ADD_SUBDIRECTORY(unittest)
+
+# --- PACKAGING ----------------------------------------------------------------
+PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
+
+SETUP_PROJECT_FINALIZE()

README.md

-# HPP-TIMEOPT-PYTHON BINDING
+# hpp-timeopt
+
+This is a modified version of the software for optimization of centroidal momentum dyanmics (https://git-amd.tuebingen.mpg.de/bponton/timeoptimization) in order to use in HPP (https://github.com/humanoid-path-planner). This module can generate COM trajectory for humanoid robot using time-optimization by pondon's works.
+
+Dependencies
+----
+The present software depends on several packages which have to be available in Ubuntu 14.04 / 16.04
+
+-Libraries
+  - Eigen3
+  - YAML_cpp (sudo apt-get install libyaml-cpp-dev)
+
+-System tools
+   - CMake (>=2.6)
+   - pkg-config
+   - usual compilation tools (GCC/G++, make, etc.)
+
+
+Install
+----
+To install this moulde: 
+
+  1. Install HPP 
+	- see https://github.com/humanoid-path-planner/hpp-doc
+	
+  2. Install YAML-cpp
+   ```bash
+   sudo apt-get install libyaml-cpp-dev
+   ```
+  3. Use CMake to install the library. For instance:
+   ```bash
+   mkdir $hpp-timeoptimization/build
+   cd $hpp-timeoptimization/build
+   cd cmake ..
+   make install
+   ```
+  4. (Optional) If you want to use this module in Python
+	- see https://github.com/ggory15/hpp-timeopt-corba
+  
+Inputs & Outputs of Algorithm
+----
+   1. Inputs
+   	robot's mass, robot's initial com, desired contact sequences, (ref) linear and angular momentum (mainly all zeros)
+	robot's desired com at final state, etc.
+   2. Outpus
+   	time sequence(especially, timeoptimization mode) 
+	robot's continous com, linear & angular momentum trajectories, etc. 
+
+Demo
+----
+The demo is based on DYROS-RED URDFs. The URDF files can be obtained by https://github.com/ggory15/dyrosred.
+
+Also you could use your robot's URDF file, if you rewrite some lines. 
+
+To run the demo script: 
+
+  1. Running the demo code. For example,
+  ```bash
+  cd build/demos/
+  ./demo_momentumopt -i <name_of_cfg_file_within_config_folder>
+  ```
+  
+  2. Dispaly the result of the demo code
+  ```bash 
+  python <path_to_momentumopt>/scripts/display.py -i <path_to_momentumopt>/config/<name_of_cfg_file_within_config_folder>_results.yaml
+  ```
+  
+ The detial of demo script is shown in https://git-amd.tuebingen.mpg.de/bponton/timeoptimization
+
+Cartesian Coordinate
+----
+The global frame of this module is as follows:
+'x' : forward direction of humanoid.
+'y' : segital direction
+
+Related Works
+----
+video (B. Ponton, "https://www.youtube.com/watch?v=ZGhSCILANDw")
+
+article (B. Ponton et al, "https://arxiv.org/abs/1709.09265")
+
+
+License
+----
+The software is distributed under the [GNU General Public License v3.0](http://www.gnu.org/copyleft/gpl.html).
+
+Credits
+----
+
++ Max Planck Society (developer of time and momentum optimization)
++ Alexander Domahidi (developer of interior point solver)
++ Timothy A. Davis (developer of sparse matrix routines)
++ B. Ponton (origianl source, https://git-amd.tuebingen.mpg.de/bponton/timeoptimization)
+

bindings/CMakeLists.txt

+IF(BUILD_PYTHON_INTERFACE)
+  ADD_SUBDIRECTORY(python)
+ENDIF(BUILD_PYTHON_INTERFACE)

bindings/python/CMakeLists.txt

+# --- MACROS --- #
+MACRO(SYMLINK_AND_INSTALL_HEADERS HEADERS SUBPATH)
+  FOREACH(header ${HEADERS})
+    GET_FILENAME_COMPONENT(headerName ${header} NAME)
+    GET_FILENAME_COMPONENT(headerPath ${header} PATH)
+    EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E ${LINK}
+      ${CMAKE_CURRENT_SOURCE_DIR}/${header}
+      ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${SUBPATH}/${header})
+
+    INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/${header}
+      DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${SUBPATH}/${headerPath}
+            PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+  ENDFOREACH(header)
+ENDMACRO(SYMLINK_AND_INSTALL_HEADERS HEADERS SUBPATH)
+
+FUNCTION(REMOVE_PATH_FROM_LIST list_name path_name dest_list)
+  SET(list_name_)
+  FOREACH(header ${list_name})
+    STRING(REGEX REPLACE "${path_name}" "" header ${header}) 
+    LIST(APPEND list_name_ ${header})
+  ENDFOREACH(header ${list_name_})
+  SET(${dest_list} ${list_name_} PARENT_SCOPE)
+ENDFUNCTION(REMOVE_PATH_FROM_LIST)
+
+
+# --- PYTHON TARGET --- #
+ADD_CUSTOM_TARGET(python)
+SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+
+SET(PYWRAP ${PROJECT_NAME}_pywrap)
+SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
+
+# --- DEPENDENCIES --- #
+SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 1.4.0")
+
+FOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
+  ADD_COMPILE_DEPENDENCY(${dep})
+ENDFOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
+
+# --- LIBRARY --- #
+SET(${PYWRAP}_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}/${PROJECT_NAME})
+
+FILE(GLOB_RECURSE ${PROJECT_NAME}_PYTHON_HEADERS
+     ${CMAKE_CURRENT_SOURCE_DIR}/timeopt/*.hpp 
+     ${CMAKE_CURRENT_SOURCE_DIR}/container/*.hpp 
+     ) 
+
+FILE(GLOB_RECURSE ${PROJECT_NAME}_PYTHON_SOURCES
+     ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp 
+     ) 
+
+REMOVE_PATH_FROM_LIST("${${PROJECT_NAME}_PYTHON_HEADERS}" "${CMAKE_CURRENT_SOURCE_DIR}/" ${PROJECT_NAME}_PYTHON_HEADERS)
+REMOVE_PATH_FROM_LIST("${${PROJECT_NAME}_PYTHON_SOURCES}" "${CMAKE_CURRENT_SOURCE_DIR}/" ${PROJECT_NAME}_PYTHON_SOURCES)
+
+LIST(APPEND HEADERS ${${PROJECT_NAME}_PYTHON_HEADERS})
+
+# Headers of the python binding
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/timeopt/bindings/python")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/timeopt/bindings/python/timeopt")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/timeopt/bindings/python/container")
+
+SYMLINK_AND_INSTALL_HEADERS("${${PROJECT_NAME}_PYTHON_HEADERS}" "bindings/python")
+
+# --- COMPILE WRAPPER
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/bindings/python/${PROJECT_NAME}")
+SET(${PYWRAP}_SOURCES ${${PROJECT_NAME}_PYTHON_SOURCES})
+SET(${PYWRAP}_HEADERS ${${PROJECT_NAME}_PYTHON_HEADERS})
+
+ADD_LIBRARY(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
+ADD_DEPENDENCIES(python ${PYWRAP})
+
+IF(BUILD_WITH_COMMIT_VERSION)
+  TAG_LIBRARY_VERSION(${PYWRAP})
+ENDIF(BUILD_WITH_COMMIT_VERSION)
+ADD_HEADER_GROUP(${PYWRAP}_HEADERS)
+ADD_SOURCE_GROUP(${PYWRAP}_SOURCES)
+PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} momentumopt)
+
+TARGET_LINK_LIBRARIES(${PYWRAP} ${PROJECT_NAME})
+TARGET_LINK_BOOST_PYTHON(${PYWRAP})
+
+IF(APPLE)
+  # We need to change the extension for python bindings
+  SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES SUFFIX ".so")
+ENDIF(APPLE)
+
+SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}")
+
+INSTALL(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
+
+# --- INSTALL SCRIPTS 
+SET(PYTHON_FILES
+  __init__.py
+  )
+
+FOREACH(python ${PYTHON_FILES})
+  INSTALL(FILES
+    "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/${python}"
+    DESTINATION ${${PYWRAP}_INSTALL_DIR})
+ENDFOREACH(python)

bindings/python/__init__.py

+import libtimeopt_pywrap as timeopt
+from libtimeopt_pywrap import *

bindings/python/container/array.hpp

+#ifndef __python_container_array_hpp__
+#define __python_container_array_hpp__
+
+/// Credit : code adapted from http://stackoverflow.com/questions/18882089/wrapping-arrays-in-boost-python
+
+#include <string>
+#include <typeinfo>
+#include <boost/python.hpp>
+#include <boost/python/suite/indexing/indexing_suite.hpp>
+
+namespace boost
+{  
+  namespace python
+  {
+    
+    namespace bp = boost::python;
+    
+    namespace details {
+      
+      template <typename> struct array_trait;
+      
+      /// @brief Type that proxies to an array.
+      template <typename T>
+      class array_proxy
+      {
+      public:
+        // Types
+        typedef T           value_type;
+        typedef T*          iterator;
+        typedef T&          reference;
+        typedef std::size_t size_type;
+        typedef std::ptrdiff_t difference_type;
+        
+        /// @brief Empty constructor.
+        array_proxy()
+        : ptr_(0),
+        length_(0)
+        {}
+        
+        /// @brief Construct with iterators.
+        template <typename Iterator>
+        array_proxy(Iterator begin, Iterator end)
+        : ptr_(&*begin),
+        length_(std::distance(begin, end))
+        {}
+        
+        /// @brief Construct with start and size.
+        array_proxy(reference begin, std::size_t length)
+        : ptr_(&begin),
+        length_(length)
+        {}
+        
+        // Iterator support.
+        iterator begin()               { return ptr_; }
+        iterator end()                 { return ptr_ + length_; }
+        
+        // Element access.
+        reference operator[](size_t i) { return ptr_[i]; }
+        
+        // Capacity.
+        size_type size()               { return length_; }
+        
+      private:
+        T* ptr_;
+        std::size_t length_;
+      };
+      
+      /// @brief Make an array_proxy.
+      template <typename T>
+      array_proxy<typename array_trait<T>::element_type>
+      make_array_proxy(T& array)
+      {
+        return array_proxy<typename array_trait<T>::element_type>(
+                                                                  array[0],
+                                                                  array_trait<T>::static_size);
+      }
+      
+      /// @brief Policy type for referenced indexing, meeting the DerivedPolicies
+      ///        requirement of bp::index_suite.
+      ///
+      /// @note Requires Container to support:
+      ///          - value_type and size_type types,
+      ///          - value_type is default constructable and copyable,
+      ///          - element access via operator[],
+      ///          - Default constructable, iterator constructable,
+      ///          - begin(), end(), and size() member functions
+      template <typename Container>
+      class ref_index_suite
+      : public bp::indexing_suite<Container, ref_index_suite<Container> >
+      {
+      public:
+        
+        typedef typename Container::value_type data_type;
+        typedef typename Container::size_type index_type;
+        typedef typename Container::size_type size_type;
+        typedef typename Container::difference_type difference_type;
+        
+        // Element access and manipulation.
+        
+        /// @brief Get element from container.
+        static data_type&
+        get_item(Container & container, index_type index)
+        {
+          return container[index];
+        }
+        
+        /// @brief Set element from container.
+        static void
+        set_item(Container & container, index_type index, const data_type& value)
+        {
+          container[index] = value;
+        }
+        
+        /// @brief Reset index to default value.
+        static void
+        delete_item(Container & container, index_type index)
+        {
+          set_item(container, index, data_type());
+        };
+        
+        // Slice support.
+        
+        /// @brief Get slice from container.
+        ///
+        /// @return Python object containing
+        static bp::object
+        get_slice(Container & container, index_type from, index_type to)
+        {
+          using bp::list;
+          if (from > to) return list();
+          
+          // Return copy, as container only references its elements.
+          list list;
+          while (from != to) list.append(container[from++]);
+          return list;
+        };
+        
+        /// @brief Set a slice in container with a given value.
+        static void
+        set_slice(Container & container, index_type from,
+                  index_type to, const data_type& value
+                  )
+        {
+          // If range is invalid, return early.
+          if (from > to) return;
+          
+          // Populate range with value.
+          while (from < to) container[from++] = value;
+        }
+        
+        /// @brief Set a slice in container with another range.
+        template <class Iterator>
+        static void
+        set_slice(Container & container, index_type from,
+                  index_type to, Iterator first, Iterator /*last*/
+                  )
+        {
+          // If range is invalid, return early.
+          if (from > to) return;
+          
+          // Populate range with other range.
+          while (from < to) container[from++] = *first++;
+        }
+        
+        /// @brief Reset slice to default values.
+        static void
+        delete_slice(Container & container, index_type from, index_type to)
+        {
+          set_slice(container, from, to, data_type());
+        }
+        
+        // Capacity.
+        
+        /// @brief Get size of container.
+        static std::size_t
+        size(Container & container) { return container.size(); }
+        
+        /// @brief Check if a value is within the container.
+        template <class T>
+        static bool
+        contains(Container & container, const T& value)
+        {
+          return std::find(container.begin(), container.end(), value)
+          != container.end();
+        }
+        
+        /// @brief Compare two indexes
+        static bool
+        compare_index(Container & /*container*/, index_type a, index_type b)
+        {
+          return a < b;
+        }
+        
+        /// @brief Minimum index supported for container.
+        static index_type
+        get_min_index(Container & /*container*/)
+        {
+          return 0;
+        }
+        
+        /// @brief Maximum index supported for container.
+        static index_type
+        get_max_index(Container & container)
+        {
+          return size(container);
+        }
+        
+        // Misc.
+        
+        /// @brief Convert python index (could be negative) to a valid container
+        ///        index with proper boundary checks.
+        static index_type
+        convert_index(Container & container, PyObject* object)
+        {
+          bp::extract<long> py_index(object);
+          
+          // If py_index cannot extract a long, then type the type is wrong so
+          // set error and return early.
+          if (!py_index.check())
+          {
+            PyErr_SetString(PyExc_TypeError, "Invalid index type");
+            bp::throw_error_already_set();
+            return index_type();
+          }
+          
+          // Extract index.
+          long index = py_index();
+          
+          // Adjust negative index.
+          if (index < 0)
+            index += container.size();
+          
+          // Boundary check.
+          if (index >= long(container.size()) || index < 0)
+          {
+            PyErr_SetString(PyExc_IndexError, "Index out of range");
+            bp::throw_error_already_set();
+          }
+          
+          return (index_type)index;
+        }
+      };
+      
+      /// @brief Trait for arrays.
+      template <typename T>
+      struct array_trait_impl;
+      
+      // Specialize for native array.
+      template <typename T, std::size_t N>
+      struct array_trait_impl<T[N]>
+      {
+        typedef T element_type;
+        enum { static_size = N };
+        typedef array_proxy<element_type> proxy_type;
+        typedef bp::default_call_policies policy;
+        typedef boost::mpl::vector<array_proxy<element_type> > signature;
+      };
+      
+      // Specialize boost::array to use the native array trait.
+      template <typename T, std::size_t N>
+      struct array_trait_impl<boost::array<T, N> >
+      : public array_trait_impl<T[N]>
+      {};
+      
+      // @brief Specialize for member objects to use and modify non member traits.
+      template <typename T, typename C>
+      struct array_trait_impl<T (C::*)>
+      : public array_trait_impl<T>
+      {
+        typedef bp::with_custodian_and_ward_postcall<
+        0, // return object (custodian)
+        1  // self or this (ward)
+        > policy;
+        
+        // Append the class to the signature.
+        typedef typename boost::mpl::push_back<
+        typename array_trait_impl<T>::signature, C&>::type signature;
+      };
+      
+      /// @brief Trait class used to deduce array information, policies, and
+      ///        signatures
+      template <typename T>
+      struct array_trait: