void
Pr2Device::control(jointMap_t &jm) {
try {
increment(timestep_);
}
catch (...) {}
sotDEBUG (25) << "state = " << state_ << std::endl;
unsigned jointId = 0;
jointMap_t::const_iterator it;
for (it=jm.begin(); it!=jm.end(); ++it, ++jointId) {
if (jointId+6 >= state_.size() || !it->second.second)
continue;
it->second.second->commanded_effort_ = state_(jointId+6);
}
}
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);
}
double HCOD::
computeStepAndUpdate( void )
{
assert(isSolutionCpt);
double tau = 1.0; ConstraintRef cst;
stage_iter_t stageUp;
for( stage_iter_t iter = stages.begin(); iter!=stages.end(); ++iter )
{
sotDEBUG(5) << "Check stage " << (*iter)->name << "." << std::endl;
if(! (*iter)->checkBound( solution,uNext,cst,tau ) )
stageUp=iter;
}
if( tau<1 )
{
update(stageUp,cst);
}
return tau;
}
/* Compute sum(i=1:sr) Ji' li, with Jsr = I and lsr = u, and check
* that the result is null. */
bool HCOD::
testLagrangeMultipliers( Index stageRef,std::ostream* os ) const
{
assert( stageRef<=stages.size() );
VectorXd verifL(sizeProblem);
/* verifL = Jsr' lsr, with Jsr = I and lsr = u. */
if( stageRef==(Index)stages.size() )
{ verifL = solution; }
else
verifL.setZero();
/* verif += sum Ji' li. */
const Index nbstage = std::min((Index)(stages.size()-1),stageRef);
for( Index i=0;i<=nbstage;++i )
{
const Stage & s = *stages[i];
sotDEBUG(5) << "verif = " << (soth::MATLAB)verifL << std::endl;
sotDEBUG(5) << "J = " << (soth::MATLAB)s.Jactive() << std::endl;
sotDEBUG(5) << "l = " << (soth::MATLAB)s.getLagrangeMultipliers() << std::endl;
verifL += s.Jactive().transpose()*s.getLagrangeMultipliers();
sotDEBUG(5) << "verif = " << (soth::MATLAB)verifL << std::endl;
}
sotDEBUG(5) << "verif = " << (soth::MATLAB)verifL << std::endl;
// Damping
if( nbstage>0 && stageRef<(Index)stages.size() )
{
const Stage & s = *stages[nbstage];
if( s.useDamp() )// && s.sizeN()>0 )
{
const Index sm = s.getSizeM();
VectorXd z(sizeProblem); z.head(sm) = Ytu.head(sm);
z.tail(sizeProblem-sm).setZero();
VectorXd Yz(sizeProblem);
Y.multiply(z,Yz);
Yz *= s.damping()*s.damping();
verifL += Yz;
}
}
sotDEBUG(5) << "verif = " << (soth::MATLAB)verifL << std::endl;
const double sumNorm = verifL.norm();
const bool res = sumNorm<1e-6;
if(os&&(!res))
(*os) << "TestLagrangian failed: norm is " << sumNorm << "."<<std::endl;
return res;
}
void PGManager::startSequence( const StepQueue& seq )
{
if(!pgEntity) {
sotERROR <<"PG not set" << std::endl;
return;
}
std::ostringstream cmdstd; cmdstd << ":StartOnLineStepSequencing ";
std::ostringstream os;
for( unsigned int i = 0; i < seq.size(); ++i ) {
const FootPrint& fp = seq.getStep(i);
cmdstd << fp.x << " " << fp.y << " " << fp.theta << " ";
}
std::istringstream cmdArg( cmdstd.str() );
std::istringstream emptyArg;
sotDEBUG(15) << "Cmd: " << cmdstd.str() << std::endl;
}
/* Assume the variable 'Ytu' contains Y^T*u.
* The result is stored in the stages (getLagrangeMultipliers()).
*/
void HCOD::
computeLagrangeMultipliers( const Index & stageRef )
{
assert( isSolutionCpt );
assert( stageRef<=stages.size() );
stage_iter_t iter=stages.begin()+stageRef;
if( iter==stages.end() )
{ rho = - Ytu; }
else
{
(*iter)->computeRho(Ytu,rho,true); // This is Ytrho, not rho.
}
sotDEBUG(5) << "rho = " << (MATLAB)rho << std::endl;
while ( iter!=stages.begin() )
{
iter--;
(*iter)->computeLagrangeMultipliers(rho);
}
}
void compute( const MatrixXd& A, const int rank_=-1, const double EPSILON=1e-8 )
{
NC=A.cols(); NR=A.rows();
#ifdef DEBUG
Ainit=A;
#endif
qrv.compute( A.transpose() );
sotDEBUG(5) << "QR= " <<(MATLAB)qrv.matrixQR() << std::endl;
rank = computeRank(rank_,EPSILON);
L = qrv.matrixQR().topRows(rank).triangularView<Upper>().transpose();
sotDEBUG(5) << "L = " << (MATLAB)L << std::endl;
if( m_computeFullV )
{
V.setIdentity(NC,NC);
V.applyOnTheRight(qrv.householderQ());
sotDEBUG(5) << "V = " << (MATLAB)V << std::endl;
}
else if( m_computeThinV )
{
V.resize(NC,rank); V.setIdentity(NC,rank);
V.applyOnTheLeft(qrv.householderQ());
}
if( m_computeFullU || m_computeThinU )
{
U = qrv.colsPermutation();
sotDEBUG(5) << "Pi = " << (MATLAB)U << std::endl;
}
for( int k=rank;k<NR;++k )
leftGivens(k);
sotDEBUG(5) << "L = " << (MATLAB)L << std::endl;
sotDEBUG(5) << "U = " << (MATLAB)U << std::endl;
}
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;
}
void HCOD::
setInitialActiveSet( const cstref_vector_t& Ir0,Index k )
{
sotDEBUG(5) << "Ir["<<k<<"]"<<std::endl;
stage(k).setInitialActiveSet(Ir0,true);
}
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);
}
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;
}
