Added output for debugging. Realized we are working with Quaternios :O

roboticslab-uc3m · Jan 11, 2018 · 083c555 · 083c555
1 parent a5ede71
commit 083c555
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/libraries/OpenravePlugins/OpenraveYarpForceEstimator/OpenraveYarpForceEstimator.cpp b/libraries/OpenravePlugins/OpenraveYarpForceEstimator/OpenraveYarpForceEstimator.cpp
@@ -210,10 +210,13 @@ void TeoSimRateThread::run() {
     double T_base_object_ox = T_base_object.rot.x;
     double T_base_object_oy = T_base_object.rot.y;
     double T_base_object_oz = T_base_object.rot.z;
+    double T_base_object_ow = T_base_object.rot.w;
+
+    cout<<"Orientacion X es: "<<T_base_object_ox<<endl;
+    cout<<"Orientacion Y es: "<<T_base_object_oy<<endl;
+    cout<<"Orientacion Z es: "<<T_base_object_oz<<endl;
+    cout<<"Valor W es: "<<T_base_object_ow<<endl;
 
-    //CD_INFO("%d",T_base_object_ox);
-    //CD_INFO("%d",T_base_object_oy);
-    //CD_INFO("%d",T_base_object_oz);
 
     //Update psqIroned to the new values
     for(int i=0; i<(sqIroned.size()); i++)