...
 
Commits (55)
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031 Subproject commit cbc92f81bdd0dcfd7fc1eae56801d8111267937d
...@@ -15,7 +15,6 @@ branches: ...@@ -15,7 +15,6 @@ branches:
only: only:
- master - master
- devel - devel
- debian
matrix: matrix:
allow_failures: allow_failures:
- compiler: clang - compiler: clang
......
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST # Copyright 2010, 2019 JRL, CNRS/AIST, LAAS CNRS
# # See LICENSE file.
# This file is part of sot-core.
# sot-core is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-core is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# sot-core. If not, see <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.8) CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
...@@ -20,16 +8,28 @@ SET(PROJECT_NAME sot-core) ...@@ -20,16 +8,28 @@ SET(PROJECT_NAME sot-core)
SET(PROJECT_DESCRIPTION "Hierarchical task solver plug-in for dynamic-graph.") SET(PROJECT_DESCRIPTION "Hierarchical task solver plug-in for dynamic-graph.")
SET(PROJECT_URL "http://github.com/${PROJECT_ORG}/${PROJECT_NAME}") SET(PROJECT_URL "http://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
# Export CMake Target
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
INCLUDE(cmake/base.cmake) INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
# Specify the project.
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
SET(DOXYGEN_USE_MATHJAX YES) find_package (Boost REQUIRED
python filesystem system thread program_options unit_test_framework)
find_package (Eigen3 REQUIRED NO_MODULE)
# Disable -Werror on Unix for now. INCLUDE(cmake/python.cmake)
SET(CXX_DISABLE_WERROR True) INCLUDE(cmake/boost.cmake)
SET(DOXYGEN_USE_MATHJAX YES)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES} ${PKG_CONFIG_ADDITIONAL_VARIABLES}
...@@ -37,47 +37,80 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES ...@@ -37,47 +37,80 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir plugindir
) )
# Specific to PKG module
# FIXME: to be changed into lib/dynamic-graph
# to avoid name collision when installing dynamic-graph in /usr.
SET(PLUGINDIR "${CMAKE_INSTALL_FULL_LIBDIR}/plugin")
# Extra macros for sot-core
install(FILES "src/sot-coreMacros.cmake"
DESTINATION "${CONFIG_INSTALL_DIR}")
set(PACKAGE_EXTRA_MACROS "${PACKAGE_EXTRA_MACROS}
include(\"\${CMAKE_CURRENT_LIST_DIR}/sot-coreMacros.cmake\")")
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON) OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF) OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
OPTION(SUFFIX_SO_VERSION
CMAKE_POLICY(SET CMP0048 OLD) "Suffix shared library name by a string depending on git status of project"
PROJECT(${PROJECT_NAME} CXX) ON)
PKG_CONFIG_APPEND_LIBS("sot-core") PKG_CONFIG_APPEND_LIBS("sot-core")
# Search for dependencies. # Search for dependencies.
# Boost # Boost
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex ) SET(BOOST_COMPONENTS
SEARCH_FOR_EIGEN() thread filesystem program_options
unit_test_framework system regex )
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.7.2") ADD_PROJECT_DEPENDENCY(dynamic-graph REQUIRED)
IF(BUILD_PYTHON_INTERFACE) IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON() FINDPYTHON()
STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME}) STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
SET(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME}) SET(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/dynamic_graph/sot/core) SET(PYTHON_INSTALL_DIR
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.4.1") ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}/dynamic_graph/sot/core)
#ADD_PROJECT_DEPENDENCY(dynamic-graph-python 3.0.0 REQUIRED)
ADD_REQUIRED_DEPENDENCY(dynamic-graph-python 3.0.0 REQUIRED)
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
ADD_COMPILE_DEPENDENCY ("pinocchio >= 2.2.1") ADD_REQUIRED_DEPENDENCY ("pinocchio >= 2.2.1")
SEARCH_FOR_BOOST() SEARCH_FOR_BOOST()
ADD_SUBDIRECTORY(include) ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting) ADD_SUBDIRECTORY(unitTesting)
ADD_SUBDIRECTORY(doc) #ADD_SUBDIRECTORY(doc)
# ********************************** # **********************************
# Robot_utils_sot_py PYTHON module * # Robot_utils_sot_py PYTHON module *
# ********************************** # **********************************
IF(BUILD_PYTHON_INTERFACE) IF(BUILD_PYTHON_INTERFACE)
PYTHON_ADD_MODULE(robot_utils_sot_py src/tools/robot-utils-py.cpp) IF(NOT CMAKE_VERSION VERSION_LESS "3.12" AND NOT WIN32)
PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py dynamic-graph) Python_add_library(robot_utils_sot_py MODULE src/tools/robot-utils-py.cpp)
ELSE(NOT CMAKE_VERSION VERSION_LESS "3.12" AND NOT WIN32)
PYTHON_ADD_MODULE(robot_utils_sot_py src/tools/robot-utils-py.cpp)
ENDIF()
TARGET_LINK_LIBRARIES(robot_utils_sot_py
${Boost_LIBRARIES}
${PYTHON_LIBRARIES} ${LIBRARY_NAME}
dynamic-graph::dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py pinocchio) PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py pinocchio)
TARGET_LINK_LIBRARIES(robot_utils_sot_py ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBRARY_NAME})
TARGET_LINK_BOOST_PYTHON(robot_utils_sot_py) TARGET_LINK_BOOST_PYTHON(robot_utils_sot_py)
INSTALL(TARGETS robot_utils_sot_py DESTINATION ${PYTHON_INSTALL_DIR}) INSTALL(TARGETS robot_utils_sot_py DESTINATION ${PYTHON_INSTALL_DIR})
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
# We do not want the project to be finalized if this is
# to install only the python interface.
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
SETUP_PROJECT_PACKAGE_FINALIZE()
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
get_cmake_property(_variableNames VARIABLES)
list (SORT _variableNames)
foreach (_variableName ${_variableNames})
message(STATUS "${_variableName}=${${_variableName}}")
endforeach()
Subproject commit 2a9086eaf5e7ef7cd6992e0fd6c6c615c0893400 Subproject commit 61344038b1352d5a8de1e20db710c83be805d2eb
...@@ -151,7 +151,7 @@ public: ...@@ -151,7 +151,7 @@ public:
virtual void display(std::ostream &os) const; virtual void display(std::ostream &os) const;
public: public:
void servoCurrentPosition(void); void servoCurrentPosition(const int &time);
private: private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time); MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
...@@ -14,6 +14,7 @@ ...@@ -14,6 +14,7 @@
#include "sot/core/feature-abstract.hh" #include "sot/core/feature-abstract.hh"
#include <dynamic-graph/signal-ptr.h> #include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h> #include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/value.h>
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */ /* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
...@@ -64,14 +65,12 @@ public: ...@@ -64,14 +65,12 @@ public:
protected: protected:
virtual dg::Vector &computeError(dg::Vector &res, int); virtual dg::Vector &computeError(dg::Vector &res, int);
virtual dg::Matrix &computeJacobian(dg::Matrix &res, int); virtual dg::Matrix &computeJacobian(dg::Matrix &res, int);
virtual dg::Vector &computeActivation(dg::Vector &res, int);
virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
signalIn_t state_; signalIn_t state_;
signalIn_t posture_; signalIn_t posture_;
signalIn_t postureDot_; signalIn_t postureDot_;
signalOut_t error_; signalOut_t error_;
dg::Matrix jacobian_;
private: private:
std::vector<bool> activeDofs_; std::vector<bool> activeDofs_;
......
...@@ -14,25 +14,26 @@ ...@@ -14,25 +14,26 @@
#include <Eigen/SVD> #include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
namespace Eigen { namespace dynamicgraph {
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, typedef Eigen::JacobiSVD<Matrix> SVD_t;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6); const double threshold = 1e-6);
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix, void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold = 1e-6); const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref, Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold = 1e-6); const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6); const double threshold = 1e-6);
} // namespace Eigen } // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_SVD_H__ */ #endif /* #ifndef __SOT_MATRIX_SVD_H__ */
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#define __SOT_MEMORY_TASK_HH #define __SOT_MEMORY_TASK_HH
#include "sot/core/api.hh" #include "sot/core/api.hh"
#include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh> #include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
...@@ -21,44 +22,48 @@ namespace dynamicgraph { ...@@ -21,44 +22,48 @@ namespace dynamicgraph {
namespace sot { namespace sot {
namespace dg = dynamicgraph; namespace dg = dynamicgraph;
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract, class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
public dg::Entity {
public: // protected: public: // protected:
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment>
Kernel_t;
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment>
KernelConst_t;
/* Internal memory to reduce the dynamic allocation at task resolution. */ /* Internal memory to reduce the dynamic allocation at task resolution. */
dg::Vector err; dg::Vector err, tmpTask, tmpVar;
dg::Matrix Jt; //( nJ,mJ ); dg::Matrix Jt; //( nJ,mJ );
dg::Matrix Jp;
dg::Matrix PJp;
dg::Matrix JK; //(nJ,mJ); dg::Matrix JK; //(nJ,mJ);
dg::Matrix Proj;
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
SVD_t svd; SVD_t svd;
Kernel_t kernel;
void resizeKernel(const Matrix::Index r, const Matrix::Index c) {
if (kernel.rows() != r || kernel.cols() != c) {
if (kernelMem.size() < r * c)
kernelMem.resize(r, c);
new (&kernel) Kernel_t(kernelMem.data(), r, c);
}
}
Kernel_t &getKernel(const Matrix::Index r, const Matrix::Index c) {
resizeKernel(r, c);
return kernel;
}
public: public:
/** /**
* \param mJ is the number of joints * \param mJ is the number of joints
* \param nJ the number of feature in the task * \param nJ the number of feature in the task
**/ **/
MemoryTaskSOT(const std::string &name, const Matrix::Index nJ = 0, MemoryTaskSOT(const Matrix::Index nJ = 0, const Matrix::Index mJ = 0);
const Matrix::Index mJ = 0);
void display(std::ostream &os) const;
virtual void initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
bool atConstruction = false); private:
void initMemory(const Matrix::Index nJ, const Matrix::Index mJ);
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME; Matrix kernelMem;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public: /* --- SIGNALS --- */
dg::Signal<dg::Matrix, int> jacobianInvSINOUT;
dg::Signal<dg::Matrix, int> jacobianConstrainedSINOUT;
dg::Signal<dg::Matrix, int> jacobianProjectedSINOUT;
dg::Signal<dg::Matrix, int> singularBaseImageSINOUT;
dg::Signal<unsigned int, int> rankSINOUT;
}; };
} /* namespace sot */ } /* namespace sot */
......
...@@ -75,13 +75,9 @@ protected: ...@@ -75,13 +75,9 @@ protected:
command computed by the stack of tasks. */ command computed by the stack of tasks. */
unsigned int nbJoints; unsigned int nbJoints;
/*! \brief Store a pointer to compute the gradient */ /*! \brief Option to disable the computation of the SVD for the last task
TaskAbstract *taskGradient; if this task is a Task with a single FeaturePosture */
bool enablePostureTaskAcceleration;
// Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>
// Proj;
/*! Force the recomputation at each step. */
bool recomputeEachTime;
public: public:
/*! \brief Threshold to compute the dumped pseudo inverse. */ /*! \brief Threshold to compute the dumped pseudo inverse. */
...@@ -174,13 +170,14 @@ public: /* --- SIGNALS --- */ ...@@ -174,13 +170,14 @@ public: /* --- SIGNALS --- */
@{ @{
*/ */
/*! \brief Intrinsec velocity of the robot, that is used to initialized /*! \brief Intrinsec velocity of the robot, that is used to initialized
* the recurence of the SOT (e.g. velocity comming from the other * the recurence of the SOT (e.g. velocity coming from the other
* OpenHRP plugins). * OpenHRP plugins).
*/ */
SignalPtr<dg::Vector, int> q0SIN; SignalPtr<dg::Vector, int> q0SIN;
/*! \brief A matrix K whose columns are a base of the desired velocity. /*! \brief A matrix K whose columns are a base of the desired velocity.
* In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free * In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
* parameter to be computed. * parameter to be computed.
* \note K should be an orthonormal matrix.
*/ */
SignalPtr<dg::Matrix, int> proj0SIN; SignalPtr<dg::Matrix, int> proj0SIN;
/*! \brief This signal allow to change the threshold for the /*! \brief This signal allow to change the threshold for the
......
<package format="2">
<name>sot-core</name>
<version>4.8.0</version>
<description>
Hierarchical task solver plug-in for dynamic-graph
</description>
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
<license>BSD</license>
<url type="website">http://stack-of-tasks.github.io</url>
<url type="repository">http://www.github.com/stack-of-tasks/sot-core</url>
<url type="bugtracker">http://www.github.com/stack-of-tasks/sot-core/issues</url>
<author>Olivier Stasse</author>
<author email="ostasse@laas.fr">ostasse@laas.fr</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend version_gte="1.1" version_lt="2.0">genmsg</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>dynamic-graph-python</build_depend>
<build_depend>pinocchio</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>dynamic-graph-python</build_export_depend>
<build_export_depend>pinocchio</build_export_depend>
<exec_depend>dynamic-graph-python</exec_depend>
<exec_depend>pinocchio</exec_depend>
<test_depend>gtest</test_depend>
<doc_depend>doxygen</doc_depend>
<doc_depend>dynamic_graph</doc_depend>
<export>
</export>
</package>
...@@ -158,15 +158,30 @@ ADD_LIBRARY(${LIBRARY_NAME} ...@@ -158,15 +158,30 @@ ADD_LIBRARY(${LIBRARY_NAME}
SHARED SHARED
${${LIBRARY_NAME}_SOURCES}) ${${LIBRARY_NAME}_SOURCES})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} target_include_directories(${LIBRARY_NAME}
PROPERTIES PUBLIC
SOVERSION ${PROJECT_VERSION}) $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/../include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/>
INTERFACE
$<INSTALL_INTERFACE:include>
)
IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION})
ENDIF()
target_link_libraries(${LIBRARY_NAME} dynamic-graph::dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph) #PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio) PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
IF(BUILD_PYTHON_INTERFACE) #IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python) # PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE) #ENDIF(BUILD_PYTHON_INTERFACE)
IF(UNIX) IF(UNIX)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS}) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
...@@ -180,7 +195,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES}) ...@@ -180,7 +195,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY) IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${LIBRARY_NAME} INSTALL(TARGETS ${LIBRARY_NAME}
DESTINATION ${CMAKE_INSTALL_LIBDIR}) EXPORT ${TARGETS_EXPORT_NAME}
PUBLIC_HEADER
INCLUDES DESTINATION include
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
#Plugins compilation, link, and installation #Plugins compilation, link, and installation
...@@ -201,12 +221,19 @@ FOREACH(plugin ${plugins}) ...@@ -201,12 +221,19 @@ FOREACH(plugin ${plugins})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES PROPERTIES
SOVERSION ${PROJECT_VERSION} INSTALL_RPATH ${PLUGINDIR}
INSTALL_RPATH ${DYNAMIC_GRAPH_PLUGINDIR}) )
IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION}
)
ENDIF()
# Link with sot-core library # Link with sot-core library
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${SOTCORE_LIB_NAME}) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${SOTCORE_LIB_NAME})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES}) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dynamic-graph::dynamic-graph)
ADD_DEPENDENCIES (${LIBRARY_NAME} ${SOTCORE_LIB_NAME}) ADD_DEPENDENCIES (${LIBRARY_NAME} ${SOTCORE_LIB_NAME})
...@@ -220,7 +247,7 @@ FOREACH(plugin ${plugins}) ...@@ -220,7 +247,7 @@ FOREACH(plugin ${plugins})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS}) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
ENDIF(UNIX) ENDIF(UNIX)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph) #PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio) PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
# build python submodule # build python submodule
...@@ -235,7 +262,11 @@ FOREACH(plugin ${plugins}) ...@@ -235,7 +262,11 @@ FOREACH(plugin ${plugins})
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY) IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
# Install plugins # Install plugins
INSTALL(TARGETS ${LIBRARY_NAME} INSTALL(TARGETS ${LIBRARY_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR}) EXPORT ${TARGETS_EXPORT_NAME}
PUBLIC_HEADER
INCLUDES DESTINATION include
LIBRARY DESTINATION ${PLUGINDIR}
)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ENDFOREACH(plugin) ENDFOREACH(plugin)
...@@ -246,10 +277,16 @@ IF(BUILD_PYTHON_INTERFACE) ...@@ -246,10 +277,16 @@ IF(BUILD_PYTHON_INTERFACE)
${NO_INSTALL_OF_INIT_PY}) ${NO_INSTALL_OF_INIT_PY})
# Install empty __init__.py files in intermediate directories. # Install empty __init__.py files in intermediate directories.
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/__init__.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/
)
INSTALL(FILES INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/__init__.py ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/__init__.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot
) )
INSTALL(FILES INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/__init__.py ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/math_small_entities.py ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/math_small_entities.py
......
__path__ = __import__('pkgutil').extend_path(__path__, __name__)
...@@ -104,12 +104,12 @@ FeaturePose<representation>::FeaturePose(const string &pointName) ...@@ -104,12 +104,12 @@ FeaturePose<representation>::FeaturePose(const string &pointName)
// //
{ {
using namespace dynamicgraph::command; using namespace dynamicgraph::command;
addCommand( addCommand("keep",
"keep", makeCommandVoid1(
makeCommandVoid0( *this, &FeaturePose<representation>::servoCurrentPosition,
*this, &FeaturePose<representation>::servoCurrentPosition, docCommandVoid1(
docCommandVoid0( "modify the desired position to servo at current pos.",
"modify the desired position to servo at current pos."))); "time")));
} }
} }
...@@ -319,15 +319,15 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot, ...@@ -319,15 +319,15 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
* to the current position. The effect on the servo is to maintain the * to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */ * current position and correct any drift. */
template <Representation_t representation> template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(void) { void FeaturePose<representation>::servoCurrentPosition(const int &time) {
check(*this); check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.accessCopy() : Id), const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
_jaMfa = _jaMfa =
(jaMfa.isPlugged() ? jaMfa.accessCopy() : Id), (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
_oMjb = oMjb.accessCopy(), _oMjb = oMjb.access(time),
_jbMfb = _jbMfb =
(jbMfb.isPlugged() ? jbMfb.accessCopy() : Id); (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb; faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
} }
......
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
// JRL, CNRS/AIST. // JRL, CNRS/AIST.
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <dynamic-graph/command-setter.h> #include <dynamic-graph/command-bind.h>
#include <dynamic-graph/factory.h> #include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h> #include <dynamic-graph/pool.h>
#include <string> #include <string>
...@@ -11,7 +11,6 @@ ...@@ -11,7 +11,6 @@
#include <sot/core/feature-posture.hh> #include <sot/core/feature-posture.hh>
namespace dg = ::dynamicgraph; namespace dg = ::dynamicgraph;
using ::dynamicgraph::command::Setter;
using dynamicgraph::sot::FeatureAbstract; using dynamicgraph::sot::FeatureAbstract;
namespace dynamicgraph { namespace dynamicgraph {
...@@ -40,10 +39,11 @@ FeaturePosture::FeaturePosture(const std::string &name) ...@@ -40,10 +39,11 @@ FeaturePosture::FeaturePosture(const std::string &name)
state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"), state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"),
posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"), posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"),
postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"), postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"),
jacobian_(), activeDofs_(), nbActiveDofs_(0) { activeDofs_(), nbActiveDofs_(0) {
signalRegistration(state_ << posture_ << postureDot_); signalRegistration(state_ << posture_ << postureDot_);
errorSOUT.addDependency(state_); errorSOUT.addDependency(state_);
jacobianSOUT.setConstant(Matrix());
std::string docstring; std::string docstring;
docstring = " \n" docstring = " \n"
...@@ -83,13 +83,9 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) { ...@@ -83,13 +83,9 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
return res; return res;
} }
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &res, int) { dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
res = jacobian_; throw std::runtime_error("jacobian signal should be constant."
return res; " This function should never be called");
}
dg::Vector &FeaturePosture::computeActivation(dg::Vector &res, int) {
return res;
} }
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) { dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
...@@ -139,16 +135,17 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) { ...@@ -139,16 +135,17 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
} }
} }
// recompute jacobian // recompute jacobian
jacobian_.resize(nbActiveDofs_, dim); Matrix J(Matrix::Zero(nbActiveDofs_, dim));
jacobian_.setZero();
std::size_t index = 0; std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) { for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
if (activeDofs_[i]) { if (activeDofs_[i]) {
jacobian_(index, i) = 1; J(index, i) = 1;
index++; index++;
} }
} }
jacobianSOUT.setConstant(J);
} }
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture"); DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
......
...@@ -4,14 +4,16 @@ ...@@ -4,14 +4,16 @@
#include <sot/core/debug.hh> #include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh> #include <sot/core/matrix-svd.hh>
namespace Eigen { namespace dynamicgraph {
using Eigen::ComputeFullV;
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) { const double threshold) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV); SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues = SVD_t::SingularValuesType m_singularValues = svd.singularValues();
svd.singularValues(); SVD_t::SingularValuesType singularValues_inv;
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues); singularValues_inv.resizeLike(m_singularValues);
for (long i = 0; i < m_singularValues.size(); ++i) { for (long i = 0; i < m_singularValues.size(); ++i) {
if (m_singularValues(i) > threshold) if (m_singularValues(i) > threshold)
...@@ -23,43 +25,34 @@ void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, ...@@ -23,43 +25,34 @@ void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
svd.matrixU().transpose()); svd.matrixU().transpose());
} }
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix, void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold) { const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t; typedef SVD_t::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas(svd.singularValues()); Eigen::ArrayWrapper<const SV_t> sigmas(svd.singularValues());
SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold)); SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
const dg::Matrix::Index m = sv_inv.size(); const Matrix::Index m = sv_inv.size();
_inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * _inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() *
svd.matrixU().leftCols(m).transpose()); svd.matrixU().leftCols(m).transpose());
} }
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref, Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold) { const double threshold) {
sotDEBUGIN(15); SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse(svd, _inverseMatrix, threshold); dampedInverse(svd, _inverseMatrix, threshold);
Uref = svd.matrixU(); Uref = svd.matrixU();
Vref = svd.matrixV(); Vref = svd.matrixV();
Sref = svd.singularValues(); Sref = svd.singularValues();
sotDEBUGOUT(15);
} }
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix, void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) { const double threshold) {
sotDEBUGIN(15); SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
dampedInverse(svd, _inverseMatrix, threshold); dampedInverse(svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
} }
} // namespace Eigen } // namespace dynamicgraph
...@@ -480,6 +480,25 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> { ...@@ -480,6 +480,25 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
}; };
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY); REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
struct RPYToQuaternion
: public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
.toRotationMatrix();
}
};
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
struct QuaternionToRPY
: public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
}
};
REGISTER_UNARY_OP(QuaternionToRPY, QuaternionToRPY);
struct QuaternionToMatrix struct QuaternionToMatrix
: public UnaryOpHeader<VectorQuaternion, MatrixRotation> { : public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
void operator()(const VectorQuaternion &r, MatrixRotation &res) { void operator()(const VectorQuaternion &r, MatrixRotation &res) {
......
set (DYNAMIC_GRAPH_PLUGINDIR \${CMAKE_CURRENT_LIST_DIR}/../../plugin)
set (DYNAMIC_GRAPH_PLUGINDIRNAME plugin)
...@@ -13,47 +13,26 @@ ...@@ -13,47 +13,26 @@
using namespace dynamicgraph::sot; using namespace dynamicgraph::sot;
using namespace dynamicgraph; using namespace dynamicgraph;
const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT"; MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) {
MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ, initMemory(nJ, mJ);
const Matrix::Index mJ)
: Entity(name),
jacobianInvSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::Jinv"),
jacobianConstrainedSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::JK"),
jacobianProjectedSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::Jt"),
singularBaseImageSINOUT("sotTaskAbstract(" + name +
")::inout(matrix)::V"),
rankSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::rank") {
signalRegistration(jacobianInvSINOUT << singularBaseImageSINOUT << rankSINOUT
<< jacobianConstrainedSINOUT
<< jacobianProjectedSINOUT);
initMemory(nJ, mJ, true);
} }
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ, void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
bool atConstruction) { err.resize(nJ);
sotDEBUG(15) << "Task-mermory " << getName() << ": resize " << nJ << "x" << mJ tmpTask.resize(nJ);
<< std::endl; tmpVar.resize(mJ);
Jt.resize(nJ, mJ); Jt.resize(nJ, mJ);
Jp.resize(mJ, nJ);
PJp.resize(mJ, nJ);
JK.resize(nJ, mJ); JK.resize(nJ, mJ);
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV); svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
// If the constraint is well conditioned, the kernel can be pre-allocated.
if (mJ > nJ)
kernelMem.resize(mJ - nJ, mJ);
JK.fill(0); JK.setZero();
if (atConstruction) { Jt.setZero();
Jt.setZero();
Jp.setZero();
PJp.setZero();
JK.setZero();
} else {
Eigen::pseudoInverse(Jt, Jp);
}
} }
void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
This diff is collapsed.
...@@ -110,7 +110,7 @@ void TaskConti::display(std::ostream &os) const { ...@@ -110,7 +110,7 @@ void TaskConti::display(std::ostream &os) const {
<< ": " << endl; << ": " << endl;
os << "--- LIST --- " << std::endl; os << "--- LIST --- " << std::endl;
for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin(); for (FeatureList_t::const_iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
os << "-> " << (*iter)->getName() << endl; os << "-> " << (*iter)->getName() << endl;
} }
......
...@@ -114,7 +114,7 @@ void Task::addFeatureFromName(const std::string &featureName) { ...@@ -114,7 +114,7 @@ void Task::addFeatureFromName(const std::string &featureName) {
void Task::clearFeatureList(void) { void Task::clearFeatureList(void) {
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin(); for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
FeatureAbstract &s = **iter; FeatureAbstract &s = **iter;
jacobianSOUT.removeDependency(s.jacobianSOUT); jacobianSOUT.removeDependency(s.jacobianSOUT);
...@@ -174,7 +174,7 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error, ...@@ -174,7 +174,7 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
int cursorError = 0; int cursorError = 0;
/* For each cell of the list, recopy value of s, s_star and error. */ /* For each cell of the list, recopy value of s, s_star and error. */
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin(); for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter; FeatureAbstract &feature = **iter;
...@@ -211,7 +211,7 @@ Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) { ...@@ -211,7 +211,7 @@ Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
res.resize(errorSOUT(time).size()); res.resize(errorSOUT(time).size());
dynamicgraph::Vector::Index cursor = 0; dynamicgraph::Vector::Index cursor = 0;
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin(); for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter; FeatureAbstract &feature = **iter;
...@@ -263,7 +263,7 @@ dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) { ...@@ -263,7 +263,7 @@ dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) {
// const Flags& selection = controlSelectionSIN(time); // const Flags& selection = controlSelectionSIN(time);
/* For each cell of the list, recopy value of s, s_star and error. */ /* For each cell of the list, recopy value of s, s_star and error. */
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin(); for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter; FeatureAbstract &feature = **iter;
sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl; sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
...@@ -315,14 +315,14 @@ void Task::display(std::ostream &os) const { ...@@ -315,14 +315,14 @@ void Task::display(std::ostream &os) const {
os << "Task " << name << ": " << endl; os << "Task " << name << ": " << endl;
os << "--- LIST --- " << std::endl; os << "--- LIST --- " << std::endl;
for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin(); for (FeatureList_t::const_iterator iter = featureList.begin();
iter != featureList.end(); ++iter) { iter != featureList.end(); ++iter) {
os << "-> " << (*iter)->getName() << endl; os << "-> " << (*iter)->getName() << endl;
} }
} }
std::ostream &Task::writeGraph(std::ostream &os) const { std::ostream &Task::writeGraph(std::ostream &os) const {
std::list<FeatureAbstract *>::const_iterator itFeatureAbstract; FeatureList_t::const_iterator itFeatureAbstract;
itFeatureAbstract = featureList.begin(); itFeatureAbstract = featureList.begin();
while (itFeatureAbstract != featureList.end()) { while (itFeatureAbstract != featureList.end()) {
os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName() os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName()
......
...@@ -23,5 +23,10 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector"); ...@@ -23,5 +23,10 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
typedef Switch<bool, int> SwitchBool; typedef Switch<bool, int> SwitchBool;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous,
"SwitchMatrixHomogeneous");
} // namespace sot } // namespace sot
} // namespace dynamicgraph } // namespace dynamicgraph
# -*- sh-mode -*
# This should be sourced, not called.
set -e
########################################
# -- VERBOSE HANDLING -- #
########################################
# More verbose handling for 'set -e'.
#
# Show a traceback if we're using bash, otherwise just a message.
# Downloaded from: https://gist.github.com/kergoth/3885825
on_exit () {
ret=$?
case $ret in
0)
;;
*)
echo >&2 "Exiting with $ret from a shell command"
;;
esac
}
on_error () {
local ret=$?
local FRAMES=${#BASH_SOURCE[@]}
echo >&2 "Traceback (most recent call last):"
for ((frame=FRAMES-2; frame >= 0; frame--)); do
local lineno=${BASH_LINENO[frame]}
printf >&2 ' File "%s", line %d, in %s\n' "${BASH_SOURCE[frame+1]}" "$lineno" "${FUNCNAME[frame+1]}"
sed >&2 -n "${lineno}s/^[ ]*/ /p" "${BASH_SOURCE[frame+1]}" || true
done
printf >&2 "Exiting with %d\n" "$ret"
exit $ret
}
case "$BASH_VERSION" in
'')
trap on_exit EXIT
;;
*)
set -o errtrace
trap on_error ERR
;;
esac
########################################
# -- GLOBAL UTILITIES -- #
########################################
# git_dependency_parsing
# ----------------------
#
# From an entry in GIT_DEPENDENCIES variable set git_dep, git_dep_uri and
# git_dep_branch in the environment
# For example given the input "jrl-umi3218/jrl-travis" the following variables
# are defined in the environment:
# - git_dep jrl-umi3218/jrl-travis
# - git_dep_uri git://github.com/jrl-umi3218/jrl-traviss
# - git_dep_branch master
# Or, given the input git@github.com:jrl-umi3218/jrl-travis#thebranch
# - git_dep jrl-umi3218/jrl-travis
# - git_dep_uri git@github.com:jrl-umi3218/jrl-travis
# - git_dep_branch thebranch
# The second (optional) argument allows to defined the default branch (defaults
# to master)
git_dependency_parsing()
{
_input=$1
export git_dep=${_input%%#*}
export git_dep_branch=${_input##*#}
if [ "$git_dep_branch" == "$git_dep" ]; then
if [ -e "$2" ]; then
export git_dep_branch=$2
else
export git_dep_branch="master"
fi
fi
git_dep_uri_base=${git_dep%%:*}
if [ "$git_dep_uri_base" == "$git_dep" ]; then
export git_dep_uri="git://github.com/$git_dep"
else
export git_dep_uri=$git_dep
export git_dep=${git_dep##*:}
fi
}
########################################
# -- ENVIRONMENT MANIPULATION -- #
########################################
_gitlab_setup_ci_vars()
{
export CI_REQUIRE_SUDO=false
export CI_PULL_REQUEST=false #FIXME Can it be provided by gitlab?
export CI_REPO_SLUG=`echo ${CI_PROJECT_DIR}|sed -e's@/builds/@@'`
export CI_BRANCH=${CI_BUILD_REF_NAME}
export CI_OS_NAME=${CI_OS_NAME:-linux}
}
_travis_setup_ci_vars()
{
export CI_REQUIRE_SUDO=${CI_REQUIRE_SUDO:-true}
export CI_PULL_REQUEST=${TRAVIS_PULL_REQUEST}
export CI_REPO_SLUG=${TRAVIS_REPO_SLUG}
export CI_BRANCH=${TRAVIS_BRANCH}
export CI_OS_NAME=${TRAVIS_OS_NAME:-linux}
}
# _setup_ci_vars
# --------------
#
# Setup CI_* variables based on the CI type
_setup_ci_vars()
{
# Check which CI tool we are using, default to travis
export CI_TOOL=${CI_TOOL:-travis}
if [ $CI_TOOL = travis ]; then
_travis_setup_ci_vars
else
_gitlab_setup_ci_vars
fi
}
# _setup_sudo_cmd
# ---------------
#
# Setup SUDO_CMD based on CI configuration
_setup_sudo_cmd()
{
if [ ${CI_REQUIRE_SUDO} = false ]; then
export SUDO_CMD=''
else
export SUDO_CMD='sudo'
fi
}
# _setup_ros
# ----------
#
# Setup ROS environment if present on the system
_setup_ros()
{
if [ -f /opt/ros/${ROS_DISTRO}/setup.sh ]; then
. /opt/ros/${ROS_DISTRO}/setup.sh
fi
}
# _setup_env_vars
# ---------------
#
# Setup environment variables
_setup_env_vars()
{
export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
export LTDL_LIBRARY_PATH="$install_dir/lib:$LTDL_LIBRARY_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$install_dir/share/pkgconfig:$PKG_CONFIG_PATH"
if type "python" > /dev/null; then
pythonsite_dir=`python -c "import sys, os; print(os.sep.join(['lib', 'python' + sys.version[:3], 'site-packages']))"`
export PYTHONPATH="$install_dir/$pythonsite_dir:$PYTHONPATH"
fi
}
# _setup_linux_env
# ----------------
#
# Environment setup specific to linux
_setup_linux_env()
{
export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
export LTDL_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LTDL_LIBRARY_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
}
# _setup_osx_env
# ----------------
#
# Environment setup specific to OSX
_setup_osx_env()
{
# Since default gcc on osx is just a front-end for LLVM...
if [[ ${CC} = gcc ]]; then
export CXX=g++-4.8
export CC=gcc-4.8
fi
export DYLD_LIBRARY_PATH="$install_dir/lib:$DYLD_LIBRARY_PATH"
export LTDL_LIBRARY_PATH="$install_dir/lib:$LTDL_LIBRARY_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
}
# setup_ci_env
# ------------
#
# Setup the CI and environment variables
setup_ci_env()
{
_setup_ci_vars
_setup_sudo_cmd
_setup_ros
_setup_env_vars
if [[ ${CI_OS_NAME} = linux ]]; then
_setup_linux_env
fi
if [[ ${CI_OS_NAME} = osx ]]; then
_setup_osx_env
fi
}
# Directories.
root_dir=`pwd`
build_dir="/tmp/_ci/build"
install_dir="/tmp/_ci/install"
echo "root_dir: " $root_dir
echo "build_dir: " $build_dir
echo "install_dir: " $install_dir
# Shortcuts.
git_clone="git clone --quiet --recursive"
# Setup all variables needed by the CI scripts
setup_ci_env
# Make cmake verbose.
export CMAKE_VERBOSE_MAKEFILE=1
export CTEST_OUTPUT_ON_FAILURE=1
# Add default DO_*_ON_BRANCH if needed
if [ -z ${DO_COVERAGE_ON_BRANCH+x} ]; then
export DO_COVERAGE_ON_BRANCH=${CI_BRANCH}
fi
if [ -z ${DO_CPPCHECK_ON_BRANCH+x} ]; then
export DO_CPPCHECK_ON_BRANCH=${CI_BRANCH}
fi
# Create layout.
mkdir -p "$build_dir"
mkdir -p "$install_dir"
#!/bin/bash #!/bin/bash
set -e
rm -rf "$build_dir" "$install_dir"
. `dirname $0`/common.sh
# Set debug mode # Set debug mode
set -x set -x
set -v set -v
# Add robotpkg # Add robotpkg
sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list " sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list "
sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list " sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list "
curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add - curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
# show memory usage before install # show memory usage before install
sudo free -m -t sudo free -m -t
# Setup environment variables. # Setup environment variables.
export APT_DEPENDENCIES="doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev libtinyxml-dev libconsole-bridge-dev robotpkg-pinocchio robotpkg-dynamic-graph-v3 robotpkg-py27-dynamic-graph-v3 robotpkg-sot-core-v3 " export APT_DEPENDENCIES="doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libboost-regex-dev libeigen3-dev libtinyxml-dev libconsole-bridge-dev robotpkg-pinocchio robotpkg-dynamic-graph-v3 robotpkg-py35-dynamic-graph-v3 "
# Add Python dependency # Add Python dependency
export APT_DEPENDENCIES=$APT_DEPENDENCIES" libboost-python-dev robotpkg-py27-eigenpy python2.7-dev python-numpy python-sphinx" export APT_DEPENDENCIES=$APT_DEPENDENCIES" libboost-python-dev robotpkg-py35-eigenpy python3.5-dev python3-numpy"
# Add Geometry dependencies
if [[ $BUILD_WITH_COLLISION_SUPPORT -eq ON ]]; then
export APT_DEPENDENCIES=$APT_DEPENDENCIES" robotpkg-hpp-fcl"
fi
##############################
# -- Helper functions -- #
##############################
_linux_setup_package_source()
{
# Speed up apt
${SUDO_CMD} sh -c "echo \"force-unsafe-io\" > /etc/dpkg/dpkg.cfg.d/02apt-speedup"
# Update the apt local cache.
${SUDO_CMD} apt-get update -qq
}
_osx_setup_package_source()
{
# Update homebrew
brew update
}
# setup_package_source
# ---------------------
#
# Setup the package source (e.g. homebrew on osx, apt on debian-like systems)
setup_package_source()
{
if [[ ${CI_OS_NAME} = linux ]]; then
_linux_setup_package_source
fi
if [[ ${CI_OS_NAME} = osx ]]; then
_osx_setup_package_source
fi
}
# setup_pbuilder
# --------------
#
# Setup a pbuilder environment
setup_pbuilder()
{
if `test x${DIST} = x`; then
echo "distribution is not set, skipping this build"
exit 0
fi
echo "Target distribution: ${DIST}"
# If we are, we install Debian package development tools and
# create a sid pbuilder. Package dependencies will be installed
# automatically.
${SUDO_CMD} apt-get install -qq \
debootstrap devscripts \
git-buildpackage debian-archive-keyring \
pkg-kde-tools dput eatmydata ccache
# Fix ccache use in pbuilder
${SUDO_CMD} addgroup --system --gid 1234 ccache
${SUDO_CMD} adduser --quiet --system --uid 1234 --ingroup ccache \
--home /var/cache/pbuilder --no-create-home pbuilder
${SUDO_CMD} mkdir -p /var/cache/pbuilder/ccache
${SUDO_CMD} chown -R pbuilder:ccache /var/cache/pbuilder/ccache
${SUDO_CMD} chmod -R g+ws /var/cache/pbuilder/ccache
# Remove previous sandbox.
${SUDO_CMD} rm -rf /var/cache/pbuilder/base-${DIST}.cow || true
# Create a pbuilder sandbox.
cp -f `dirname $0`/pbuilderrc $HOME/.pbuilderrc
sed -i "s|@DIST@|${DIST}|g" $HOME/.pbuilderrc
git-pbuilder create
# Speed up pbuilder.
echo "echo \"force-unsafe-io\" > /etc/dpkg/dpkg.cfg.d/02apt-speedup" | \
git-pbuilder login --save-after-exec
# Add additional PPAs
for ppa in ${DEBIAN_PPA}; do
echo "apt-add-repository ppa:${ppa}" | \
git-pbuilder login --save-after-exec
done
# Retrieve PPA package list.
git-pbuilder update
# FIXME There is something fishy here...
# ccache is not necessary in our case and may cause permission
# issues.
echo "apt-get -y remove ccache" | \
git-pbuilder login --save-after-exec
}
# catkin_git_dependency
# --------------------
#
# Clone catkin package into the workspace
# See arguments of build_git_dependency
# Branch defaults to $ROS_DISTRO instead of master
catkin_git_dependency()
{
git_dependency_parsing $1 $ROS_DISTRO
echo "--> Getting $git_dep (branch $git_dep_branch)"
CATKIN_DEP_WORKSPACE=/tmp/_ci/catkin_dep_ws
cd $CATKIN_DEP_WORKSPACE/src
$git_clone -b $git_dep_branch "$git_dep_uri" "$git_dep"
}
# catkin_build_workspace
# ----------------------
#
# Build catkin workspace
catkin_build_workspace()
{
CATKIN_DEP_WORKSPACE=/tmp/_ci/catkin_dep_ws
cd $CATKIN_DEP_WORKSPACE
catkin_make
}
# build_git_dependency
# --------------------
#
# Build a dependency directly from the Git development tree.
# First argument: repository's GitHub URL or repository's URI + optional branch
# For example: "jrl-umi3218/jrl-travis" or "jrl-umi3218/jrl-travis#dev"
# Or: user@host:path/to/repo or git@github.com:organization/repo#branch
build_git_dependency()
{
git_dependency_parsing $1
echo "--> Compiling $git_dep (branch $git_dep_branch)"
cd "$build_dir"
mkdir -p "$git_dep"
$git_clone -b $git_dep_branch "$git_dep_uri" "$git_dep"
cd "$git_dep"
mkdir -p build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" \
-DDISABLE_TESTS:BOOL=ON ${CMAKE_ADDITIONAL_OPTIONS}
make install || make
}
_osx_install_dependencies()
{
# Install user-specified packages
brew install cppcheck ${HOMEBREW_DEPENDENCIES}
}
_linux_install_dependencies()
{
# Add additional PPAs
for ppa in ${MASTER_PPA}; do
${SUDO_CMD} add-apt-repository -y ppa:${ppa}
done
${SUDO_CMD} apt-get update -qq
${SUDO_CMD} apt-get install -qq curl cppcheck ${APT_DEPENDENCIES}
# Install lcov from github
cd "$build_dir"
wget https://github.com/linux-test-project/lcov/releases/download/v1.12/lcov-1.12.tar.gz
tar zxvf lcov-1.12.tar.gz
cd lcov-1.12
# Reset lcov to release 1.12
${SUDO_CMD} make install