Commit 1a906c9b authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

Minor corrections to account for Eigen3.0.

parent 611409a4
......@@ -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;
......
......@@ -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() \
......
......@@ -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 )
......
......@@ -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();
......
......@@ -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);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment