diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp index 7eb439a5a55419e1ab531bbe666f4c8f45cb852e..b96f37a37d104568184b2b88656fb315d6aa857b 100644 --- a/src/solver-op-space.cpp +++ b/src/solver-op-space.cpp @@ -716,7 +716,13 @@ namespace dynamicgraph sotDEBUG(1) << "Vdyn = " << (MATLAB)(VectorXd)(Cdyn*solution) << std::endl; sotDEBUG(1) << "Vcontact = " << (MATLAB)(VectorXd)(Ccontact*solution) << std::endl; sotDEBUG(1) << "Vzmp = " << (MATLAB)(VectorXd)(Czmp*solution) << std::endl; - + for( int i=0;i<(int)stack.size();++i ) + { + MatrixXd & Ctask1 = Ctasks[i]; + VectorBound & btask1 = btasks[i]; + sotDEBUG(1) << "Vtask"<<i<<" = " << (MATLAB)(VectorXd)(Ctask1*solution) << std::endl; + sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl; + } sotDEBUG(1) << "control = " << control << std::endl; return control; }