Skip to content

Commit 835c9b7

Browse files
author
Olivier Stasse
authored
Merge pull request #51 from jmirabel/master
Fix Timer class
2 parents 136552f + 1c46ea8 commit 835c9b7

15 files changed

+218
-143
lines changed

‎include/sot/core/feature-posture.hh

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,17 +72,18 @@ namespace dynamicgraph {
7272
explicit FeaturePosture (const std::string& name);
7373
virtual ~FeaturePosture ();
7474
virtual unsigned int& getDimension( unsigned int& res,int );
75-
void setPosture (const dg::Vector& posture);
7675
void selectDof (unsigned dofId, bool control);
7776

7877
protected:
7978

8079
virtual dg::Vector& computeError( dg::Vector& res, int );
8180
virtual dg::Matrix& computeJacobian( dg::Matrix& res, int );
8281
virtual dg::Vector& computeActivation( dg::Vector& res, int );
82+
virtual dg::Vector& computeErrorDot (dg::Vector& res,int time);
8383

8484
signalIn_t state_;
8585
signalIn_t posture_;
86+
signalIn_t postureDot_;
8687
signalOut_t error_;
8788
dg::Matrix jacobian_;
8889
private:

‎include/sot/core/matrix-svd.hh

Lines changed: 4 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -35,64 +35,22 @@ namespace Eigen {
3535

3636
void pseudoInverse( dg::Matrix& _inputMatrix,
3737
dg::Matrix& _inverseMatrix,
38-
const double threshold = 1e-6) {
39-
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
40-
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
41-
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
42-
singularValues_inv.resizeLike(m_singularValues);
43-
for ( long i=0; i<m_singularValues.size(); ++i) {
44-
if ( m_singularValues(i) > threshold )
45-
singularValues_inv(i)=1.0/m_singularValues(i);
46-
else singularValues_inv(i)=0;
47-
}
48-
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
49-
}
38+
const double threshold = 1e-6);
5039

5140
void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
5241
dg::Matrix& _inverseMatrix,
53-
const double threshold = 1e-6) {
54-
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
55-
ArrayWrapper<const SV_t> sigmas (svd.singularValues());
56-
57-
SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
58-
59-
_inverseMatrix.noalias() =
60-
( svd.matrixV() * sv_inv.asDiagonal() * svd.matrixU().transpose());
61-
}
42+
const double threshold = 1e-6);
6243

6344
void dampedInverse( const dg::Matrix& _inputMatrix,
6445
dg::Matrix& _inverseMatrix,
6546
dg::Matrix& Uref,
6647
dg::Vector& Sref,
6748
dg::Matrix& Vref,
68-
const double threshold = 1e-6) {
69-
sotDEBUGIN(15);
70-
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
71-
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
72-
73-
dampedInverse (svd, _inverseMatrix, threshold);
74-
75-
Uref = svd.matrixU();
76-
Vref = svd.matrixV();
77-
Sref = svd.singularValues();
78-
79-
sotDEBUGOUT(15);
80-
}
49+
const double threshold = 1e-6);
8150

8251
void dampedInverse( const dg::Matrix& _inputMatrix,
8352
dg::Matrix& _inverseMatrix,
84-
const double threshold = 1e-6) {
85-
sotDEBUGIN(15);
86-
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
87-
88-
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
89-
dampedInverse (svd, _inverseMatrix, threshold);
90-
91-
sotDEBUGOUT(15);
92-
}
93-
94-
95-
53+
const double threshold = 1e-6);
9654

9755
}
9856

