Skip to content

Commit 9353661

Browse files
author
Rohan Budhiraja
committed
Replace jrl-mal and boost::ublas with eigen. Changes in unitTesting.
>[unitTest] test_ptrcast doesn't work anymore. Can't plug MatrixRotation (Eigen::Matrix4d) in dg::Matrix (Eigen::MatrixXd). >[unitTest] matrix-homogeneous (Eigen::Transform<double,3,2>) has different constructors. matrix-homogeneous trivial constructor test out-dated now.
1 parent 4fa17b4 commit 9353661

File tree

14 files changed

+103
-132
lines changed

14 files changed

+103
-132
lines changed

‎include/sot/core/derivator.hh

Lines changed: 74 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -38,69 +38,89 @@
3838
/* STD */
3939
#include <string>
4040

41-
namespace dynamicgraph { namespace sot {
42-
namespace dg = dynamicgraph;
43-
44-
/* --------------------------------------------------------------------- */
45-
/* --- CLASS ----------------------------------------------------------- */
46-
/* --------------------------------------------------------------------- */
47-
48-
template< class T >
49-
class Derivator : public dg::Entity
50-
{
51-
DYNAMIC_GRAPH_ENTITY_DECL();
52-
protected:
53-
T memory;
54-
bool initialized;
55-
double timestep;
56-
static const double TIMESTEP_DEFAULT ; //= 1.;
57-
58-
public: /* --- CONSTRUCTION --- */
59-
60-
static std::string getTypeName( void ) { return "Unknown"; }
61-
62-
Derivator( const std::string& name )
63-
: dg::Entity(name)
64-
,memory(),initialized(false)
65-
,timestep(TIMESTEP_DEFAULT)
66-
,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::sin")
67-
,SOUT( boost::bind(&Derivator<T>::computeDerivation,this,_1,_2),
68-
SIN,"sotDerivator<"+getTypeName()+">("+name+")::output("+getTypeName()+")::sout")
69-
,timestepSIN("sotDerivator<"+getTypeName()+">("+name+")::input(double)::dt")
41+
namespace dynamicgraph {
42+
namespace sot {
43+
namespace dg = dynamicgraph;
44+
45+
/* --------------------------------------------------------------------- */
46+
/* --- CLASS ----------------------------------------------------------- */
47+
/* --------------------------------------------------------------------- */
48+
49+
template< class T >
50+
class Derivator : public dg::Entity
7051
{
71-
signalRegistration( SIN<<SOUT<<timestepSIN );
72-
timestepSIN.setReferenceNonConstant( &timestep );
73-
timestepSIN.setKeepReference(true);
74-
}
75-
76-
virtual ~Derivator( void ) {};
77-
78-
public: /* --- SIGNAL --- */
79-
80-
dg::SignalPtr<T,int> SIN;
81-
dg::SignalTimeDependent<T,int> SOUT;
82-
dg::Signal<double,int> timestepSIN;
83-
84-
protected:
85-
T& computeDerivation( T& res,int time )
86-
{
87-
if(initialized)
88-
{
89-
res = memory; res *= -1;
90-
memory = SIN(time);
91-
res += memory;
92-
if( timestep!=1. ) res *= (1./timestep);
93-
} else {
52+
DYNAMIC_GRAPH_ENTITY_DECL();
53+
protected:
54+
T memory;
55+
bool initialized;
56+
double timestep;
57+
static const double TIMESTEP_DEFAULT ; //= 1.;
58+
59+
public: /* --- CONSTRUCTION --- */
60+
61+
static std::string getTypeName( void ) { return "Unknown"; }
62+
63+
Derivator( const std::string& name )
64+
: dg::Entity(name)
65+
,memory(),initialized(false)
66+
,timestep(TIMESTEP_DEFAULT)
67+
,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::sin")
68+
,SOUT( boost::bind(&Derivator<T>::computeDerivation,this,_1,_2),
69+
SIN,"sotDerivator<"+getTypeName()+">("+name+")::output("+getTypeName()+")::sout")
70+
,timestepSIN("sotDerivator<"+getTypeName()+">("+name+")::input(double)::dt")
71+
{
72+
signalRegistration( SIN<<SOUT<<timestepSIN );
73+
timestepSIN.setReferenceNonConstant( &timestep );
74+
timestepSIN.setKeepReference(true);
75+
}
76+
77+
virtual ~Derivator( void ) {};
78+
79+
public: /* --- SIGNAL --- */
80+
81+
dg::SignalPtr<T,int> SIN;
82+
dg::SignalTimeDependent<T,int> SOUT;
83+
dg::Signal<double,int> timestepSIN;
84+
85+
protected:
86+
T& computeDerivation( T& res,int time )
87+
{
88+
if(initialized)
89+
{
90+
res = memory;
91+
res *= -1;
92+
memory = SIN(time);
93+
res += memory;
94+
if( timestep!=1. ) res *= (1./timestep);
95+
} else {
9496
initialized = true;
9597
memory = SIN(time);
9698
res = memory;
9799
res *= 0;
98100
}
101+
return res;
102+
}
103+
};
104+
//TODO Derivation of unit quaternion?
105+
template<>
106+
VectorQuaternion& Derivator<VectorQuaternion>::computeDerivation(VectorQuaternion& res, int time) {
107+
if(initialized) {
108+
res = memory;
109+
res.coeffs() *= -1;
110+
memory = SIN(time);
111+
res.coeffs() += memory.coeffs();
112+
if( timestep!=1. ) res.coeffs() *= (1./timestep);
113+
} else {
114+
initialized = true;
115+
memory = SIN(time);
116+
res = memory;
117+
res.coeffs() *= 0;
118+
}
99119
return res;
100120
}
101-
};
102121

103-
} /* namespace sot */} /* namespace dynamicgraph */
122+
} /* namespace sot */
123+
} /* namespace dynamicgraph */
104124

105125

106126

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

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,6 @@
2525
#include "sot/core/feature-abstract.hh"
2626
#include <dynamic-graph/signal-ptr.h>
2727
#include <dynamic-graph/signal-time-dependent.h>
28-
2928
/* --------------------------------------------------------------------- */
3029
/* --- API ------------------------------------------------------------- */
3130
/* --------------------------------------------------------------------- */

‎include/sot/core/kalman.hh

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
#include <dynamic-graph/all-signals.h>
3232
#include <dynamic-graph/entity.h>
3333
#include <dynamic-graph/linear-algebra.h>
34+
#include <Eigen/LU>
3435
#include <sot/core/constraint.hh>
3536

3637
/* -------------------------------------------------------------------------- */

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

Lines changed: 0 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -64,40 +64,5 @@ namespace dynamicgraph {
6464
} // namespace sot
6565
} // namespace dynamicgraph
6666

67-
namespace Eigen {
68-
inline std::ostream& operator << (std::ostream &os,
69-
Eigen::AngleAxis<double> inst) {
70-
os << inst.toRotationMatrix() <<std::endl;
71-
return os;
72-
}
73-
74-
inline std::ostream& operator << (std::ostream &os,
75-
Eigen::Transform<double, 3, Eigen::Affine> inst) {
76-
os << inst.matrix() <<std::endl;
77-
return os;
78-
}
79-
80-
//TODO : CHECK TYPE AND WRITE PROPER
81-
inline std::istringstream& operator >> (std::istringstream &iss,
82-
Eigen::Transform<double,3,Eigen::Affine> &inst) {
83-
std::string ss("new test string");
84-
Matrix3d m;
85-
m = AngleAxisd(0.0, Vector3d::UnitZ());
86-
inst = m;
87-
iss >> ss;
88-
return iss;
89-
}
90-
91-
//TODO : CHECK TYPE AND WRITE PROPER
92-
inline std::istringstream& operator >> (std::istringstream &iss,
93-
Eigen::AngleAxis<double> &inst) {
94-
std::string ss("new test string");
95-
Matrix3d m;
96-
m = AngleAxisd(0.0, Vector3d::UnitZ());
97-
inst = m;
98-
iss >> ss;
99-
return iss;
100-
}
101-
}
10267

10368
#endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ namespace Eigen {
3535

