...
 
Commits (55)
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
Subproject commit cbc92f81bdd0dcfd7fc1eae56801d8111267937d
......@@ -15,7 +15,6 @@ branches:
only:
- master
- devel
- debian
matrix:
allow_failures:
- compiler: clang
......
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
#
# 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/>.
# Copyright 2010, 2019 JRL, CNRS/AIST, LAAS CNRS
# See LICENSE file.
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
......@@ -20,16 +8,28 @@ SET(PROJECT_NAME sot-core)
SET(PROJECT_DESCRIPTION "Hierarchical task solver plug-in for dynamic-graph.")
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/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.
SET(CXX_DISABLE_WERROR True)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/boost.cmake)
SET(DOXYGEN_USE_MATHJAX YES)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
......@@ -37,47 +37,80 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
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 (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
OPTION(SUFFIX_SO_VERSION
"Suffix shared library name by a string depending on git status of project"
ON)
PKG_CONFIG_APPEND_LIBS("sot-core")
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex )
SEARCH_FOR_EIGEN()
SET(BOOST_COMPONENTS
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)
FINDPYTHON()
STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
SET(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/dynamic_graph/sot/core)
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.4.1")
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
SET(PYTHON_INSTALL_DIR
${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)
ADD_COMPILE_DEPENDENCY ("pinocchio >= 2.2.1")
ADD_REQUIRED_DEPENDENCY ("pinocchio >= 2.2.1")
SEARCH_FOR_BOOST()
ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting)
ADD_SUBDIRECTORY(doc)
#ADD_SUBDIRECTORY(doc)
# **********************************
# Robot_utils_sot_py PYTHON module *
# **********************************
IF(BUILD_PYTHON_INTERFACE)
IF(NOT CMAKE_VERSION VERSION_LESS "3.12" AND NOT WIN32)
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)
PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py dynamic-graph)
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)
TARGET_LINK_LIBRARIES(robot_utils_sot_py ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBRARY_NAME})
TARGET_LINK_BOOST_PYTHON(robot_utils_sot_py)
INSTALL(TARGETS robot_utils_sot_py DESTINATION ${PYTHON_INSTALL_DIR})
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:
virtual void display(std::ostream &os) const;
public:
void servoCurrentPosition(void);
void servoCurrentPosition(const int &time);
private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
......@@ -14,6 +14,7 @@
#include "sot/core/feature-abstract.hh"
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/value.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -64,14 +65,12 @@ public:
protected:
virtual dg::Vector &computeError(dg::Vector &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);
signalIn_t state_;
signalIn_t posture_;
signalIn_t postureDot_;
signalOut_t error_;
dg::Matrix jacobian_;
private:
std::vector<bool> activeDofs_;
......
......@@ -14,25 +14,26 @@
#include <Eigen/SVD>
#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);
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
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);
} // namespace Eigen
} // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_SVD_H__ */
......@@ -11,6 +11,7 @@
#define __SOT_MEMORY_TASK_HH
#include "sot/core/api.hh"
#include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
......@@ -21,44 +22,48 @@ namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract,
public dg::Entity {
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
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. */
dg::Vector err;
dg::Vector err, tmpTask, tmpVar;
dg::Matrix Jt; //( nJ,mJ );
dg::Matrix Jp;
dg::Matrix PJp;
dg::Matrix JK; //(nJ,mJ);
dg::Matrix Proj;
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
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:
/**
* \param mJ is the number of joints
* \param nJ the number of feature in the task
**/
MemoryTaskSOT(const std::string &name, const Matrix::Index nJ = 0,
const Matrix::Index mJ = 0);
virtual void initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
bool atConstruction = false);
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
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;
MemoryTaskSOT(const Matrix::Index nJ = 0, const Matrix::Index mJ = 0);
void display(std::ostream &os) const;
private:
void initMemory(const Matrix::Index nJ, const Matrix::Index mJ);
Matrix kernelMem;
};
} /* namespace sot */
......
......@@ -75,13 +75,9 @@ protected:
command computed by the stack of tasks. */
unsigned int nbJoints;
/*! \brief Store a pointer to compute the gradient */
TaskAbstract *taskGradient;
// Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>
// Proj;
/*! Force the recomputation at each step. */
bool recomputeEachTime;
/*! \brief Option to disable the computation of the SVD for the last task
if this task is a Task with a single FeaturePosture */
bool enablePostureTaskAcceleration;
public:
/*! \brief Threshold to compute the dumped pseudo inverse. */
......@@ -174,13 +170,14 @@ public: /* --- SIGNALS --- */
@{
*/
/*! \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).
*/
SignalPtr<dg::Vector, int> q0SIN;
/*! \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
* parameter to be computed.
* \note K should be an orthonormal matrix.
*/
SignalPtr<dg::Matrix, int> proj0SIN;
/*! \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}
SHARED
${${LIBRARY_NAME}_SOURCES})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
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})
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)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE)
#IF(BUILD_PYTHON_INTERFACE)
# PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
#ENDIF(BUILD_PYTHON_INTERFACE)
IF(UNIX)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
......@@ -180,7 +195,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
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)
#Plugins compilation, link, and installation
......@@ -199,14 +219,21 @@ FOREACH(plugin ${plugins})
PROPERTIES
PREFIX "")
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
INSTALL_RPATH ${PLUGINDIR}
)
IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION}
INSTALL_RPATH ${DYNAMIC_GRAPH_PLUGINDIR})
)
ENDIF()
# Link with sot-core library
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${SOTCORE_LIB_NAME})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dynamic-graph::dynamic-graph)
ADD_DEPENDENCIES (${LIBRARY_NAME} ${SOTCORE_LIB_NAME})
......@@ -220,7 +247,7 @@ FOREACH(plugin ${plugins})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
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)
# build python submodule
......@@ -235,7 +262,11 @@ FOREACH(plugin ${plugins})
IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
# Install plugins
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)
ENDFOREACH(plugin)
......@@ -246,10 +277,16 @@ IF(BUILD_PYTHON_INTERFACE)
${NO_INSTALL_OF_INIT_PY})
# 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
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/__init__.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot
)
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/core/__init__.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)
//
{
using namespace dynamicgraph::command;
addCommand(
"keep",
makeCommandVoid0(
addCommand("keep",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid0(
"modify the desired position to servo at current pos.")));
docCommandVoid1(
"modify the desired position to servo at current pos.",
"time")));
}
}
......@@ -319,15 +319,15 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
* to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(void) {
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.accessCopy() : Id),
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
_jaMfa =
(jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
_oMjb = oMjb.accessCopy(),
(jaMfa.isPlugged() ? jaMfa.access(time) : Id),
_oMjb = oMjb.access(time),
_jbMfb =
(jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
(jbMfb.isPlugged() ? jbMfb.access(time) : Id);
faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}
......
......@@ -2,7 +2,7 @@
// JRL, CNRS/AIST.
#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/pool.h>
#include <string>
......@@ -11,7 +11,6 @@
#include <sot/core/feature-posture.hh>
namespace dg = ::dynamicgraph;
using ::dynamicgraph::command::Setter;
using dynamicgraph::sot::FeatureAbstract;
namespace dynamicgraph {
......@@ -40,10 +39,11 @@ FeaturePosture::FeaturePosture(const std::string &name)
state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"),
posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"),
postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"),
jacobian_(), activeDofs_(), nbActiveDofs_(0) {
activeDofs_(), nbActiveDofs_(0) {
signalRegistration(state_ << posture_ << postureDot_);
errorSOUT.addDependency(state_);
jacobianSOUT.setConstant(Matrix());
std::string docstring;
docstring = " \n"
......@@ -83,13 +83,9 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
return res;
}
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &res, int) {
res = jacobian_;
return res;
}
dg::Vector &FeaturePosture::computeActivation(dg::Vector &res, int) {
return res;
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
throw std::runtime_error("jacobian signal should be constant."
" This function should never be called");
}
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
......@@ -139,16 +135,17 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
}
}
// recompute jacobian
jacobian_.resize(nbActiveDofs_, dim);
jacobian_.setZero();
Matrix J(Matrix::Zero(nbActiveDofs_, dim));
std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
if (activeDofs_[i]) {
jacobian_(index, i) = 1;
J(index, i) = 1;
index++;
}
}
jacobianSOUT.setConstant(J);
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
......
......@@ -4,14 +4,16 @@
#include <sot/core/debug.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) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues =
svd.singularValues();
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues = svd.singularValues();
SVD_t::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
for (long i = 0; i < m_singularValues.size(); ++i) {
if (m_singularValues(i) > threshold)
......@@ -23,43 +25,34 @@ void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
svd.matrixU().transpose());
}
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas(svd.singularValues());
typedef SVD_t::SingularValuesType SV_t;
Eigen::ArrayWrapper<const SV_t> sigmas(svd.singularValues());
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() *
svd.matrixU().leftCols(m).transpose());
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse(svd, _inverseMatrix, threshold);
Uref = svd.matrixU();
Vref = svd.matrixV();
Sref = svd.singularValues();
sotDEBUGOUT(15);
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV);
dampedInverse(svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
} // namespace Eigen
} // namespace dynamicgraph
......@@ -480,6 +480,25 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
};
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
: public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
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 @@
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT";
MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ,
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);
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) {
initMemory(nJ, mJ);
}
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
bool atConstruction) {
sotDEBUG(15) << "Task-mermory " << getName() << ": resize " << nJ << "x" << mJ
<< std::endl;
void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
err.resize(nJ);
tmpTask.resize(nJ);
tmpVar.resize(mJ);
Jt.resize(nJ, mJ);
Jp.resize(mJ, nJ);
PJp.resize(mJ, nJ);
JK.resize(nJ, mJ);
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);
if (atConstruction) {
Jt.setZero();
Jp.setZero();
PJp.setZero();
JK.setZero();
} else {
Eigen::pseudoInverse(Jt, Jp);
}
Jt.setZero();
}
void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
This diff is collapsed.
......@@ -110,7 +110,7 @@ void TaskConti::display(std::ostream &os) const {
<< ": " << 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) {
os << "-> " << (*iter)->getName() << endl;
}
......
......@@ -114,7 +114,7 @@ void Task::addFeatureFromName(const std::string &featureName) {
void Task::clearFeatureList(void) {
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &s = **iter;
jacobianSOUT.removeDependency(s.jacobianSOUT);
......@@ -174,7 +174,7 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
int cursorError = 0;
/* 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) {
FeatureAbstract &feature = **iter;
......@@ -211,7 +211,7 @@ Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
res.resize(errorSOUT(time).size());
dynamicgraph::Vector::Index cursor = 0;
for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &feature = **iter;
......@@ -263,7 +263,7 @@ dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) {
// const Flags& selection = controlSelectionSIN(time);
/* 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) {
FeatureAbstract &feature = **iter;
sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
......@@ -315,14 +315,14 @@ void Task::display(std::ostream &os) const {
os << "Task " << name << ": " << 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) {
os << "-> " << (*iter)->getName() << endl;
}
}
std::ostream &Task::writeGraph(std::ostream &os) const {
std::list<FeatureAbstract *>::const_iterator itFeatureAbstract;
FeatureList_t::const_iterator itFeatureAbstract;
itFeatureAbstract = featureList.begin();
while (itFeatureAbstract != featureList.end()) {
os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName()
......
......@@ -23,5 +23,10 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
typedef Switch<bool, int> SwitchBool;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous,
"SwitchMatrixHomogeneous");
} // namespace sot
} // 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
set -e
rm -rf "$build_dir" "$install_dir"
. `dirname $0`/common.sh
# Set debug mode
set -x
set -v
# 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/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 $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list "
curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
# show memory usage before install
sudo free -m -t
# 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
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
gem install coveralls-lcov
}
install_dependencies()
{
if [[ ${CI_OS_NAME} = linux ]]; then
_linux_install_dependencies
fi
if [[ ${CI_OS_NAME} = osx ]]; then
_osx_install_dependencies
fi
# and we build directly dependencies from the Git repository
for package in ${ROS_GIT_DEPENDENCIES}; do
catkin_git_dependency "$package"
done
if `test "x${ROS_GIT_DEPENDENCIES}" != x`; then
catkin_build_workspace
fi
for package in ${GIT_DEPENDENCIES}; do
build_git_dependency "$package"
done
}
#########################
# -- Main script -- #
#########################
# Print Git version
git --version
# Setup Git identity.
git config --global user.name "JRL/IDH Continuous Integration Tool"
git config --global user.email "jrl-idh+ci@gmail.com"
# Retrieve the submodules.
git submodule update --quiet --init --recursive
# Fetch tags to compute the distance between the last release tag
# and us.
git fetch --quiet --tags
# Shortcuts.
git_clone="git clone --quiet --recursive"
# Display environment
echo "Environment:"
env
# When this script is called the current directory is ./custom_travis
. ./.travis/run ../.travis/before_install
setup_package_source
sudo sh -c "chmod a+rw -R /opt/openrobots"
# Check if we are in a debian branch...
if [ -d debian ]; then
setup_pbuilder
else
if [ ! x${DIST} = x ]; then
echo "skipping this build"
exit 0
fi
install_dependencies
fi
# End debug mode
set +v
......
......@@ -14,6 +14,8 @@ else
fi
export PKG_CONFIG_PATH="${PKG_CONFIG_PATH}:/opt/openrobots/lib/pkgconfig"
export LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:/opt/openrobots/lib"
export CMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH}:/opt/openrobots/"
export PYTHONPATH="${PYTHONPATH}:/opt/openrobots/lib/python3.5/site-packages"
# Setup environment variables.
. ./.travis/run ../.travis/build
......
......@@ -169,11 +169,11 @@ FOREACH(test ${tests})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${CMAKE_DL_LIBS})
ENDIF(UNIX)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph)
# PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE)
# IF(BUILD_PYTHON_INTERFACE)
# PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python)
# ENDIF(BUILD_PYTHON_INTERFACE)
# Link against Boost.
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY})
......
......@@ -141,3 +141,53 @@ BOOST_AUTO_TEST_CASE(test_matrix_selector) {
output << aMatrixSelector->getClassName();
BOOST_CHECK(output.is_equal("Selec_of_matrix"));
}
BOOST_AUTO_TEST_SUITE(test_rotation_conversions)
template <typename type> type random();
template <> VectorRollPitchYaw random<VectorRollPitchYaw>() {
return VectorRollPitchYaw::Random();
}
template <> VectorQuaternion random<VectorQuaternion>() {
return VectorQuaternion(Eigen::Vector4d::Random().normalized());
}
template <> MatrixRotation random<MatrixRotation>() {
return MatrixRotation(random<VectorQuaternion>());
}