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:
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,50 +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)
OPTION(SUFFIX_SO_VERSION
"Suffix shared library name by a string depending on git status of project"
ON)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
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)
PYTHON_ADD_MODULE(robot_utils_sot_py src/tools/robot-utils-py.cpp)
PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py dynamic-graph)
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)
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 a3ab52d6525aa6350b657068fddb41665aec4e08
Subproject commit 61344038b1352d5a8de1e20db710c83be805d2eb
......@@ -151,7 +151,7 @@ public:
virtual void display(std::ostream &os) const;
public:
void servoCurrentPosition(const int& time);
void servoCurrentPosition(const int &time);
private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
......@@ -12,9 +12,9 @@
#include "sot/core/api.hh"
#include "sot/core/feature-abstract.hh"
#include <dynamic-graph/value.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/value.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......
......@@ -11,8 +11,8 @@
#define __SOT_MEMORY_TASK_HH
#include "sot/core/api.hh"
#include <sot/core/task-abstract.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......@@ -24,8 +24,10 @@ namespace dg = dynamicgraph;
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;
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, tmpTask, tmpVar;
......@@ -36,17 +38,16 @@ public: // protected:
SVD_t svd;
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 (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);
}
}
Kernel_t& getKernel(const Matrix::Index r, const Matrix::Index c)
{
resizeKernel(r,c);
Kernel_t &getKernel(const Matrix::Index r, const Matrix::Index c) {
resizeKernel(r, c);
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}
SHARED
${${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})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION})
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)
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})
......@@ -182,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
......@@ -203,7 +221,7 @@ FOREACH(plugin ${plugins})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
INSTALL_RPATH ${DYNAMIC_GRAPH_PLUGINDIR}
INSTALL_RPATH ${PLUGINDIR}
)
IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
......@@ -215,6 +233,7 @@ FOREACH(plugin ${plugins})
# 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})
......@@ -228,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
......@@ -243,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)
......@@ -254,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",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid1(
"modify the desired position to servo at current pos.", "time")));
addCommand("keep",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid1(
"modify the desired position to servo at current pos.",
"time")));
}
}
......@@ -319,7 +319,7 @@ 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(const int& time) {
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
......
......@@ -84,8 +84,8 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
}
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
throw std::runtime_error ("jacobian signal should be constant."
" This function should never be called");
throw std::runtime_error("jacobian signal should be constant."
" This function should never be called");
}
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
......@@ -135,7 +135,7 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
}
}
// recompute jacobian
Matrix J (Matrix::Zero(nbActiveDofs_, dim));
Matrix J(Matrix::Zero(nbActiveDofs_, dim));
std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
......
......@@ -5,15 +5,14 @@
#include <sot/core/matrix-svd.hh>
namespace dynamicgraph {
using Eigen::ComputeFullV;
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
using Eigen::ComputeFullV;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues =
svd.singularValues();
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) {
......@@ -56,4 +55,4 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dampedInverse(svd, _inverseMatrix, threshold);
}
} // namespace Eigen
} // namespace dynamicgraph
......@@ -480,7 +480,8 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
};
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
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()) *
......@@ -490,7 +491,8 @@ struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaterni
};
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
struct QuaternionToRPY : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
struct QuaternionToRPY
: public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
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;
using namespace dynamicgraph;
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) {
: kernel(NULL, 0, 0) {
initMemory(nJ, 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);
// 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();
Jt.setZero();
......
......@@ -29,12 +29,12 @@ sotSOT__INIT sotSOT_initiator;
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
#include <sot/core/factory.hh>
#include <sot/core/feature-posture.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/memory-task-sot.hh>
#include <sot/core/pool.hh>
#include <sot/core/task.hh>
#include <sot/core/feature-posture.hh>
using namespace std;
using namespace dynamicgraph::sot;
......@@ -54,8 +54,7 @@ const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::Sot(const std::string &name)
: Entity(name), stack(), nbJoints(0),
enablePostureTaskAcceleration (false),
: Entity(name), stack(), nbJoints(0), enablePostureTaskAcceleration(false),
q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
inversionThresholdSIN(NULL,
......@@ -93,16 +92,22 @@ Sot::Sot(const std::string &name)
*this, &Sot::getNbDof, docstring));
addCommand("enablePostureTaskAcceleration",
dynamicgraph::command::makeDirectSetter(*this,
&enablePostureTaskAcceleration, dynamicgraph::command::docDirectSetter(
"option to bypass SVD computation for the posture task at the last"
"level", "boolean")));
dynamicgraph::command::makeDirectSetter(
*this, &enablePostureTaskAcceleration,
dynamicgraph::command::docDirectSetter(
"option to bypass SVD computation for the posture task at "
"the last"
"level",
"boolean")));
addCommand("isPostureTaskAccelerationEnabled",
dynamicgraph::command::makeDirectGetter(*this,
&enablePostureTaskAcceleration, dynamicgraph::command::docDirectGetter(
"option to bypass SVD computation for the posture task at the last"
"level", "boolean")));
dynamicgraph::command::makeDirectGetter(
*this, &enablePostureTaskAcceleration,
dynamicgraph::command::docDirectGetter(
"option to bypass SVD computation for the posture task at "
"the last"
"level",
"boolean")));
docstring = " \n"
" push a task into the stack.\n"
......@@ -260,8 +265,7 @@ void Sot::down(const TaskAbstract &key) {
}
void Sot::clear(void) {
for (StackType::iterator it = stack.begin();
stack.end() != it; ++it) {
for (StackType::iterator it = stack.begin(); stack.end() != it; ++it) {
removeDependency(**it);
}
stack.clear();
......@@ -277,8 +281,8 @@ void Sot::defineNbDof(const unsigned int &nbDof) {
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
const Matrix &computeJacobianActivated(TaskAbstract *Ta, Task* T, Matrix &Jmem,
const int &iterTime) {
const Matrix &computeJacobianActivated(TaskAbstract *Ta, Task *T, Matrix &Jmem,
const int &iterTime) {
if (T != NULL) {
const Flags &controlSelec = T->controlSelectionSIN(iterTime);
sotDEBUG(25) << "Control selection = " << controlSelec << endl;
......@@ -293,7 +297,7 @@ const Matrix &computeJacobianActivated(TaskAbstract *Ta, Task* T, Matrix &Jmem,
return Ta->jacobianSOUT.accessCopy();
} else {
sotDEBUG(15) << "Task not activated." << endl;
const Matrix& Jac = Ta->jacobianSOUT.accessCopy();
const Matrix &Jac = Ta->jacobianSOUT.accessCopy();
Jmem = Matrix::Zero(Jac.rows(), Jac.cols());
return Jmem;
}
......@@ -305,48 +309,48 @@ typedef MemoryTaskSOT::Kernel_t Kernel_t;
typedef MemoryTaskSOT::KernelConst_t KernelConst_t;
template <typename MapType, typename MatrixType>
inline void makeMap (MapType& map, MatrixType& m) {
inline void makeMap(MapType &map, MatrixType &m) {
// There is not memory allocation here.
// See https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
new (&map) KernelConst_t(m.data(), m.rows(), m.cols());
}
void updateControl (MemoryTaskSOT* mem, const Matrix::Index rankJ,
bool has_kernel, const KernelConst_t& kernel, Vector& control)
{
const SVD_t &svd (mem->svd);
Vector &tmpTask (mem->tmpTask);
Vector &tmpVar (mem->tmpVar);
const Vector &err (mem->err);
void updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
bool has_kernel, const KernelConst_t &kernel,
Vector &control) {
const SVD_t &svd(mem->svd);
Vector &tmpTask(mem->tmpTask);
Vector &tmpVar(mem->tmpVar);
const Vector &err(mem->err);
// tmpTask <- S^-1 * U^T * err
tmpTask.head(rankJ).noalias() = svd.matrixU().leftCols(rankJ).adjoint() * err;
tmpTask.head(rankJ).array() *= svd.singularValues().head(rankJ).array().inverse();
tmpTask.head(rankJ).array() *=
svd.singularValues().head(rankJ).array().inverse();
// control <- kernel * (V * S^-1 * U^T * err)
if (has_kernel) {
tmpVar.head(kernel.cols()).noalias() = svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
tmpVar.head(kernel.cols()).noalias() =
svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
control.noalias() += kernel * tmpVar.head(kernel.cols());
} else
control.noalias() += svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
}
bool isFullPostureTask (Task* task, const Matrix::Index& nDof,
const int &iterTime) {
if (task == NULL
|| task->getFeatureList().size() != 1
|| !task->controlSelectionSIN(iterTime))
bool isFullPostureTask(Task *task, const Matrix::Index &nDof,
const int &iterTime) {
if (task == NULL || task->getFeatureList().size() != 1 ||
!task->controlSelectionSIN(iterTime))
return false;
FeaturePosture* posture =
dynamic_cast<FeaturePosture*>(task->getFeatureList().front());
FeaturePosture *posture =
dynamic_cast<FeaturePosture *>(task->getFeatureList().front());
assert(posture->dimensionSOUT(iterTime) <= nDof-6);
return posture !=NULL
&& posture->dimensionSOUT(iterTime) == nDof-6;
assert(posture->dimensionSOUT(iterTime) <= nDof - 6);
return posture != NULL && posture->dimensionSOUT(iterTime) == nDof - 6;
}
MemoryTaskSOT *getMemory (TaskAbstract& t, const Matrix::Index& tDim,
const Matrix::Index& nDof) {
MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim,
const Matrix::Index &nDof) {
MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(t.memoryInternal);
if (NULL == mem) {
if (NULL != t.memoryInternal)
......@@ -377,23 +381,22 @@ MemoryTaskSOT *getMemory (TaskAbstract& t, const Matrix::Index& tDim,
double dt
#define sotSTART_CHRONO1 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t0)
#define sotCHRONO1 \
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
dt = ((double)(t1.tv_sec - t0.tv_sec) * 1e6 + \
(double)(t1.tv_nsec - t0.tv_nsec) / 1e3 ); \
(double)(t1.tv_nsec - t0.tv_nsec) / 1e3); \
sotDEBUG(1) << "dT " << (long int)dt << std::endl
#define sotINITPARTCOUNTERS struct timespec tpart0
#define sotSTARTPARTCOUNTERS clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart0)
#define sotCOUNTER(nbc1, nbc2) \
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart##nbc2); \
dt##nbc2 = \
((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1e6 + \
(double)(tpart##nbc2.tv_nsec - tpart##nbc1.tv_nsec) / 1e3 )
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart##nbc2); \
dt##nbc2 = ((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1e6 + \
(double)(tpart##nbc2.tv_nsec - tpart##nbc1.tv_nsec) / 1e3)
#define sotINITCOUNTER(nbc1) \
struct timespec tpart##nbc1; \
double dt##nbc1 = 0;
#define sotPRINTCOUNTER(nbc1) \
sotDEBUG(1) << "dt" << iterTask << '_' << nbc1 << ' ' \
<< (long int)dt##nbc1 << ' '
sotDEBUG(1) << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \
<< ' '
#else // #ifdef WITH_CHRONO
#define sotINIT_CHRONO1
#define sotSTART_CHRONO1
......@@ -461,7 +464,7 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
// Get initial projector if any.
if (proj0SIN.isPlugged()) {
const Matrix &K = proj0SIN.access(iterTime);
makeMap (kernel, K);
makeMap(kernel, K);
has_kernel = true;
}
for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
......@@ -469,22 +472,23 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
sotDEBUGF(5, "Rank %d.", iterTask);
TaskAbstract &taskA = **iter;
Task *task = dynamic_cast<Task*>(*iter);
Task *task = dynamic_cast<Task *>(*iter);
bool last = (iterTask+1 == stack.size());
bool fullPostureTask = (last && enablePostureTaskAcceleration
&& isFullPostureTask(task, nbJoints, iterTime));
bool last = (iterTask + 1 == stack.size());
bool fullPostureTask = (last && enablePostureTaskAcceleration &&
isFullPostureTask(task, nbJoints, iterTime));
sotDEBUG(15) << "Task: e_" << taskA.getName() << std::endl;
/// Computing first the jacobian may be a little faster overall.
if (!fullPostureTask) taskA.jacobianSOUT.recompute(iterTime);
if (!fullPostureTask)
taskA.jacobianSOUT.recompute(iterTime);
taskA.taskSOUT.recompute(iterTime);
const Matrix::Index dim = taskA.taskSOUT.accessCopy().size();
sotCOUNTER(0, 1); // Direct Dynamic
/* Init memory. */
MemoryTaskSOT *mem = getMemory (taskA, dim, nbJoints);
MemoryTaskSOT *mem = getMemory(taskA, dim, nbJoints);
/***/ sotCOUNTER(1, 2); // first allocs
Matrix::Index rankJ = -1;
......@@ -500,16 +504,17 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
/* --- COMPUTE QDOT AND P --- */
if (!controlIsZero)
mem->err.noalias() -= control.tail(<