Commit 1c9f7b4a authored by ggory15's avatar ggory15
Browse files

full upload

parent 90b88885
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()
# 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)
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
# --- 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)
import libtimeopt_pywrap as timeopt
from libtimeopt_pywrap import *
#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: