dg::Matrix& TaskDynJointLimits::
computeTjlJdot( dg::Matrix& Jdot,int time )
{
sotDEBUGIN(15);
const dg::Matrix& currentJ = jacobianSOUT(time);
const double& dt = dtSIN(time);
if( previousJ.rows()!=currentJ.rows() ) previousJset = false;
if( previousJset )
{
assert( currentJ.rows()==previousJ.rows()
&& currentJ.cols()==previousJ.cols() );
Jdot .resize( currentJ.rows(),currentJ.cols() );
Jdot = currentJ - previousJ;
Jdot *= 1/dt;
}
else { Jdot.resize(currentJ.rows(),currentJ.cols() ); Jdot.setZero(); }
previousJ = currentJ;
previousJset = true;
sotDEBUGOUT(15);
return Jdot;
}
void HCOD::
show( std::ostream& os, bool check )
{
sotDEBUGIN(15);
for( stage_sequence_size_t i=0;i<stages.size();++i )
{
stages[i]->show(os,i+1,check);
}
MatrixXd Yex(sizeProblem,sizeProblem); Yex.setIdentity();
Y.applyThisOnTheLeft(Yex);
os<<"Y = " << (MATLAB)Yex << std::endl;
if( isSolutionCpt )
{
os << "u0 = " << (MATLAB)solution << std::endl;
os << "u1 = " << (MATLAB)uNext << std::endl;
os << "Ytu0 = " << (MATLAB)Ytu << std::endl;
os << "Ytu1 = " << (MATLAB)YtuNext << std::endl;
os << "Y.matrixExplicit" << Y.matrixExplicit<< std::endl;
os << "(solution-Y.matrixExplicit*Ytu).norm()=" << (solution-Y.matrixExplicit*Ytu).norm() << std::endl;
assert( (solution-Y.matrixExplicit*Ytu).norm() < Stage::EPSILON );
assert( (uNext-Y.matrixExplicit*YtuNext).norm() < Stage::EPSILON );
}
sotDEBUGOUT(15);
}
void HCOD::
computeSolution( bool compute_u )
{
sotDEBUGIN(5);
assert(isInit);
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
YtuNext.setZero();
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
for( stage_sequence_size_t i=0;i<stages.size();++i )
{
stages[i]->computeSolution(YtuNext);
}
if( compute_u )
{
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
Y.multiply(YtuNext,uNext);
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
sotDEBUG(5) << "u0 = " << (MATLAB)solution << std::endl;
sotDEBUG(5) << "u1 = " << (MATLAB)uNext << std::endl;
}
sotDEBUG(5) << "Ytu0 = " << (MATLAB)Ytu << std::endl;
sotDEBUG(5) << "Ytu1 = " << (MATLAB)YtuNext << std::endl;
isSolutionCpt=true;
sotDEBUGOUT(5);
}
dg::sot::VectorMultiBound& TaskDynJointLimits::
computeTaskDynJointLimits( dg::sot::VectorMultiBound& res,int time )
{
sotDEBUGIN(45);
sotDEBUG(45) << "# In " << getName() << " {" << std::endl;
const dg::Vector & position = positionSIN(time);
sotDEBUG(35) << "position = " << position << std::endl;
const dg::Vector & velocity = velocitySIN(time);
sotDEBUG(35) << "velocity = " << velocity << std::endl;
const dg::Vector & refInf = referenceInfSIN(time);
const dg::Vector & refSup = referenceSupSIN(time);
const double & dt = dtSIN(time);
const double kt=2/(dt*dt);
res.resize(position.size());
for( unsigned int i=0;i<res.size();++i )
{
res[i] = dg::sot::MultiBound (kt*(refInf(i)-position(i)-dt*velocity(i)),
kt*(refSup(i)-position(i)-dt*velocity(i)));
}
sotDEBUG(15) << "taskU = "<< res << std::endl;
sotDEBUGOUT(45);
return res;
}
DynamicRomeo::DynamicRomeo (const std::string & name)
: Dynamic (name, false)
{
sotDEBUGIN(15);
DynamicRomeo::buildModel ();
sotDEBUGOUT(15);
}
/** Compute the interaction matrix from a subset of
* the possible features.
*/
dg::Matrix& FeatureProjectedLine::
computeJacobian( dg::Matrix& J,int time )
{
sotDEBUGIN(15);
const MatrixHomogeneous & A = xaSIN(time), & B = xbSIN(time);
const dg::Vector & C = xcSIN(time);
const double
xa=A(0,3),xb=B(0,3),xc=C(0),
ya=A(1,3),yb=B(1,3),yc=C(1);
const dg::Matrix & JA = JaSIN(time), & JB = JbSIN(time);
const int nq=JA.cols();
assert((int)JB.cols()==nq);
J.resize(1,nq);
for( int i=0;i<nq;++i )
{
const double
& dxa=JA(0,i),& dxb=JB(0,i),
& dya=JA(1,i),& dyb=JB(1,i);
J(0,i) = dxa*(yb-yc) - dxb*(ya-yc) - dya*(xb-xc) + dyb*(xa-xc);
}
sotDEBUGOUT(15);
return J;
}
PGManager::PGManager( const std::string& name )
: Entity(name)
{
sotDEBUGIN(5);
sotDEBUGOUT(5);
}
void NextStep::
stoper( const int & timeCurr )
{
sotDEBUGIN(15);
sotDEBUGOUT(15);
return;
}
NextStep::
~NextStep( void )
{
sotDEBUGIN(5);
sotDEBUGOUT(5);
return;
}
void NextStep::thisIsZero()
{
sotDEBUGIN(15);
rfMref0 = referencePositionRightSIN.accessCopy();
lfMref0 = referencePositionLeftSIN.accessCopy();
sotDEBUGOUT(15);
}
MatrixHomogeneous& NextStepTwoHandObserver::
computeReferencePositionRight( MatrixHomogeneous& res,int timeCurr )
{
sotDEBUGIN(15);
const MatrixHomogeneous& wMsf = rightFootPositionSIN( timeCurr );
sotDEBUGOUT(15);
return computeRefPos( res,timeCurr,wMsf );
}
void HCOD::
setInitialActiveSet( const std::vector<cstref_vector_t> & Ir0)
{
sotDEBUGIN(5);
assert(Ir0.size() == stages.size() );
for( int k=0;k<(int)stages.size();k++ )
setInitialActiveSet(Ir0[k],k);
sotDEBUGOUT(5);
}
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);
}
ml::Vector& NextStepTwoHandObserver::
computeReferenceAcceleration( const ml::Vector& right,
const ml::Vector& left,
ml::Vector& res )
{
sotDEBUGIN(15);
/* TODO */
sotDEBUGOUT(15);
return res;
}
void HCOD::
showActiveSet( std::ostream& os ) const
{
sotDEBUGIN(15);
os << "{" << std::endl;
for( stage_sequence_size_t i=0;i<stages.size();++i )
{
os<< " "; stages[i]->showActiveSet(os); os << std::endl;
}
os << "}" << std::endl;
sotDEBUGOUT(15);
}
/*
Computation of the output velocity based on the input velocity and the
experimentally determined control rule
*/
ml::Vector& VelocityCorrection::
velocityOUTSOUT_function( ml::Vector& res, int time )
{
sotDEBUGIN(15);
const ml::Vector & mlvelocity = velocityINSIN(time);
res.resize( mlvelocity.size() );
for( unsigned int i=0; i<mlvelocity.size(); i++)
res(i) = applyVelocityTransferFunction( mlvelocity, i);
return res;
}
Kalman::Kalman( const std::string& name )
: Entity(name)
,measureSIN (NULL,"Kalman("+name+")::input(vector)::y")
,modelTransitionSIN( NULL,"Kalman("+name+")::input(matrix)::F" )
,modelMeasureSIN( NULL,"Kalman("+name+")::input(matrix)::H" )
,noiseTransitionSIN( NULL,"Kalman("+name+")::input(matrix)::Q" )
,noiseMeasureSIN( NULL,"Kalman("+name+")::input(matrix)::R" )
,statePredictedSIN (0, "Kalman("+name+")::input(vector)::x_pred")
,observationPredictedSIN (0, "Kalman("+name+")::input(vector)::y_pred")
,varianceUpdateSOUT ("Kalman("+name+")::output(vector)::P")
,stateUpdateSOUT ("Kalman("+name+")::output(vector)::x_est"),
stateEstimation_ (),
stateVariance_ ()
{
sotDEBUGIN(15);
varianceUpdateSOUT.setFunction
(boost::bind(&Kalman::computeVarianceUpdate,
this,_1,_2));
stateUpdateSOUT.setFunction (boost::bind(&Kalman::computeStateUpdate,
this, _1, _2));
signalRegistration( measureSIN << observationPredictedSIN
<< modelTransitionSIN
<< modelMeasureSIN << noiseTransitionSIN
<< noiseMeasureSIN << statePredictedSIN
<< stateUpdateSOUT << varianceUpdateSOUT );
std::string docstring =
" Set initial state estimation\n"
"\n"
" input:\n"
" - a vector: initial state\n";
addCommand ("setInitialState",
new Setter <Kalman, Vector> (*this,
&Kalman::setStateEstimation,
docstring));
docstring =
" Set variance of initial state estimation\n"
"\n"
" input:\n"
" - a matrix: variance covariance matrix\n";
addCommand ("setInitialVariance",
new Setter <Kalman, Matrix> (*this,
&Kalman::setStateVariance,
docstring));
sotDEBUGOUT(15);
}
int& NextStep::
triggerCall( int& dummy,int timeCurrent )
{
sotDEBUGIN(45);
switch( state )
{
case STATE_STOPED: break;
case STATE_STARTED:
{
int nextIntoductionTime = timeLastIntroduction+period;
if( nextIntoductionTime<=timeCurrent )
{
nextStep( timeCurrent );
if( NULL!=verbose )
{
FootPrint & lastStep = footPrintList.back();
(*verbose) << "<T=" << timeCurrent << "> Introduced a new step: ";
switch( lastStep.contact )
{
case CONTACT_LEFT_FOOT: (*verbose) << "LF " ; break;
case CONTACT_RIGHT_FOOT: (*verbose) << "RF " ; break;
}
(*verbose) << lastStep.x << "," << lastStep.y << ","
<< lastStep.theta << std::endl;
}
introductionCallBack( timeCurrent );
timeLastIntroduction=timeCurrent;
}
break;
}
case STATE_STARTING:
{
starter( timeCurrent );
break;
}
case STATE_STOPING:
{
stoper( timeCurrent );
break;
}
};
sotDEBUGOUT(45);
return dummy;
}
Matrix & Kalman::
computeVarianceUpdate (Matrix& Pk_k,const int& time)
{
sotDEBUGIN(15);
if (time == 0) {
// First time return variance initial state
Pk_k = stateVariance_;
// Set dependency to input signals for latter computations
varianceUpdateSOUT.addDependency (noiseTransitionSIN);
varianceUpdateSOUT.addDependency (modelTransitionSIN);
} else {
const Matrix &Q = noiseTransitionSIN( time );
const Matrix& R = noiseMeasureSIN (time);
const Matrix &F = modelTransitionSIN( time );
const Matrix& H = modelMeasureSIN (time);
const Matrix &Pk_1_k_1 = stateVariance_;
sotDEBUG(15) << "Q=" << Q << std::endl;
sotDEBUG(15) << "R=" << R << std::endl;
sotDEBUG(15) << "F=" << F << std::endl;
sotDEBUG(15) << "H=" << H << std::endl;
sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
FP_ = F * Pk_1_k_1;
Pk_k_1_ = FP_*F.transpose();
Pk_k_1_ += Q;
sotDEBUG(15) << "F " <<std::endl << F << std::endl;
sotDEBUG(15) << "P_{k-1|k-1} " <<std::endl<< Pk_1_k_1 << std::endl;
sotDEBUG(15) << "F^T " <<std::endl<< F.transpose() << std::endl;
sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
S_ = H * Pk_k_1_ * H.transpose () + R;
K_ = Pk_k_1_ * H.transpose () * S_.inverse ();
Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
sotDEBUG (15) << "S_{k} " << std::endl << S_ << std::endl;
sotDEBUG (15) << "K_{k} " << std::endl << K_ << std::endl;
sotDEBUG (15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
sotDEBUGOUT(15);
stateVariance_ = Pk_k;
}
return Pk_k;
}
/** Compute the error between two visual features from a subset
* a the possible features.
*/
dg::Vector&
FeatureProjectedLine::computeError( dg::Vector& error,int time )
{
sotDEBUGIN(15);
const MatrixHomogeneous & A = xaSIN(time),& B = xbSIN(time);
const dg::Vector & C = xcSIN(time);
const double
xa=A(0,3),xb=B(0,3),xc=C(0),
ya=A(1,3),yb=B(1,3),yc=C(1);
error.resize(1);
error(0) = (xb-xa)*(yc-ya)-(yb-ya)*(xc-xa);
sotDEBUGOUT(15);
return error ;
}
MatrixHomogeneous& NextStepTwoHandObserver::
computeRefPos( MatrixHomogeneous& res,int timeCurr,const MatrixHomogeneous& wMsf )
{
sotDEBUGIN(15);
#define RIGHT_HAND_REFERENCE 1
#if RIGHT_HAND_REFERENCE
const MatrixHomogeneous& wMrh = referencePositionRightSIN( timeCurr );
MatrixHomogeneous sfMw; sfMw = wMsf.inverse();
res = sfMw * wMrh;
#else
const MatrixHomogeneous& wMlh = referencePositionLeftSIN( timeCurr );
const MatrixHomogeneous& wMrh = referencePositionRightSIN( timeCurr );
MatrixHomogeneous sfMw; sfMw = wMsf.inverse();
MatrixHomogeneous sfMlh; sfMlh = sfMw * wMlh;
MatrixHomogeneous sfMrh; sfMrh = sfMw * wMrh;
MatrixRotation R;
VectorRollPitchYaw rpy;
Vector prh(3); prh = sfMrh.translation();
R = sfMrh.linear();
VectorRollPitchYaw rpy_rh; rpy_rh = (R.eulerAngles(2,1,0)).reverse();
Vector plh(3); plh = sfMlh.translation();
R = sfMlh.linear();
VectorRollPitchYaw rpy_lh; rpy_lh = (R.eulerAngles(2,1,0)).reverse();
Vector p = 0.5 * (plh + prh);
rpy = 0.5 * (rpy_rh + rpy_lh);
R = (Eigen::AngleAxisd(rpy(2),Eigen::Vector3d::UnitZ())*
Eigen::AngleAxisd(rpy(1),Eigen::Vector3d::UnitY())*
Eigen::AngleAxisd(rpy(0),Eigen::Vector3d::UnitX())).toRotationMatrix();
res.linear() = R;
res.translation() = p;
#endif
sotDEBUGOUT(15);
return res;
}
void NextStep::
starter( const int & timeCurr )
{
sotDEBUGIN(15);
footPrintList.clear();
FootPrint initSteps[4];
initSteps[0].contact = CONTACT_RIGHT_FOOT;
initSteps[0].x = 0;
initSteps[0].y = - zeroStepPosition/2;
initSteps[0].theta = 0;
initSteps[0].introductionTime = -1;
footPrintList.push_back( initSteps[0] );
introductionCallBack( -1 );
initSteps[1].contact = CONTACT_LEFT_FOOT;
initSteps[1].x = 0;
initSteps[1].y = + zeroStepPosition;
initSteps[1].theta = 0;
initSteps[1].introductionTime = -1;
footPrintList.push_back( initSteps[1] );
introductionCallBack( -1 );
initSteps[2].contact = CONTACT_RIGHT_FOOT;
initSteps[2].x = 0;
initSteps[2].y = - zeroStepPosition;
initSteps[2].theta = 0;
initSteps[2].introductionTime = -1;
footPrintList.push_back( initSteps[2] );
introductionCallBack( -1 );
initSteps[3].contact = CONTACT_LEFT_FOOT;
initSteps[3].x = 0;
initSteps[3].y = + zeroStepPosition;
initSteps[3].theta = 0;
initSteps[3].introductionTime = -1;
footPrintList.push_back( initSteps[3] );
introductionCallBack( -1 );
timeLastIntroduction = timeCurr - period+1;
if( verbose ) (*verbose) << "NextStep started." << std::endl;
sotDEBUGOUT(15);
return;
}
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);
}
Vector& Kalman::
computeStateUpdate (Vector& x_est,const int& time )
{
sotDEBUGIN(15);
if (time == 0) {
// First time return variance initial state
x_est = stateEstimation_;
// Set dependency to input signals for latter computations
stateUpdateSOUT.addDependency (measureSIN);
stateUpdateSOUT.addDependency (observationPredictedSIN);
stateUpdateSOUT.addDependency (modelMeasureSIN);
stateUpdateSOUT.addDependency (noiseTransitionSIN);
stateUpdateSOUT.addDependency (noiseMeasureSIN);
stateUpdateSOUT.addDependency (statePredictedSIN);
stateUpdateSOUT.addDependency (varianceUpdateSOUT);
} else {
varianceUpdateSOUT.recompute (time);
const Vector & x_pred = statePredictedSIN (time);
const Vector & y_pred = observationPredictedSIN (time);
const Vector & y = measureSIN (time);
sotDEBUG(25) << "K_{k} = "<<std::endl<< K_ << std::endl;
sotDEBUG(25) << "y = " << y << std::endl;
sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
sotDEBUG(25) << "y = " << y << std::endl;
// Innovation: z_ = y - Hx
z_ = y - y_pred;
//x_est = x_pred + (K*(y-(H*x_pred)));
x_est = x_pred + K_ * z_;
sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
stateEstimation_ = x_est;
}
sotDEBUGOUT (15);
return x_est;
}
NextStep::
NextStep( const std::string & name )
:Entity(name)
,footPrintList()
,period(PERIOD_DEFAULT)
,timeLastIntroduction( 0 )
,mode(MODE_3D)
,state( STATE_STOPED )
,zeroStepPosition( ZERO_STEP_POSITION_DEFAULT )
,rfMref0()
,lfMref0()
,twoHandObserver( name )
,verbose(0x0)
,referencePositionLeftSIN( NULL,"NextStep("+name+")::input(vector)::posrefleft" )
,referencePositionRightSIN( NULL,"NextStep("+name+")::input(vector)::posrefright" )
,contactFootSIN( NULL,"NextStep("+name+")::input(uint)::contactfoot" )
,triggerSOUT( "NextStep("+name+")::input(dummy)::trigger" )
{
sotDEBUGIN(5);
triggerSOUT.setFunction( boost::bind(&NextStep::triggerCall,this,_1,_2) );
signalRegistration( referencePositionLeftSIN<<referencePositionRightSIN
<<contactFootSIN<<triggerSOUT );
signalRegistration( twoHandObserver.getSignals() );
referencePositionLeftSIN.plug( &twoHandObserver.referencePositionLeftSOUT );
referencePositionRightSIN.plug( &twoHandObserver.referencePositionRightSOUT );
sotDEBUGOUT(5);
}
void HCOD::activeSearch( VectorXd & u )
{
// if( isDebugOnce ) { sotDebugTrace::openFile(); isDebugOnce = false; }
// else { if(sotDEBUGFLOW.outputbuffer.good()) sotDebugTrace::closeFile(); }
//if(sotDEBUGFLOW.outputbuffer.good()) { sotDebugTrace::closeFile();sotDebugTrace::openFile(); }
sotDEBUGIN(15);
/*
* foreach stage: stage.initCOD(Ir_init)
* u = 0
* u0 = solve
* do
* tau,cst_ref = max( violation(stages) )
* u += (1-tau)u0 + tau*u1
* if( tau<1 )
* update(cst_ref); break;
*
* lambda,w = computeLambda
* cst_ref,lmin = min( lambda,w )
* if lmin<0
* downdate( cst_ref )
*
*/
assert(VectorXi::LinSpaced(3,0,2)[0] == 0
&& VectorXi::LinSpaced(3,0,2)[1] == 1
&& VectorXi::LinSpaced(3,0,2)[2] == 2
&& "new version of Eigen might have change the "
"order of arguments in LinSpaced, please correct");
/*struct timeval t0,t1,t2;double time1,time2;
gettimeofday(&t0,NULL);*/
initialize();
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
Y.computeExplicitly(); // TODO: this should be done automatically on Y size.
sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl;
/*gettimeofday(&t1,NULL);
time1 = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);*/
int iter = 0;
startTime=getCPUtime();
Index stageMinimal = 0;
do
{
iter ++; sotDEBUG(5) << " --- *** \t" << iter << "\t***.---" << std::endl;
//if( iter>1 ) { break; }
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
assert( testRecomposition(&std::cerr) );
damp();
computeSolution();
assert( testSolution(&std::cerr) );
double tau = computeStepAndUpdate();
if( tau<1 )
{
sotDEBUG(5) << "Update done, make step <1." << std::endl;
makeStep(tau);
}
else
{
sotDEBUG(5) << "No update, make step ==1." << std::endl;
makeStep();
for( ;stageMinimal<=(Index)stages.size();++stageMinimal )
{
sotDEBUG(5) << "--- Started to examinate stage " << stageMinimal << std::endl;
computeLagrangeMultipliers(stageMinimal);
if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW );
//assert( testLagrangeMultipliers(stageMinimal,std::cerr) );
if( searchAndDowndate(stageMinimal) )
{
sotDEBUG(5) << "Lagrange<0, downdate done." << std::endl;
break;
}
for( Index i=0;i<stageMinimal;++i )
stages[i]->freezeSlacks(false);
if( stageMinimal<nbStages() )
stages[stageMinimal]->freezeSlacks(true);
}
}
lastTime=getCPUtime()-startTime;
lastNumberIterations=iter;
if( lastTime>maxTime ) throw 667;
if( iter>maxNumberIterations ) throw 666;
} while(stageMinimal<=nbStages());
sotDEBUG(5) << "Lagrange>=0, no downdate, active search completed." << std::endl;
/*gettimeofday(&t2,NULL);
time2 = ((t2.tv_sec-t1.tv_sec)+(t2.tv_usec-t1.tv_usec)/1.0e6);
std::ofstream fup("/tmp/haset.dat",std::ios::app);
fup << time1<<"\t"<<time2<<"\t"<<iter<<"\t";*/
u=solution;
sotDEBUG(5) << "uf =" << (MATLAB)u << std::endl;
sotDEBUGOUT(15);
}
void NextStep::
nextStep( const int & timeCurr )
{
sotDEBUGIN(15);
const unsigned& sfoot = contactFootSIN( timeCurr );
const MatrixHomogeneous& wMlf = twoHandObserver.leftFootPositionSIN.access( timeCurr );
const MatrixHomogeneous& wMrf = twoHandObserver.rightFootPositionSIN.access( timeCurr );
// actual and reference position of reference frame in fly foot,
// position of fly foot in support foot.
MatrixHomogeneous ffMref, ffMref0;
MatrixHomogeneous sfMff;
if( sfoot != 1 ) // --- left foot support ---
{
ffMref = referencePositionRightSIN.access( timeCurr );
ffMref0 = rfMref0;
MatrixHomogeneous sfMw; sfMw = wMlf.inverse(); sfMff = sfMw*wMrf;
}
else // -- right foot support ---
{
ffMref = referencePositionLeftSIN.access( timeCurr );
ffMref0 = lfMref0;
MatrixHomogeneous sfMw; sfMw = wMrf.inverse(); sfMff = sfMw*wMlf;
}
// homogeneous transform from ref position of ref frame to
// actual position of ref frame.
MatrixHomogeneous ref0Mff; ref0Mff = ffMref0.inverse();
MatrixHomogeneous ref0Mref; ref0Mref = ref0Mff * ffMref;
// extract the translation part and express it in the support
// foot frame.
MatrixHomogeneous sfMref0; sfMref0 = sfMff * ffMref0;
Vector t_ref0(3); t_ref0 = ref0Mref.translation();
MatrixRotation sfRref0; sfRref0 = sfMref0.linear();
Vector t_sf = sfRref0 * t_ref0;
// add it to the position of the fly foot in support foot to
// get the new position of fly foot in support foot.
Vector pff_sf(3); pff_sf = sfMff.translation();
t_sf += pff_sf;
// compute the rotation that transforms ref0 into ref,
// express it in the support foot frame. Then get the
// associated yaw (rot around z).
MatrixRotation ref0Rsf; ref0Rsf = sfRref0.transpose();
MatrixRotation ref0Rref; ref0Rref = ref0Mref.linear();
MatrixRotation tmp = ref0Rref * ref0Rsf;
MatrixRotation Rref = sfRref0*tmp;
VectorRollPitchYaw rpy; rpy = (Rref.eulerAngles(2,1,0)).reverse();
// get the yaw of the current orientation of the ff wrt sf.
// Add it to the previously computed rpy.
MatrixRotation sfRff; sfRff = sfMff.linear();
VectorRollPitchYaw rpy_ff; rpy_ff = (sfRff.eulerAngles(2,1,0)).reverse();
rpy += rpy_ff;
// Now we can compute and insert the new step (we just need
// to express the coordinates of the vector that joins the
// support foot to the new fly foot in the coordinate frame of the
// new fly foot).
//
// [dX;dY] = A^t [X;Y]
//
// where A is the planar rotation matrix of angle theta, [X;Y]
// is the planar column-vector joining the support foot to the new fly foot,
// expressed in the support foot frame, and [dX;dY] is this same planar
// column-vector expressed in the coordinates frame of the new fly foot.
//
// See the technical report of Olivier Stasse for more details,
// on top of page 79.
double ns_x = 0, ns_y = 0, ns_theta = 0;
if(mode != MODE_1D) {
ns_theta = rpy(2) * 180 / 3.14159265;
if(fabs(ns_theta) < 10) {
ns_theta = 0;
rpy(2) = 0;
}
double x = t_sf(0);
double y = t_sf(1);
double ctheta = cos(rpy(2));
double stheta = sin(rpy(2));
ns_x = x * ctheta + y * stheta;
ns_y = -x * stheta + y * ctheta;
ns_theta = rpy(2) * 180 / 3.14159265;
if(fabs(ns_theta) < 10){ ns_theta = 0; }
}
else {
ns_x = t_sf(0);
if(sfoot != 1){ ns_y = -ZERO_STEP_POSITION_DEFAULT; }
else{ ns_y = ZERO_STEP_POSITION_DEFAULT; }
ns_theta = 0.;
}
FootPrint newStep;
if(sfoot != 1){ newStep.contact = CONTACT_LEFT_FOOT; }
else{ newStep.contact = CONTACT_RIGHT_FOOT; }
newStep.x = ns_x;
newStep.y = ns_y;
newStep.theta = ns_theta;
newStep.introductionTime = timeCurr;
footPrintList.push_back( newStep );
footPrintList.pop_front();
sotDEBUGOUT(15);
}