From 66a1570e9521a61f68734dc6959e0d3ee97bcf1e Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Wed, 17 Aug 2011 15:47:52 +0200
Subject: [PATCH] Added some traces to check the task validity.
---
src/solver-op-space.cpp | 8 +++++++-
1 file changed, 7 insertions(+), 1 deletion(-)
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index 7eb439a..b96f37a 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;
}
--
GitLab