Commit 82d44380 authored by olivier stasse's avatar olivier stasse Committed by GitHub

Merge pull request #120 from olivier-stasse/devel

CMake export rules
parents bbd69cab 194cef0b
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,50 +37,80 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES ...@@ -37,50 +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 OPTION(SUFFIX_SO_VERSION
"Suffix shared library name by a string depending on git status of project" "Suffix shared library name by a string depending on git status of project"
ON) ON)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
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 a3ab52d6525aa6350b657068fddb41665aec4e08 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(const int& time); void servoCurrentPosition(const int &time);
private: private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time); MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
...@@ -12,9 +12,9 @@ ...@@ -12,9 +12,9 @@
#include "sot/core/api.hh" #include "sot/core/api.hh"
#include "sot/core/feature-abstract.hh" #include "sot/core/feature-abstract.hh"
#include <dynamic-graph/value.h>
#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 ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
......
...@@ -11,8 +11,8 @@ ...@@ -11,8 +11,8 @@
#define __SOT_MEMORY_TASK_HH #define __SOT_MEMORY_TASK_HH
#include "sot/core/api.hh" #include "sot/core/api.hh"
#include <sot/core/task-abstract.hh>
#include <sot/core/matrix-svd.hh> #include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */
...@@ -24,8 +24,10 @@ namespace dg = dynamicgraph; ...@@ -24,8 +24,10 @@ namespace dg = dynamicgraph;
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract { class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
public: // protected: public: // protected:
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment> Kernel_t; typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment>
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment> KernelConst_t; 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, tmpTask, tmpVar; dg::Vector err, tmpTask, tmpVar;
...@@ -36,17 +38,16 @@ public: // protected: ...@@ -36,17 +38,16 @@ public: // protected:
SVD_t svd; SVD_t svd;
Kernel_t kernel; Kernel_t kernel;
void resizeKernel(const Matrix::Index r, const Matrix::Index c) void resizeKernel(const Matrix::Index r, const Matrix::Index c) {
{
if (kernel.rows() != r || kernel.cols() != c) { if (kernel.rows() != r || kernel.cols() != c) {
if (kernelMem.size() < r*c) kernelMem.resize(r, c); if (kernelMem.size() < r * c)
kernelMem.resize(r, c);
new (&kernel) Kernel_t(kernelMem.data(), r, c); new (&kernel) Kernel_t(kernelMem.data(), r, c);
} }
} }
Kernel_t& getKernel(const Matrix::Index r, const Matrix::Index c) Kernel_t &getKernel(const Matrix::Index r, const Matrix::Index c) {
{ resizeKernel(r, c);
resizeKernel(r,c);
return kernel; return kernel;
} }
......
<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,17 +158,30 @@ ADD_LIBRARY(${LIBRARY_NAME} ...@@ -158,17 +158,30 @@ ADD_LIBRARY(${LIBRARY_NAME}
SHARED SHARED
${${LIBRARY_NAME}_SOURCES}) ${${LIBRARY_NAME}_SOURCES})
target_include_directories(${LIBRARY_NAME}
PUBLIC
$<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}) IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES PROPERTIES
SOVERSION ${PROJECT_VERSION}) SOVERSION ${PROJECT_VERSION})
ENDIF() ENDIF()
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph) target_link_libraries(${LIBRARY_NAME} dynamic-graph::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})
...@@ -182,7 +195,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES}) ...@@ -182,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
...@@ -203,7 +221,7 @@ FOREACH(plugin ${plugins}) ...@@ -203,7 +221,7 @@ FOREACH(plugin ${plugins})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES PROPERTIES
INSTALL_RPATH ${DYNAMIC_GRAPH_PLUGINDIR} INSTALL_RPATH ${PLUGINDIR}
) )
IF (${SUFFIX_SO_VERSION}) IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME} SET_TARGET_PROPERTIES(${LIBRARY_NAME}
...@@ -215,6 +233,7 @@ FOREACH(plugin ${plugins}) ...@@ -215,6 +233,7 @@ FOREACH(plugin ${plugins})
# 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})
...@@ -228,7 +247,7 @@ FOREACH(plugin ${plugins}) ...@@ -228,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
...@@ -243,7 +262,11 @@ FOREACH(plugin ${plugins}) ...@@ -243,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)
...@@ -254,10 +277,16 @@ IF(BUILD_PYTHON_INTERFACE) ...@@ -254,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(
makeCommandVoid1( *this, &FeaturePose<representation>::servoCurrentPosition,
*this, &FeaturePose<representation>::servoCurrentPosition, docCommandVoid1(
docCommandVoid1( "modify the desired position to servo at current pos.",
"modify the desired position to servo at current pos.", "time"))); "time")));
} }
} }
...@@ -319,7 +319,7 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot, ...@@ -319,7 +319,7 @@ 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(const int& time) { void FeaturePose<representation>::servoCurrentPosition(const int &time) {
check(*this); check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id), const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
......
...@@ -84,8 +84,8 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) { ...@@ -84,8 +84,8 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
} }
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) { dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
throw std::runtime_error ("jacobian signal should be constant." throw std::runtime_error("jacobian signal should be constant."
" This function should never be called"); " This function should never be called");
} }
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) { dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
...@@ -135,7 +135,7 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) { ...@@ -135,7 +135,7 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
} }
} }
// recompute jacobian // recompute jacobian
Matrix J (Matrix::Zero(nbActiveDofs_, dim)); Matrix J(Matrix::Zero(nbActiveDofs_, dim));
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) {
......
...@@ -5,15 +5,14 @@ ...@@ -5,15 +5,14 @@
#include <sot/core/matrix-svd.hh> #include <sot/core/matrix-svd.hh>
namespace dynamicgraph { namespace dynamicgraph {
using Eigen::ComputeFullV;
using Eigen::ComputeThinU; using Eigen::ComputeThinU;
using Eigen::ComputeThinV; using Eigen::ComputeThinV;
using Eigen::ComputeFullV;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix, void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) { const double threshold) {
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues = SVD_t::SingularValuesType m_singularValues = svd.singularValues();
svd.singularValues();
SVD_t::SingularValuesType singularValues_inv; SVD_t::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) {
...@@ -56,4 +55,4 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, ...@@ -56,4 +55,4 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dampedInverse(svd, _inverseMatrix, threshold); dampedInverse(svd, _inverseMatrix, threshold);
} }
} // namespace Eigen } // namespace dynamicgraph
...@@ -480,7 +480,8 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> { ...@@ -480,7 +480,8 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
}; };
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY); REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> { struct RPYToQuaternion
: public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) { void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
...@@ -490,7 +491,8 @@ struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaterni ...@@ -490,7 +491,8 @@ struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaterni
}; };
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion); REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
struct QuaternionToRPY : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> { struct QuaternionToRPY
: public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) { void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse(); res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
} }
......
set (DYNAMIC_GRAPH_PLUGINDIR \${CMAKE_CURRENT_LIST_DIR}/../../plugin)
set (DYNAMIC_GRAPH_PLUGINDIRNAME plugin)
...@@ -14,7 +14,7 @@ using namespace dynamicgraph::sot; ...@@ -14,7 +14,7 @@ using namespace dynamicgraph::sot;
using namespace dynamicgraph; using namespace dynamicgraph;
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ) MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) { : kernel(NULL, 0, 0) {
initMemory(nJ, mJ); initMemory(nJ, mJ);
} }
...@@ -28,7 +28,8 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) { ...@@ -28,7 +28,8 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index 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 the constraint is well conditioned, the kernel can be pre-allocated.
if (mJ > nJ) kernelMem.resize(mJ-nJ, mJ); if (mJ > nJ)
kernelMem.resize(mJ - nJ, mJ);
JK.setZero(); JK.setZero();
Jt.setZero(); Jt.setZero();
......
This diff is collapsed.
...@@ -25,6 +25,8 @@ typedef Switch<bool, int> SwitchBool; ...@@ -25,6 +25,8 @@ typedef Switch<bool, int> SwitchBool;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous; typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous, "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