3636
void pseudoInverse( dg::Matrix& _inputMatrix,
3737
dg::Matrix& _inverseMatrix,
38-
const float threshold = 1e-6)
38+
const double threshold = 1e-6)
3939
{
4040
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
4141
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
@@ -51,7 +51,7 @@ namespace Eigen {
5151

5252
void dampedInverse( dg::Matrix& _inputMatrix,
5353
dg::Matrix& _inverseMatrix,
54-
const float threshold = 1e-6,
54+
const double threshold = 1e-6,
5555
dg::Matrix* Uref = NULL,
5656
dg::Vector* Sref = NULL,
5757
dg::Matrix* Vref = NULL) {

‎src/feature/feature-posture.cpp

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,8 @@
2020
#include <dynamic-graph/factory.h>
2121
#include <dynamic-graph/pool.h>
2222

23-
#include "sot/core/feature-posture.hh"
24-
23+
#include <sot/core/feature-posture.hh>
24+
#include <boost/numeric/conversion/cast.hpp>
2525
namespace dg = ::dynamicgraph;
2626

2727
using ::dynamicgraph::command::Setter;
@@ -121,19 +121,18 @@ namespace dynamicgraph {
121121
{
122122
dg::Vector state = state_.accessCopy();
123123
dg::Vector posture = posture_.accessCopy ();
124-
int dim = state.size();
124+
unsigned int dim = state.size();
125125

126126
if (dim != posture.size ()) {
127127
throw std::runtime_error
128128
("Posture and State should have same dimension.");
129129
}
130-
131130
// If activeDof_ vector not initialized, initialize it
132131
if (activeDofs_.size () != dim) {
133132
activeDofs_ = std::vector <bool> (dim, false);
134133
nbActiveDofs_ = 0;
135134
}
136-
135+
137136
// Check that selected dof id is valid
138137
if ((dofId < 6) || (dofId >= dim))
139138
{

‎src/matrix/operator.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
#include <dynamic-graph/linear-algebra.h>
3333
#include <sot/core/factory.hh>
3434
#include <sot/core/debug.hh>
35-
35+
#include <boost/numeric/conversion/cast.hpp>
3636
#include <deque>
3737

3838
namespace dg = ::dynamicgraph;
@@ -814,7 +814,7 @@ namespace dynamicgraph {
814814
void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Matrix& m2,dynamicgraph::Vector& res )
815815
{
816816
memory.push_front( v1 );
817-
while( memory.size()>m2.cols() ) memory.pop_back();
817+
while( memory.size() > m2.cols() ) memory.pop_back();
818818
convolution( memory,m2,res );
819819
}
820820
};

‎src/signal/signal-cast.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
1919
*/
2020

21+
#include <sot/core/matrix-geometry.hh>
2122
#include <dynamic-graph/signal-caster.h>
2223
#include <dynamic-graph/eigen-io.h>
2324
#include <Eigen/Core>
@@ -26,7 +27,6 @@
2627

2728
#include <sot/core/feature-abstract.hh>
2829
#include <sot/core/trajectory.hh>
29-
#include <sot/core/matrix-geometry.hh>
3030
#include <sot/core/flags.hh>
3131
#include <sot/core/multi-bound.hh>
3232
#include <dynamic-graph/signal-caster.h>

‎src/sot/sot.cpp

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -331,15 +331,15 @@ computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
331331
dynamicgraph::Matrix& Jff,
332332
dynamicgraph::Matrix& Jact )
333333
{
334-
const unsigned int nJ = Jac.rows();
335-
const unsigned int mJ = K.cols();
336-
const unsigned int nbConstraints = Jac.cols() - mJ;
334+
const int nJ = Jac.rows();
335+
const int mJ = K.cols();
336+
const int nbConstraints = Jac.cols() - mJ;
337337

338338
if (nbConstraints == 0) {
339339
JK = Jac;
340340
return JK;
341341
}
342-
for( unsigned int i=0;i<nJ;++i )
342+
for( int i=0;i<nJ;++i )
343343
{
344344
for( int j=0;j<nbConstraints;++j ) Jff(i,j)=Jac(i,j);
345345
for( int j=nbConstraints;j<Jac.cols();++j )
@@ -663,13 +663,13 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
663663
vtmp1 = MRAWDATA(V);
664664
/***/sotCOUNTER(6,7); // Ppre
665665

666-
for( unsigned int i=0;i<mJ;++i )
666+
for( int i=0;i<mJ;++i )
667667
{
668668
vtmp2 = MRAWDATA(V);
669-
for( unsigned int j=0;j<mJ;++j )
669+
for( int j=0;j<mJ;++j )
670670
{
671671
v1 = vtmp1; v2 =vtmp2;
672-
for( unsigned int k=0;k<rankJ;++k )
672+
for(unsigned int k=0;k<rankJ;++k )
673673
//for( unsigned int k=0;k<mJ;++k )
674674
{
675675
(*p) -=( *v1) * (*v2);
@@ -744,7 +744,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
744744

745745
/* --- COMPUTE ERR --- */
746746
dynamicgraph::Vector Herr( err.size() );
747-
for( unsigned int i=0;i<err.size(); ++i )
747+
for( int i=0;i<err.size(); ++i )
748748
{
749749
Herr(i) = err(i);
750750
}

‎src/traces/reader.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ getNextData( dynamicgraph::Vector& res, const unsigned int time )
140140

141141
res.resize(dim);
142142
int cursor=0;
143-
for( int i=0;i<curr.size();++i )
143+
for(unsigned int i=0;i<curr.size();++i )
144144
if( selection(i) ) res(cursor++)=curr[i];
145145

146146
sotDEBUGOUT(15);

0 commit comments

Comments
 (0)