‎include/sot/core/memory-task-sot.hh

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ namespace dynamicgraph {
3838
{
3939
public:// protected:
4040
/* Internal memory to reduce the dynamic allocation at task resolution. */
41+
dg::Vector err;
4142
dg::Matrix Jt; //( nJ,mJ );
4243
dg::Matrix Jp;
4344
dg::Matrix PJp;
@@ -46,20 +47,20 @@ namespace dynamicgraph {
4647
dg::Matrix Jact; //( nJ,mJ ); // Activated part
4748
dg::Matrix JK; //(nJ,mJ);
4849

49-
dg::Matrix V;
50+
dg::Matrix Proj;
5051

5152
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
5253
SVD_t svd;
5354

5455
public:
5556
/* mJ is the number of actuated joints, nJ the number of feature in the task,
5657
* and ffsize the number of unactuated DOF. */
57-
MemoryTaskSOT( const std::string & name,const unsigned int nJ=0,
58-
const unsigned int mJ=0,const unsigned int ffsize =0 );
58+
MemoryTaskSOT( const std::string & name,const Matrix::Index nJ=0,
59+
const Matrix::Index mJ=0,const Matrix::Index ffsize =0 );
5960

60-
virtual void initMemory( const unsigned int nJ,
61-
const unsigned int mJ,
62-
const unsigned int ffsize,
61+
virtual void initMemory( const Matrix::Index nJ,
62+
const Matrix::Index mJ,
63+
const Matrix::Index ffsize,
6364
bool atConstruction = false);
6465

6566
public: /* --- ENTITY INHERITANCE --- */

‎include/sot/core/sot.hh

Lines changed: 3 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -112,8 +112,6 @@ namespace dynamicgraph {
112112
/*! \brief Store a pointer to compute the gradient */
113113
TaskAbstract* taskGradient;
114114

115-
/*! Projection used to compute the control law. */
116-
dg::Matrix Proj;
117115
//Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
118116
/*! Force the recomputation at each step. */
119117
bool recomputeEachTime;
@@ -131,13 +129,11 @@ namespace dynamicgraph {
131129

132130
static dg::Matrix & computeJacobianConstrained( const dg::Matrix& Jac,
133131
const dg::Matrix& K,
134-
dg::Matrix& JK,
135-
dg::Matrix& Jff,
136-
dg::Matrix& Jact );
132+
dg::Matrix& JK);
137133
static dg::Matrix & computeJacobianConstrained( const TaskAbstract& task,
138134
const dg::Matrix& K );
139-
static dg::Vector
140-
taskVectorToMlVector(const VectorMultiBound& taskVector);
135+
static void
136+
taskVectorToMlVector(const VectorMultiBound& taskVector, Vector& err);
141137

142138
public:
143139

‎include/sot/core/timer.hh

Lines changed: 27 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,7 @@ class Timer_EXPORT Timer
7171
protected:
7272

7373
struct timeval t0,t1;
74+
clock_t c0, c1;
7475
double dt;
7576

7677
public:
@@ -87,6 +88,7 @@ class Timer_EXPORT Timer
8788

8889
dg::SignalPtr<T,int> sigSIN;
8990
dg::SignalTimeDependent<T,int> sigSOUT;
91+
dg::SignalTimeDependent<T,int> sigClockSOUT;
9092
dg::Signal<double,int> timerSOUT;
9193

9294

@@ -96,20 +98,33 @@ class Timer_EXPORT Timer
9698
sigSIN = &sig; dt=0.;
9799
}
98100

101+
template <bool UseClock>
99102
T& compute( T& t,const int& time )
100103
{
101104
sotDEBUGIN(15);
102-
gettimeofday(&t0,NULL);
103-
sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
105+
if (UseClock) {
106+
c0 = clock();
107+
sotDEBUG(15) << "t0: "<< c0 << std::endl;
108+
} else {
109+
gettimeofday(&t0,NULL);
110+
sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
111+
}
104112

105113
t = sigSIN( time );
106114

107-
gettimeofday(&t1,NULL);
108-
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.
109-
+ (t1.tv_usec-t0.tv_usec+0.) / 1000. );
110-
sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
115+
if (UseClock) {
116+
c1 = clock();
117+
sotDEBUG(15) << "t1: "<< c0 << std::endl;
118+
dt = ((double)(c1 - c0) * 1000 ) / CLOCKS_PER_SEC;
119+
} else {
120+
gettimeofday(&t1,NULL);
121+
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.
122+
+ (t1.tv_usec-t0.tv_usec+0.) / 1000. );
123+
sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
124+
}
111125

112126
timerSOUT = dt;
127+
timerSOUT.setTime (time);
113128

114129
sotDEBUGOUT(15);
115130
return t;
@@ -145,18 +160,20 @@ template< class T >
145160
Timer<T>::
146161
Timer( const std::string& name )
147162
:Entity(name)
148-
,t0(),t1()
149163
,dt(0.)
150-
,sigSIN( NULL,"Timer("+name+")::output(T)::sin" )
151-
,sigSOUT( boost::bind(&Timer::compute,this,_1,_2),
164+
,sigSIN( NULL,"Timer("+name+")::input(T)::sin" )
165+
,sigSOUT( boost::bind(&Timer::compute<false>,this,_1,_2),
152166
sigSIN,
153167
"Timer("+name+")::output(T)::sout" )
168+
,sigClockSOUT( boost::bind(&Timer::compute<true>,this,_1,_2),
169+
sigSIN,
170+
"Timer("+name+")::output(T)::clockSout" )
154171
,timerSOUT( "Timer("+name+")::output(double)::timer" )
155172
{
156173
sotDEBUGIN(15);
157174
timerSOUT.setFunction( boost::bind(&Timer::getDt,this,_1,_2) );
158175

159-
signalRegistration( sigSIN<<sigSOUT<<timerSOUT );
176+
signalRegistration( sigSIN<<sigSOUT<<sigClockSOUT<<timerSOUT );
160177
sotDEBUGOUT(15);
161178
}
162179

‎src/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,8 @@ SET(${LIBRARY_NAME}_SOURCES
152152
tools/periodic-call
153153
tools/device
154154
tools/trajectory
155+
156+
matrix/matrix-svd
155157
)
156158

157159
ADD_LIBRARY(${LIBRARY_NAME}

‎src/feature/feature-posture.cpp

Lines changed: 23 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -53,11 +53,12 @@ namespace dynamicgraph {
5353
: FeatureAbstract(name),
5454
state_(NULL, "FeaturePosture("+name+")::input(Vector)::state"),
5555
posture_(0, "FeaturePosture("+name+")::input(Vector)::posture"),
56+
postureDot_(0, "FeaturePosture("+name+")::input(Vector)::postureDot"),
5657
jacobian_(),
5758
activeDofs_ (),
5859
nbActiveDofs_ (0)
5960
{
60-
signalRegistration (state_ << posture_);
61+
signalRegistration (state_ << posture_ << postureDot_);
6162

6263
errorSOUT.addDependency (state_);
6364

@@ -91,8 +92,8 @@ namespace dynamicgraph {
9192

9293
dg::Vector& FeaturePosture::computeError( dg::Vector& res, int t)
9394
{
94-
dg::Vector state = state_.access (t);
95-
dg::Vector posture = posture_.access (t);
95+
const dg::Vector& state = state_.access (t);
96+
const dg::Vector& posture = posture_.access (t);
9697

9798
res.resize (nbActiveDofs_);
9899
std::size_t index=0;
@@ -116,14 +117,29 @@ namespace dynamicgraph {
116117
return res;
117118
}
118119

120+
dg::Vector& FeaturePosture::computeErrorDot( dg::Vector& res, int t)
121+
{
122+
const Vector& postureDot = postureDot_.access (t);
123+
124+
res.resize (nbActiveDofs_);
125+
std::size_t index=0;
126+
for (std::size_t i=0; i<activeDofs_.size (); ++i) {
127+
if (activeDofs_ [i]) {
128+
res (index) = postureDot (i);
129+
index ++;
130+
}
131+
}
132+
return res;
133+
}
134+
119135
void
120136
FeaturePosture::selectDof (unsigned dofId, bool control)
121137
{
122-
dg::Vector state = state_.accessCopy();
123-
dg::Vector posture = posture_.accessCopy ();
124-
unsigned int dim = state.size();
138+
const Vector& state = state_.accessCopy();
139+
const Vector& posture = posture_.accessCopy ();
140+
std::size_t dim (state.size());
125141

126-
if (dim != posture.size ()) {
142+
if (dim != (std::size_t)posture.size ()) {
127143
throw std::runtime_error
128144
("Posture and State should have same dimension.");
129145
}

‎src/matrix/matrix-svd.cpp

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
// Copyright (c) 2018, Joseph Mirabel
2+
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3+
//
4+
// This file is part of sot-core.
5+
// sot-core is free software: you can redistribute it
6+
// and/or modify it under the terms of the GNU Lesser General Public
7+
// License as published by the Free Software Foundation, either version
8+
// 3 of the License, or (at your option) any later version.
9+
//
10+
// sot-core is distributed in the hope that it will be
11+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13+
// General Lesser Public License for more details. You should have
14+
// received a copy of the GNU Lesser General Public License along with
15+
// sot-core. If not, see <http://www.gnu.org/licenses/>.
16+
17+
18+
#include <sot/core/debug.hh>
19+
#include <sot/core/matrix-svd.hh>
20+
21+
namespace Eigen {
22+
23+
void pseudoInverse( dg::Matrix& _inputMatrix,
24+
dg::Matrix& _inverseMatrix,
25+
const double threshold) {
26+
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
27+
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
28+
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
29+
singularValues_inv.resizeLike(m_singularValues);
30+
for ( long i=0; i<m_singularValues.size(); ++i) {
31+
if ( m_singularValues(i) > threshold )
32+
singularValues_inv(i)=1.0/m_singularValues(i);
33+
else singularValues_inv(i)=0;
34+
}
35+
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
36+
}
37+
38+
void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
39+
dg::Matrix& _inverseMatrix,
40+
const double threshold) {
41+
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
42+
ArrayWrapper<const SV_t> sigmas (svd.singularValues());
43+
44+
SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
45+
const dg::Matrix::Index m = sv_inv.size();
46+
47+
_inverseMatrix.noalias() =
48+
( svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * svd.matrixU().leftCols(m).transpose());
49+
}
50+
51+
void dampedInverse( const dg::Matrix& _inputMatrix,
52+
dg::Matrix& _inverseMatrix,
53+
dg::Matrix& Uref,
54+
dg::Vector& Sref,
55+
dg::Matrix& Vref,
56+
const double threshold) {
57+
sotDEBUGIN(15);
58+
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
59+
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
60+
61+
dampedInverse (svd, _inverseMatrix, threshold);
62+
63+
Uref = svd.matrixU();
64+
Vref = svd.matrixV();
65+
Sref = svd.singularValues();
66+
67+
sotDEBUGOUT(15);
68+
}
69+
70+
void dampedInverse( const dg::Matrix& _inputMatrix,
71+
dg::Matrix& _inverseMatrix,
72+
const double threshold) {
73+
sotDEBUGIN(15);
74+
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
75+
76+
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
77+
dampedInverse (svd, _inverseMatrix, threshold);
78+
79+
sotDEBUGOUT(15);
80+
}
81+
82+
}

0 commit comments

Comments
 (0)