From 1a906c9b87152e7184135dae4dd5f1223436ca12 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 20 Jun 2011 17:44:39 +0200
Subject: [PATCH] Minor corrections to account for Eigen3.0.
---
src/dynamic-integrator.cpp | 14 +++++++-------
src/mal-to-eigen.h | 23 ++++++++++++++++++++---
src/pseudo-robot-dynamic.cpp | 4 ++--
src/solver-kine.cpp | 3 +--
src/solver-op-space.cpp | 26 +++++++++++++-------------
5 files changed, 43 insertions(+), 27 deletions(-)
diff --git a/src/dynamic-integrator.cpp b/src/dynamic-integrator.cpp
index 78eb76a..cc08dd0 100644
--- a/src/dynamic-integrator.cpp
+++ b/src/dynamic-integrator.cpp
@@ -314,7 +314,7 @@ namespace dynamicgraph
sotDEBUGIN(15);
/* --- Convert acceleration, velocity and position to amelif style ------- */
- EIGEN_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
+ EIGEN_CONST_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
EIGEN_VECTOR_FROM_SIGNAL( velocity,mlvelocity );
EIGEN_VECTOR_FROM_SIGNAL( position,mlposition );
@@ -323,21 +323,21 @@ namespace dynamicgraph
sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
- VectorBlock< Map<VectorXd> > fftrans = position.head(3);
- VectorBlock< Map<VectorXd> > ffeuler = position.segment(3,3);
+ VectorBlock<SigVectorXd> fftrans = position.head(3);
+ VectorBlock<SigVectorXd> ffeuler = position.segment(3,3);
Matrix3d ffrot = computeRotationMatrixFromEuler(ffeuler);
sotDEBUG(15) << "Rff_start = " << (MATLAB)ffrot << std::endl;
sotDEBUG(15) << "tff_start = " << (MATLAB)fftrans << std::endl;
- const VectorBlock< Map<VectorXd> > ffvtrans = velocity.head(3);
- const VectorBlock< Map<VectorXd> > ffvrot = velocity.segment(3,3);
+ VectorBlock<SigVectorXd> ffvtrans = velocity.head(3);
+ VectorBlock<SigVectorXd> ffvrot = velocity.segment(3,3);
Vector3d v_lin,v_ang;
djj2amelif( v_ang,v_lin,ffvrot,ffvtrans,fftrans,ffrot );
sotDEBUG(15) << "vff_start = " << (MATLAB)v_lin << std::endl;
sotDEBUG(15) << "wff_start = " << (MATLAB)v_ang << std::endl;
- const VectorBlock< Map<VectorXd> > ffatrans = acceleration.head(3);
- const VectorBlock< Map<VectorXd> > ffarot = acceleration.segment(3,3);
+ const VectorBlock<const_SigVectorXd> ffatrans = acceleration.head(3);
+ const VectorBlock<const_SigVectorXd> ffarot = acceleration.segment(3,3);
Vector3d a_lin,a_ang;
djj2amelif( a_ang,a_lin,ffarot,ffatrans,fftrans,ffrot );
sotDEBUG(15) << "alff_start = " << (MATLAB)a_lin << std::endl;
diff --git a/src/mal-to-eigen.h b/src/mal-to-eigen.h
index 29a6545..a21b80e 100644
--- a/src/mal-to-eigen.h
+++ b/src/mal-to-eigen.h
@@ -23,17 +23,34 @@
namespace Eigen
{
typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRXd;
+ typedef Map<MatrixRXd> SigMatrixXd;
+ typedef Map<VectorXd> SigVectorXd;
+ typedef const Map<const MatrixRXd> const_SigMatrixXd;
+ typedef const Map<const VectorXd> const_SigVectorXd;
}
+#define EIGEN_CONST_MATRIX_FROM_SIGNAL(name,signal) \
+ Eigen::const_SigMatrixXd name \
+ ( \
+ signal.accessToMotherLib().data().begin(), \
+ signal.nbRows(), \
+ signal.nbCols() \
+ )
#define EIGEN_MATRIX_FROM_SIGNAL(name,signal) \
- Eigen::Map<Eigen::MatrixRXd> name \
+ Eigen::SigMatrixXd name \
( \
- const_cast<double*>(signal.accessToMotherLib().data().begin()), \
+ signal.accessToMotherLib().data().begin(), \
signal.nbRows(), \
signal.nbCols() \
)
+#define EIGEN_CONST_VECTOR_FROM_SIGNAL(name,signal) \
+ Eigen::const_SigVectorXd name \
+ ( \
+ signal.accessToMotherLib().data().begin(), \
+ signal.size() \
+ )
#define EIGEN_VECTOR_FROM_SIGNAL(name,signal) \
- Eigen::Map<Eigen::VectorXd> name \
+ Eigen::SigVectorXd name \
( \
signal.accessToMotherLib().data().begin(), \
signal.size() \
diff --git a/src/pseudo-robot-dynamic.cpp b/src/pseudo-robot-dynamic.cpp
index a982074..863a965 100644
--- a/src/pseudo-robot-dynamic.cpp
+++ b/src/pseudo-robot-dynamic.cpp
@@ -96,7 +96,7 @@ namespace dynamicgraph
controlSIN(time);
integrateFromSignals(time);
- EIGEN_VECTOR_FROM_SIGNAL(v,velocity );
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(v,velocity );
EIGEN_VECTOR_FROM_VECTOR( qdot,mlqdot,v.size()-6 );
qdot = v.tail(v.size()-6);
@@ -243,7 +243,7 @@ namespace dynamicgraph
using namespace Eigen;
using PseudoRobotDynamic_Static::computeEulerFromRotationMatrix;
- EIGEN_MATRIX_FROM_SIGNAL(M,mlM);
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(M,mlM);
Vector3d r = computeEulerFromRotationMatrix( M.topLeftCorner(3,3) );
EIGEN_VECTOR_FROM_SIGNAL( q,position );
if( q.size()<6 )
diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index 059d9ab..c1d98d3 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -225,7 +225,7 @@ namespace dynamicgraph
MatrixXd & Ctask = Ctasks[i];
VectorBound & btask = btasks[i];
- EIGEN_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
const int nx = ddx.size();
@@ -241,7 +241,6 @@ namespace dynamicgraph
sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
}
-
/* --- */
sotDEBUG(1) << "Initial config." << std::endl;
hsolver->reset();
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index 447480c..d8c8b33 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.cpp
@@ -282,7 +282,7 @@ namespace dynamicgraph
template< typename D1, typename D2 >
void computeForceNormalConversion( Eigen::MatrixBase<D1> & Ci,
- Eigen::MatrixBase<D2> & positions )
+ const Eigen::MatrixBase<D2> & positions )
{
/* General Constraint is: phi^0 = Psi.fi, with Psi = [ I; skew(OP1);
skew(OP2); skew(OP3); skew(OP4) ]. But phi^0 = X_c^0*phi^c (both feet
@@ -446,9 +446,9 @@ namespace dynamicgraph
// if( t==1112 ) { hsolver->debugOnce(); }
- EIGEN_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
- EIGEN_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
- EIGEN_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
using namespace sotOPH;
using namespace soth;
@@ -500,7 +500,7 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact & contact = pContact.second;
- EIGEN_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
const int ri = contact.range.first;
Cdyn.COLS_F.COLS(ri,ri+6) = Jc.transpose();
}
@@ -520,20 +520,20 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second; const int n6 = nci*6;
- EIGEN_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
Ccontact.COLS_Q.ROWS(n6,n6+6) = Jc;
VectorXd reference = VectorXd::Zero(6);
if( (*contact.JdotSIN) )
{
sotDEBUG(5) << "Accounting for Jcontact_dot. " << std::endl;
- EIGEN_MATRIX_FROM_SIGNAL(Jcdot,(*contact.JdotSIN)(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(Jcdot,(*contact.JdotSIN)(t));
reference -= Jcdot*dq;
}
if( (*contact.correctorSIN) )
{
sotDEBUG(5) << "Accounting for contact_xddot. " << std::endl;
- EIGEN_VECTOR_FROM_SIGNAL(xdd,(*contact.correctorSIN)(t));
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(xdd,(*contact.correctorSIN)(t));
reference += xdd;
}
for( int r=0;r<6;++r ) bcontact[n6+r] = reference[r];
@@ -551,7 +551,7 @@ namespace dynamicgraph
BOOST_FOREACH(const contacts_t::value_type& pContact, contactMap)
{
const Contact & contact = pContact.second;
- EIGEN_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
const int nbP = support.cols();
const int ri = contact.range.first, rs=contact.range.second;
@@ -585,8 +585,8 @@ namespace dynamicgraph
MatrixXd & Ctask1 = Ctasks[i];
VectorBound & btask1 = btasks[i];
- EIGEN_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
- EIGEN_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
const int nx1 = ddx.size();
@@ -645,8 +645,8 @@ namespace dynamicgraph
const double Kv = breakFactorSIN(t);
if( postureSIN && positionSIN )
{
- EIGEN_VECTOR_FROM_SIGNAL(qref,postureSIN(t));
- EIGEN_VECTOR_FROM_SIGNAL(q,positionSIN(t));
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(qref,postureSIN(t));
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(q,positionSIN(t));
const double Kp = .25*Kv*Kv;
ref = (-Kp*(q-qref)-Kv*dq).tail(nbDofs);
}
--
GitLab