Skip to content
Snippets Groups Projects
Commit 369934fa authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Improve CM buffer print

parent 34ad5584
No related branches found
No related tags found
No related merge requests found
...@@ -849,24 +849,21 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) ...@@ -849,24 +849,21 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl; cout << endl;
} }
if (C->isMotion() && metric) if (C->isMotion() )
{ {
try CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
cout << " buffer size : " << CM->getBuffer().get().size() << endl;
if ( metric && ! CM->getBuffer().get().empty())
{ {
CaptureMotionPtr CM = std::static_pointer_cast<CaptureMotion>(C); cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
cout << " buffer size : " << CM->getBuffer().get().size() << endl; if (CM->hasCalibration())
if ( CM->getCalibSize() > 0 && ! CM->getBuffer().get().empty())
{ {
cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl; cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl; cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl; cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl; cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
} }
} }
catch (std::runtime_error& e)
{
}
} }
if (depth >= 3) if (depth >= 3)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment