Unverified Commit 835c9b74 authored by olivier stasse's avatar olivier stasse Committed by GitHub
Browse files

Merge pull request #51 from jmirabel/master

Fix Timer class
parents 136552fd 1c46ea84
......@@ -72,7 +72,6 @@ namespace dynamicgraph {
explicit FeaturePosture (const std::string& name);
virtual ~FeaturePosture ();
virtual unsigned int& getDimension( unsigned int& res,int );
void setPosture (const dg::Vector& posture);
void selectDof (unsigned dofId, bool control);
protected:
......@@ -80,9 +79,11 @@ namespace dynamicgraph {
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:
......
......@@ -35,64 +35,22 @@ namespace Eigen {
void pseudoInverse( dg::Matrix& _inputMatrix,
dg::Matrix& _inverseMatrix,
const double threshold = 1e-6) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
for ( long i=0; i<m_singularValues.size(); ++i) {
if ( m_singularValues(i) > threshold )
singularValues_inv(i)=1.0/m_singularValues(i);
else singularValues_inv(i)=0;
}
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
}
const double threshold = 1e-6);
void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
dg::Matrix& _inverseMatrix,
const double threshold = 1e-6) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas (svd.singularValues());
SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
_inverseMatrix.noalias() =
( svd.matrixV() * sv_inv.asDiagonal() * svd.matrixU().transpose());
}
const double threshold = 1e-6);
void dampedInverse( const dg::Matrix& _inputMatrix,
dg::Matrix& _inverseMatrix,
dg::Matrix& Uref,
dg::Vector& Sref,
dg::Matrix& Vref,
const double threshold = 1e-6) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse (svd, _inverseMatrix, threshold);
Uref = svd.matrixU();
Vref = svd.matrixV();
Sref = svd.singularValues();
sotDEBUGOUT(15);
}
const double threshold = 1e-6);
void dampedInverse( const dg::Matrix& _inputMatrix,
dg::Matrix& _inverseMatrix,
const double threshold = 1e-6) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse (svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
const double threshold = 1e-6);
}
......
......@@ -38,6 +38,7 @@ namespace dynamicgraph {
{
public:// protected:
/* Internal memory to reduce the dynamic allocation at task resolution. */
dg::Vector err;
dg::Matrix Jt; //( nJ,mJ );
dg::Matrix Jp;
dg::Matrix PJp;
......@@ -46,7 +47,7 @@ namespace dynamicgraph {
dg::Matrix Jact; //( nJ,mJ ); // Activated part
dg::Matrix JK; //(nJ,mJ);
dg::Matrix V;
dg::Matrix Proj;
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
SVD_t svd;
......@@ -54,12 +55,12 @@ namespace dynamicgraph {
public:
/* mJ is the number of actuated joints, nJ the number of feature in the task,
* and ffsize the number of unactuated DOF. */
MemoryTaskSOT( const std::string & name,const unsigned int nJ=0,
const unsigned int mJ=0,const unsigned int ffsize =0 );
MemoryTaskSOT( const std::string & name,const Matrix::Index nJ=0,
const Matrix::Index mJ=0,const Matrix::Index ffsize =0 );
virtual void initMemory( const unsigned int nJ,
const unsigned int mJ,
const unsigned int ffsize,
virtual void initMemory( const Matrix::Index nJ,
const Matrix::Index mJ,
const Matrix::Index ffsize,
bool atConstruction = false);
public: /* --- ENTITY INHERITANCE --- */
......
......@@ -112,8 +112,6 @@ namespace dynamicgraph {
/*! \brief Store a pointer to compute the gradient */
TaskAbstract* taskGradient;
/*! Projection used to compute the control law. */
dg::Matrix Proj;
//Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
/*! Force the recomputation at each step. */
bool recomputeEachTime;
......@@ -131,13 +129,11 @@ namespace dynamicgraph {
static dg::Matrix & computeJacobianConstrained( const dg::Matrix& Jac,
const dg::Matrix& K,
dg::Matrix& JK,
dg::Matrix& Jff,
dg::Matrix& Jact );
dg::Matrix& JK);
static dg::Matrix & computeJacobianConstrained( const TaskAbstract& task,
const dg::Matrix& K );
static dg::Vector
taskVectorToMlVector(const VectorMultiBound& taskVector);
static void
taskVectorToMlVector(const VectorMultiBound& taskVector, Vector& err);
public:
......
......@@ -71,6 +71,7 @@ class Timer_EXPORT Timer
protected:
struct timeval t0,t1;
clock_t c0, c1;
double dt;
public:
......@@ -87,6 +88,7 @@ class Timer_EXPORT Timer
dg::SignalPtr<T,int> sigSIN;
dg::SignalTimeDependent<T,int> sigSOUT;
dg::SignalTimeDependent<T,int> sigClockSOUT;
dg::Signal<double,int> timerSOUT;
......@@ -96,20 +98,33 @@ class Timer_EXPORT Timer
sigSIN = &sig; dt=0.;
}
template <bool UseClock>
T& compute( T& t,const int& time )
{
sotDEBUGIN(15);
gettimeofday(&t0,NULL);
sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
if (UseClock) {
c0 = clock();
sotDEBUG(15) << "t0: "<< c0 << std::endl;
} else {
gettimeofday(&t0,NULL);
sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
}
t = sigSIN( time );
gettimeofday(&t1,NULL);
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.
+ (t1.tv_usec-t0.tv_usec+0.) / 1000. );
sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
if (UseClock) {
c1 = clock();
sotDEBUG(15) << "t1: "<< c0 << std::endl;
dt = ((double)(c1 - c0) * 1000 ) / CLOCKS_PER_SEC;
} else {
gettimeofday(&t1,NULL);
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.
+ (t1.tv_usec-t0.tv_usec+0.) / 1000. );
sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
}
timerSOUT = dt;
timerSOUT.setTime (time);
sotDEBUGOUT(15);
return t;
......@@ -145,18 +160,20 @@ template< class T >
Timer<T>::
Timer( const std::string& name )
:Entity(name)
,t0(),t1()
,dt(0.)
,sigSIN( NULL,"Timer("+name+")::output(T)::sin" )
,sigSOUT( boost::bind(&Timer::compute,this,_1,_2),
,sigSIN( NULL,"Timer("+name+")::input(T)::sin" )
,sigSOUT( boost::bind(&Timer::compute<false>,this,_1,_2),
sigSIN,
"Timer("+name+")::output(T)::sout" )
,sigClockSOUT( boost::bind(&Timer::compute<true>,this,_1,_2),
sigSIN,
"Timer("+name+")::output(T)::clockSout" )
,timerSOUT( "Timer("+name+")::output(double)::timer" )
{
sotDEBUGIN(15);
timerSOUT.setFunction( boost::bind(&Timer::getDt,this,_1,_2) );
signalRegistration( sigSIN<<sigSOUT<<timerSOUT );
signalRegistration( sigSIN<<sigSOUT<<sigClockSOUT<<timerSOUT );
sotDEBUGOUT(15);
}
......
......@@ -152,6 +152,8 @@ SET(${LIBRARY_NAME}_SOURCES
tools/periodic-call
tools/device
tools/trajectory
matrix/matrix-svd
)
ADD_LIBRARY(${LIBRARY_NAME}
......
......@@ -53,11 +53,12 @@ namespace dynamicgraph {
: FeatureAbstract(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)
{
signalRegistration (state_ << posture_);
signalRegistration (state_ << posture_ << postureDot_);
errorSOUT.addDependency (state_);
......@@ -91,8 +92,8 @@ namespace dynamicgraph {
dg::Vector& FeaturePosture::computeError( dg::Vector& res, int t)
{
dg::Vector state = state_.access (t);
dg::Vector posture = posture_.access (t);
const dg::Vector& state = state_.access (t);
const dg::Vector& posture = posture_.access (t);
res.resize (nbActiveDofs_);
std::size_t index=0;
......@@ -116,14 +117,29 @@ namespace dynamicgraph {
return res;
}
dg::Vector& FeaturePosture::computeErrorDot( dg::Vector& res, int t)
{
const Vector& postureDot = postureDot_.access (t);
res.resize (nbActiveDofs_);
std::size_t index=0;
for (std::size_t i=0; i<activeDofs_.size (); ++i) {
if (activeDofs_ [i]) {
res (index) = postureDot (i);
index ++;
}
}
return res;
}
void
FeaturePosture::selectDof (unsigned dofId, bool control)
{
dg::Vector state = state_.accessCopy();
dg::Vector posture = posture_.accessCopy ();
unsigned int dim = state.size();
const Vector& state = state_.accessCopy();
const Vector& posture = posture_.accessCopy ();
std::size_t dim (state.size());
if (dim != posture.size ()) {
if (dim != (std::size_t)posture.size ()) {
throw std::runtime_error
("Posture and State should have same dimension.");
}
......
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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/>.
#include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh>
namespace Eigen {
void pseudoInverse( dg::Matrix& _inputMatrix,
dg::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;
singularValues_inv.resizeLike(m_singularValues);
for ( long i=0; i<m_singularValues.size(); ++i) {
if ( m_singularValues(i) > threshold )
singularValues_inv(i)=1.0/m_singularValues(i);
else singularValues_inv(i)=0;
}
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
}
void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
dg::Matrix& _inverseMatrix,
const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas (svd.singularValues());
SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
const dg::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,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
JacobiSVD<dg::Matrix> 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,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
dampedInverse (svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
}
......@@ -30,9 +30,9 @@ const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT";
MemoryTaskSOT::
MemoryTaskSOT( const std::string & name
,const unsigned int nJ
,const unsigned int mJ
,const unsigned int ffsize )
,const Matrix::Index nJ
,const Matrix::Index mJ
,const Matrix::Index ffsize )
:
Entity( name )
,jacobianInvSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::Jinv" )
......@@ -49,7 +49,7 @@ MemoryTaskSOT( const std::string & name
void MemoryTaskSOT::
initMemory( const unsigned int nJ,const unsigned int mJ,const unsigned int ffsize,
initMemory( const Matrix::Index nJ,const Matrix::Index mJ,const Matrix::Index ffsize,
bool atConstruction )
{
sotDEBUG(15) << "Task-mermory " << getName() << ": resize "
......@@ -63,9 +63,8 @@ initMemory( const unsigned int nJ,const unsigned int mJ,const unsigned int ffsiz
Jact.resize( nJ,mJ );
JK.resize( nJ,mJ );
V.resize( mJ,mJ );
svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeThinV);
svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
JK.fill(0);
if (atConstruction) {
......@@ -75,7 +74,6 @@ initMemory( const unsigned int nJ,const unsigned int mJ,const unsigned int ffsiz
Jff.setZero();
Jact.setZero();
JK.setZero();
V.setZero();
} else {
Eigen::pseudoInverse(Jt,Jp);
}
......
......@@ -674,7 +674,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
{
TaskAbstract & task = **(stack.begin());
const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
const dynamicgraph::Vector err = Sot::taskVectorToMlVector(task.taskSOUT(iterTime));
const dynamicgraph::Vector err;
Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
const unsigned int mJ = Jac.rows();
/***/sotCOUNTER(0,1); // Direct Dynamic
......@@ -765,7 +766,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
TaskAbstract & task = **iter;
const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
const dynamicgraph::Vector err = Sot::taskVectorToMlVector(task.taskSOUT(iterTime));
const dynamicgraph::Vector err;
Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
const unsigned int mJ = Jac.rows();
/***/sotCOUNTER(0,1); // Direct Dynamic
......@@ -875,7 +877,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
TaskAbstract & task = **iter;
MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
const dynamicgraph::Matrix &Jac = mem->JK;
const dynamicgraph::Vector &err = Sot::taskVectorToMlVector(task.taskSOUT.accessCopy());
const dynamicgraph::Vector err;
Sot::taskVectorToMlVector(task.taskSOUT.accessCopy(), err);
dynamicgraph::Vector diffErr(err.size());
diffErr = Jac*control;
......
......@@ -334,13 +334,11 @@ defineNbDof( const unsigned int& nbDof )
dynamicgraph::Matrix & Sot::
computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
const dynamicgraph::Matrix& K,
dynamicgraph::Matrix& JK,
dynamicgraph::Matrix& Jff,
dynamicgraph::Matrix& Jact )
dynamicgraph::Matrix& JK)
{
const int nJ = Jac.rows();
const int mJ = K.cols();
const int nbConstraints = Jac.cols() - mJ;
const Matrix::Index nJ = Jac.rows();
const Matrix::Index mJ = K.cols();
const Matrix::Index nbConstraints = Jac.cols() - mJ;
if (nbConstraints == 0) {
JK = Jac;
......@@ -361,10 +359,8 @@ computeJacobianConstrained( const TaskAbstract& task,
const dynamicgraph::Matrix &Jac = task.jacobianSOUT.accessCopy ();
MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
if( NULL==mem ) throw; // TODO
dynamicgraph::Matrix &Jff = mem->Jff;
dynamicgraph::Matrix &Jact = mem->Jact;
dynamicgraph::Matrix &JK = mem->JK;
return computeJacobianConstrained(Jac,K,JK,Jff,Jact);
return computeJacobianConstrained(Jac,K,JK);
}
static void computeJacobianActivated( Task* taskSpec,
......@@ -423,15 +419,15 @@ static void computeJacobianActivated( Task* taskSpec,
# define sotSTART_CHRONO1 gettimeofday(&t0,NULL)
# define sotCHRONO1 \
gettimeofday(&t1,NULL);\
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.* 1000.\
+ (t1.tv_usec-t0.tv_usec+0.) );\
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)
# define sotCOUNTER(nbc1,nbc2) \
gettimeofday(&tpart##nbc2,NULL); \
dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
+ (tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+0.) )
dt##nbc2 += ( (double)(tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
+ (double)(tpart##nbc2.tv_usec-tpart##nbc1.tv_usec) )
# define sotINITCOUNTER(nbc1) \
struct timeval tpart##nbc1; double dt##nbc1=0;
# define sotPRINTCOUNTER(nbc1) sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
......@@ -446,17 +442,16 @@ static void computeJacobianActivated( Task* taskSpec,
# define sotPRINTCOUNTER(nbc1)
#endif // #ifdef WITH_CHRONO
dynamicgraph::Vector Sot::
taskVectorToMlVector( const VectorMultiBound& taskVector )
void Sot::
taskVectorToMlVector( const VectorMultiBound& taskVector, Vector& res )
{
dynamicgraph::Vector res(taskVector.size()); res.setZero();
res.resize(taskVector.size());
unsigned int i=0;
for( VectorMultiBound::const_iterator iter=taskVector.begin();
iter!=taskVector.end();++iter,++i ) {
res(i)=iter->getSingleBound();
}
return res;
}
dynamicgraph::Vector& Sot::
......@@ -472,8 +467,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
sotSTARTPARTCOUNTERS;
const double &th = inversionThresholdSIN(iterTime);
const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
const int mJ = K.cols(); // number dofs - number constraints
const Matrix &K = constraintSOUT(iterTime);
const Matrix::Index mJ = K.cols(); // number dofs - number constraints
try {
control = q0SIN( iterTime );
......@@ -489,17 +484,17 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
sotDEBUGF(5, " --- Time %d -------------------", iterTime );
unsigned int iterTask = 0;
const Matrix* PrevProj = NULL;
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);
const dynamicgraph::Vector err = taskVectorToMlVector(task.taskSOUT(iterTime));
sotCOUNTER(0,1); // Direct Dynamic
unsigned int rankJ;
const int nJ = Jac.rows(); // number dofs
const Matrix::Index nJ = Jac.rows(); // number dofs
/* Init memory. */
MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
......@@ -510,14 +505,16 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
task.memoryInternal = mem;
}
dynamicgraph::Matrix &Jp = mem->Jp;
dynamicgraph::Matrix &V = mem->V;
dynamicgraph::Matrix &JK = mem->JK;
dynamicgraph::Matrix &Jt = mem->Jt;
Matrix &Jp = mem->Jp;
Matrix &JK = mem->JK;
Matrix &Jt = mem->Jt;
Matrix &Proj = mem->Proj;
MemoryTaskSOT::SVD_t& svd = mem->svd;
taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
const dynamicgraph::Vector &err = mem->err;
Jp.resize( mJ,nJ );
V.resize( mJ,mJ );
Jt.resize( nJ,mJ );
JK.resize( nJ,mJ );
......@@ -554,7 +551,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
/***/sotCOUNTER(2,3); // compute JK
/* --- COMPUTE Jt --- */
if( 0<iterTask ) Jt.noalias() = JK*Proj; else { Jt = JK; }
if( 0<iterTask ) Jt.noalias() = JK*(*PrevProj); else { Jt = JK; }
/***/sotCOUNTER(3,4); // compute Jt
/* --- COMPUTE S --- */
......@@ -564,9 +561,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
/* --- PINV --- */
svd.compute (Jt);
Eigen::dampedInverse (svd, Jp, th);
V.noalias() = svd.matrixV();
/***/sotCOUNTER(5,6); // PINV
sotDEBUG(2) << "V after dampedInverse." << V <<endl;
sotDEBUG(20) << "V after dampedInverse." << svd.matrixV() <<endl;
/* --- RANK --- */
{
rankJ = 0;
......@@ -584,7 +580,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
sotDEBUG(45) << "S"<<iterTask<<" = "<< svd.singularValues()<<endl;
sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
sotDEBUG(45) << "V"<<iterTask<<" = "<< svd.matrixV()<<endl;