...
 
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)
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 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}
PROPERTIES
SOVERSION ${PROJECT_VERSION})
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
......@@ -201,12 +221,19 @@ FOREACH(plugin ${plugins})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION}
INSTALL_RPATH ${DYNAMIC_GRAPH_PLUGINDIR})
INSTALL_RPATH ${PLUGINDIR}
)
IF (${SUFFIX_SO_VERSION})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION}
)
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(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid0(
"modify the desired position to servo at current pos.")));
addCommand("keep",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
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);
}
JK.setZero();
Jt.setZero();
}
void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
......@@ -24,12 +24,16 @@ public:
sotSOT__INIT sotSOT_initiator;
#endif //#ifdef VP_DEBUG
#include <sot/core/sot.hh>
#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/sot.hh>
#include <sot/core/task.hh>
using namespace std;
......@@ -50,8 +54,7 @@ const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::Sot(const std::string &name)
: Entity(name), stack(), nbJoints(0), taskGradient(0),
recomputeEachTime(true),
: Entity(name), stack(), nbJoints(0), enablePostureTaskAcceleration(false),
q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
inversionThresholdSIN(NULL,
......@@ -88,6 +91,24 @@ Sot::Sot(const std::string &name)
new dynamicgraph::command::Getter<Sot, const unsigned int &>(
*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")));
addCommand("isPostureTaskAccelerationEnabled",
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"
" \n"
......@@ -160,7 +181,7 @@ TaskAbstract &Sot::pop(void) {
return *res;
}
bool Sot::exist(const TaskAbstract &key) {
std::list<TaskAbstract *>::iterator it;
StackType::iterator it;
for (it = stack.begin(); stack.end() != it; ++it) {
if (*it == &key) {
return true;
......@@ -170,7 +191,7 @@ bool Sot::exist(const TaskAbstract &key) {
}
void Sot::remove(const TaskAbstract &key) {
bool find = false;
std::list<TaskAbstract *>::iterator it;
StackType::iterator it;
for (it = stack.begin(); stack.end() != it; ++it) {
if (*it == &key) {
find = true;
......@@ -193,7 +214,7 @@ void Sot::removeDependency(const TaskAbstract &key) {
void Sot::up(const TaskAbstract &key) {
bool find = false;
std::list<TaskAbstract *>::iterator it;
StackType::iterator it;
for (it = stack.begin(); stack.end() != it; ++it) {
if (*it == &key) {
find = true;
......@@ -207,7 +228,7 @@ void Sot::up(const TaskAbstract &key) {
return;
}
std::list<TaskAbstract *>::iterator pos = it;
StackType::iterator pos = it;
pos--;
TaskAbstract *task = *it;
stack.erase(it);
......@@ -216,7 +237,7 @@ void Sot::up(const TaskAbstract &key) {
}
void Sot::down(const TaskAbstract &key) {
bool find = false;
std::list<TaskAbstract *>::iterator it;
StackType::iterator it;
for (it = stack.begin(); stack.end() != it; ++it) {
if (*it == &key) {
find = true;
......@@ -230,7 +251,7 @@ void Sot::down(const TaskAbstract &key) {
return;
}
std::list<TaskAbstract *>::iterator pos = it;
StackType::iterator pos = it;
pos++;
TaskAbstract *task = *it;
stack.erase(it);
......@@ -244,8 +265,7 @@ void Sot::down(const TaskAbstract &key) {
}
void Sot::clear(void) {
for (std::list<TaskAbstract *>::iterator it = stack.begin();
stack.end() != it; ++it) {
for (StackType::iterator it = stack.begin(); stack.end() != it; ++it) {
removeDependency(**it);
}
stack.clear();
......@@ -261,28 +281,84 @@ void Sot::defineNbDof(const unsigned int &nbDof) {
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
static void computeJacobianActivated(Task *taskSpec, dynamicgraph::Matrix &Jt,
const int &iterTime) {
if (NULL != taskSpec) {
const Flags &controlSelec = taskSpec->controlSelectionSIN(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;
if (controlSelec) {
if (!controlSelec) {
sotDEBUG(15) << "Control selection." << endl;
for (int i = 0; i < Jt.cols(); ++i) {
if (!controlSelec(i)) {
Jt.col(i).setZero();
}
}
} else {
sotDEBUG(15) << "S is equal to Id." << endl;
}
Jmem = Ta->jacobianSOUT.accessCopy();
for (int i = 0; i < Jmem.cols(); ++i)
if (!controlSelec(i))
Jmem.col(i).setZero();
return Jmem;
} else
return Ta->jacobianSOUT.accessCopy();
} else {
sotDEBUG(15) << "Task not activated." << endl;
Jt *= 0;
const Matrix &Jac = Ta->jacobianSOUT.accessCopy();
Jmem = Matrix::Zero(Jac.rows(), Jac.cols());
return Jmem;
}
} else { /* No selection specification: nothing to do. */
} else /* No selection specification: nothing to do. */
return Ta->jacobianSOUT.accessCopy();
}
typedef MemoryTaskSOT::Kernel_t Kernel_t;
typedef MemoryTaskSOT::KernelConst_t KernelConst_t;
template <typename MapType, typename MatrixType>
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);
// 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();
// control <- kernel * (V * S^-1 * U^T * err)
if (has_kernel) {
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))
return false;
FeaturePosture *posture =
dynamic_cast<FeaturePosture *>(task->getFeatureList().front());
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 *mem = dynamic_cast<MemoryTaskSOT *>(t.memoryInternal);
if (NULL == mem) {
if (NULL != t.memoryInternal)
delete t.memoryInternal;
mem = new MemoryTaskSOT(tDim, nDof);
t.memoryInternal = mem;
}
return mem;
}
/* --------------------------------------------------------------------- */
......@@ -301,26 +377,26 @@ static void computeJacobianActivated(Task *taskSpec, dynamicgraph::Matrix &Jt,
#ifdef WITH_CHRONO
#define sotINIT_CHRONO1 \
struct timeval t0, t1; \
struct timespec t0, t1; \
double dt
#define sotSTART_CHRONO1 gettimeofday(&t0, NULL)
#define sotSTART_CHRONO1 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t0)
#define sotCHRONO1 \
gettimeofday(&t1, NULL); \
dt = ((double)(t1.tv_sec - t0.tv_sec) * 1000. * 1000. + \
(double)(t1.tv_usec - t0.tv_usec)); \
sotDEBUG(1) << "dt: " << dt / 1000. << std::endl
#define sotINITPARTCOUNTERS struct timeval tpart0
#define sotSTARTPARTCOUNTERS gettimeofday(&tpart0, NULL)
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
dt = ((double)(t1.tv_sec - t0.tv_sec) * 1e6 + \
(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) \
gettimeofday(&tpart##nbc2, NULL); \
dt##nbc2 += \
((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1000. * 1000. + \
(double)(tpart##nbc2.tv_usec - tpart##nbc1.tv_usec))
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 timeval tpart##nbc1; \
struct timespec tpart##nbc1; \
double dt##nbc1 = 0;
#define sotPRINTCOUNTER(nbc1) \
sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
sotDEBUG(1) << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \
<< ' '
#else // #ifdef WITH_CHRONO
#define sotINIT_CHRONO1
#define sotSTART_CHRONO1
......@@ -355,21 +431,19 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
sotINITCOUNTER(4);
sotINITCOUNTER(5);
sotINITCOUNTER(6);
sotINITCOUNTER(7);
sotINITCOUNTER(8);
sotSTART_CHRONO1;
sotSTARTPARTCOUNTERS;
const double &th = inversionThresholdSIN(iterTime);
const Matrix::Index mJ = nbJoints; // number dofs
bool controlIsZero = true;
if (q0SIN.isPlugged()) {
control = q0SIN(iterTime);
if (control.size() != mJ) {
controlIsZero = false;
if (control.size() != nbJoints) {
std::ostringstream oss;
oss << "SOT(" << getName() << "): q0SIN value length is "
<< control.size() << "while the expected lenth is " << mJ;
<< control.size() << "while the expected lenth is " << nbJoints;
throw std::length_error(oss.str());
}
} else {
......@@ -379,165 +453,113 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
<< ") contains no task and q0SIN is not plugged.";
throw std::logic_error(oss.str());
}
control = Vector::Zero(mJ);
control = Vector::Zero(nbJoints);
sotDEBUG(25) << "No initial velocity." << endl;
}
sotDEBUGF(5, " --- Time %d -------------------", iterTime);
unsigned int iterTask = 0;
const Matrix *PrevProj = NULL;
KernelConst_t kernel(NULL, 0, 0);
bool has_kernel = false;
// Get initial projector if any.
if (proj0SIN.isPlugged()) {
const Matrix &initialProjector = proj0SIN.access(iterTime);
PrevProj = &initialProjector;
const Matrix &K = proj0SIN.access(iterTime);
makeMap(kernel, K);
has_kernel = true;
}
for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
sotDEBUGF(5, "Rank %d.", iterTask);
TaskAbstract &task = **iter;
sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
sotCOUNTER(0, 1); // Direct Dynamic
unsigned int rankJ;
const Matrix::Index nJ = Jac.rows(); // task dimension
assert(Jac.cols() == mJ);
/* Init memory. */
MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
if (NULL == mem) {
if (NULL != task.memoryInternal)
delete task.memoryInternal;
mem = new MemoryTaskSOT(task.getName() + "_memSOT", nJ, mJ);
task.memoryInternal = mem;
}
Matrix &Jp = mem->Jp;
Matrix &JK = mem->JK;
Matrix &Jt = mem->Jt;
Matrix &Proj = mem->Proj;
MemoryTaskSOT::SVD_t &svd = mem->svd;
sotSTARTPARTCOUNTERS;
taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
const dynamicgraph::Vector &err = mem->err;
Jp.resize(mJ, nJ);
Jt.resize(nJ, mJ);
JK.resize(nJ, mJ);
sotDEBUGF(5, "Rank %d.", iterTask);
TaskAbstract &taskA = **iter;
Task *task = dynamic_cast<Task *>(*iter);
if ((recomputeEachTime) ||
(task.jacobianSOUT.getTime() > mem->jacobianInvSINOUT.getTime()) ||
(mem->jacobianInvSINOUT.accessCopy().rows() != mJ) ||
(mem->jacobianInvSINOUT.accessCopy().cols() != nJ) ||
(task.jacobianSOUT.getTime() >
mem->jacobianConstrainedSINOUT.getTime()) ||
(task.jacobianSOUT.getTime() > mem->rankSINOUT.getTime()) ||
(task.jacobianSOUT.getTime() >
mem->singularBaseImageSINOUT.getTime())) {
sotDEBUG(2) << "Recompute inverse." << endl;
bool last = (iterTask + 1 == stack.size());
bool fullPostureTask = (last && enablePostureTaskAcceleration &&
isFullPostureTask(task, nbJoints, iterTime));
/* --- FIRST ALLOCS --- */
sotDEBUG(1) << "nJ=" << nJ << " "
<< "mJ=" << mJ << std::endl;
sotDEBUG(15) << "Task: e_" << taskA.getName() << std::endl;
/***/ sotCOUNTER(1, 2); // first allocs
/// Computing first the jacobian may be a little faster overall.
if (!fullPostureTask)
taskA.jacobianSOUT.recompute(iterTime);
taskA.taskSOUT.recompute(iterTime);
const Matrix::Index dim = taskA.taskSOUT.accessCopy().size();
sotCOUNTER(0, 1); // Direct Dynamic
/* --- COMPUTE JK --- */
JK = task.jacobianSOUT.accessCopy();
/***/ sotCOUNTER(2, 3); // compute JK
/* Init memory. */
MemoryTaskSOT *mem = getMemory(taskA, dim, nbJoints);
/***/ sotCOUNTER(1, 2); // first allocs
Matrix::Index rankJ = -1;
taskVectorToMlVector(taskA.taskSOUT(iterTime), mem->err);
if (fullPostureTask) {
/***/ sotCOUNTER(2, 3); // compute JK*S
/***/ sotCOUNTER(3, 4); // compute Jt
// Jp = kernel.transpose()
rankJ = kernel.cols();
/***/ sotCOUNTER(4, 5); // SVD and rank
/* --- COMPUTE QDOT AND P --- */
if (!controlIsZero)
mem->err.noalias() -= control.tail(nbJoints - 6);
mem->tmpVar.head(rankJ).noalias() =
kernel.transpose().rightCols(nbJoints - 6) * mem->err;
control.noalias() += kernel * mem->tmpVar.head(rankJ);
controlIsZero = false;
} else {
assert(taskA.jacobianSOUT.accessCopy().cols() == nbJoints);
/* --- COMPUTE S --- */
computeJacobianActivated(dynamic_cast<Task *>(&task), JK, iterTime);
/***/ sotCOUNTER(3, 4); // compute JK*S
/* --- COMPUTE S * JK --- */
const Matrix &JK =
computeJacobianActivated(&taskA, task, mem->JK, iterTime);
/***/ sotCOUNTER(2, 3); // compute JK*S
/* --- COMPUTE Jt --- */
if (PrevProj != NULL)
Jt.noalias() = JK * (*PrevProj);
else {
Jt = JK;
}
/***/ sotCOUNTER(4, 5); // compute Jt
/* --- PINV --- */
svd.compute(Jt);
Eigen::dampedInverse(svd, Jp, th);
/***/ sotCOUNTER(5, 6); // PINV
sotDEBUG(20) << "V after dampedInverse." << svd.matrixV() << endl;
/* --- RANK --- */
{
rankJ = 0;
while (rankJ < svd.singularValues().size() &&
th < svd.singularValues()[rankJ]) {
++rankJ;
}
const Matrix *Jt = &mem->Jt;
if (has_kernel)
mem->Jt.noalias() = JK * kernel;
else
Jt = &JK;
/***/ sotCOUNTER(3, 4); // compute Jt
/* --- SVD and RANK--- */
SVD_t &svd = mem->svd;
if (last)
svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeThinV);
else
svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeFullV);
svd.setThreshold(th);
rankJ = svd.rank();
/***/ sotCOUNTER(4, 5); // SVD and rank
/* --- COMPUTE QDOT AND P --- */
if (!controlIsZero)
mem->err.noalias() -= JK * control;
updateControl(mem, rankJ, has_kernel, kernel, control);
controlIsZero = false;
if (!last) {
Matrix::Index cols = svd.matrixV().cols() - rankJ;
if (has_kernel)
mem->getKernel(nbJoints, cols).noalias() =
kernel * svd.matrixV().rightCols(cols);
else
mem->getKernel(nbJoints, cols).noalias() =
svd.matrixV().rightCols(cols);
makeMap(kernel, mem->kernel);
has_kernel = true;
}
sotDEBUG(45) << "control" << iterTask << " = " << control << endl;
sotDEBUG(25) << "J" << iterTask << " = " << Jac << endl;
sotDEBUG(25) << "JK" << iterTask << " = " << JK << endl;
sotDEBUG(25) << "Jt" << iterTask << " = " << Jt << endl;
sotDEBUG(15) << "Jp" << iterTask << " = " << Jp << endl;
sotDEBUG(15) << "e" << iterTask << " = " << err << endl;
sotDEBUG(45) << "JJp" << iterTask << " = " << JK * Jp << endl;
// sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
sotDEBUG(45) << "S" << iterTask << " = " << svd.singularValues() << endl;
sotDEBUG(45) << "V" << iterTask << " = " << svd.matrixV() << endl;
sotDEBUG(45) << "U" << iterTask << " = " << svd.matrixU() << endl;
mem->jacobianInvSINOUT = Jp;
mem->jacobianInvSINOUT.setTime(iterTime);
mem->jacobianConstrainedSINOUT = JK;
mem->jacobianConstrainedSINOUT.setTime(iterTime);
mem->jacobianProjectedSINOUT = Jt;
mem->jacobianProjectedSINOUT.setTime(iterTime);
mem->singularBaseImageSINOUT = svd.matrixV().leftCols(rankJ);
mem->singularBaseImageSINOUT.setTime(iterTime);
mem->rankSINOUT = rankJ;
mem->rankSINOUT.setTime(iterTime);
sotDEBUG(25) << "Inverse recomputed." << endl;
} else {
sotDEBUG(2) << "Inverse not recomputed." << endl;
rankJ = mem->rankSINOUT.accessCopy();
Jp = mem->jacobianInvSINOUT.accessCopy();
JK = mem->jacobianConstrainedSINOUT.accessCopy();
Jt = mem->jacobianProjectedSINOUT.accessCopy();
}
/***/ sotCOUNTER(6, 7); // TRACE
/* --- COMPUTE QDOT AND P --- */
/*DEBUG: normally, the first iter (ie the test below)
* is the same than the other, starting with control_0 = q0SIN. */
if (PrevProj == NULL)
control.noalias() += Jp * err;
else
control += *PrevProj * (Jp * (err - JK * control));
/***/ sotCOUNTER(7, 8); // QDOT
/* --- OPTIMAL FORM: To debug. --- */
if (PrevProj == NULL) {
Proj.noalias() = svd.matrixV().rightCols(svd.matrixV().cols() - rankJ);
} else {
Proj.noalias() =
*PrevProj * svd.matrixV().rightCols(svd.matrixV().cols() - rankJ);
}
/* --- OLIVIER START --- */
// Update by Joseph Mirabel to match Eigen API
/***/ sotCOUNTER(5, 6); // QDOT + Projector
sotDEBUG(2) << "Proj non optimal (rankJ= " << rankJ
<< ", iterTask =" << iterTask << ")";
sotDEBUG(20) << "V = " << svd.matrixV();
sotDEBUG(20) << "Jt = " << Jt;
sotDEBUG(20) << "JpxJt = " << Jp * Jt;
sotDEBUG(25) << "Proj-Jp*Jt" << iterTask << " = " << (Proj - Jp * Jt)
<< endl;
/* --- OLIVIER END --- */
sotDEBUG(15) << "q" << iterTask << " = " << control << std::endl;
sotDEBUG(25) << "P" << iterTask << " = " << Proj << endl;
iterTask++;
PrevProj = &Proj;
sotPRINTCOUNTER(1);
sotPRINTCOUNTER(2);
......@@ -545,76 +567,12 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
sotPRINTCOUNTER(4);
sotPRINTCOUNTER(5);
sotPRINTCOUNTER(6);
sotPRINTCOUNTER(7);
sotPRINTCOUNTER(8);
if (last || kernel.cols() == 0)
break;
}
sotCHRONO1;
if (0 != taskGradient) {
const dynamicgraph::Matrix &Jac =
taskGradient->jacobianSOUT.access(iterTime);
const Matrix::Index nJ = Jac.rows();
MemoryTaskSOT *mem =
dynamic_cast<MemoryTaskSOT *>(taskGradient->memoryInternal);
if (NULL == mem) {
if (NULL != taskGradient->memoryInternal) {
delete taskGradient->memoryInternal;
}
mem = new MemoryTaskSOT(taskGradient->getName() + "_memSOT", nJ, mJ);
taskGradient->memoryInternal = mem;
}
taskVectorToMlVector(taskGradient->taskSOUT.access(iterTime), mem->err);
const dynamicgraph::Vector &err = mem->err;
sotDEBUG(45) << "Jac = " << Jac << endl;
/* --- MEMORY INIT --- */
dynamicgraph::Matrix &Jp = mem->Jp;
dynamicgraph::Matrix &PJp = mem->PJp;
dynamicgraph::Matrix &Jt = mem->Jt;