From 83c68fc388e4893737a5d15a8c449e61691a9fa5 Mon Sep 17 00:00:00 2001
From: jgvictores <jcgvicto@gmail.com>
Date: Sat, 9 Sep 2023 18:26:00 +0200
Subject: [PATCH] access obj pos

---
 .../OpenraveYarpIroning.cpp                   | 73 ++++---------------
 1 file changed, 13 insertions(+), 60 deletions(-)

diff --git a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp
index fbd1e9a7..63d4a3ff 100644
--- a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp
+++ b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp
@@ -197,64 +197,20 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase,
         tool.rot = ee.rot;
         OpenRAVE::Transform tcp = ee * tool;
         //Transform tcp = ee;
-        yCInfo(ORYPS) << "TCP at" << tcp.trans.x << tcp.trans.y << tcp.trans.z;
+        //yCInfo(ORYPS) << "TCP at" << tcp.trans.x << tcp.trans.y << tcp.trans.z;
 
-        // Get new object (pen) position
-        /*T_base_object = _objPtr->GetTransform();
-        double T_base_object_x = T_base_object.trans.x;
-        double T_base_object_y = T_base_object.trans.y;
-        double T_base_object_z = T_base_object.trans.z;*/
-
-        // Create new object in the scene "palete" to change brush colours.
-
-        /*OpenRAVE::Transform pos_palete_magenta = _palete_magenta->GetLink("palete-magenta")->GetGeometry(0)->GetTransform();
-        OpenRAVE::Transform pos_palete_yellow = _palete_yellow->GetLink("palete-yellow")->GetGeometry(0)->GetTransform();
-        OpenRAVE::Transform pos_palete_cyan = _palete_cyan->GetLink("palete-cyan")->GetGeometry(0)->GetTransform();*/
-
-        // std::cout<<"Base x obj : "<<T_base_object_x<<std::endl;
-        // std::cout<<"Base y obj : "<<T_base_object_y<<std::endl;
-        // std::cout<<"Base z obj : "<<T_base_object_z<<std::endl;
-
-        /*double pos_cyan_x = pos_palete_cyan.trans.x;
-        double pos_cyan_y = pos_palete_cyan.trans.y;
-        double pos_cyan_z = pos_palete_cyan.trans.z;
-        double dist_cyan = std::sqrt(std::pow(T_base_object_x - pos_cyan_x, 2) + std::pow(T_base_object_y - pos_cyan_y, 2) + std::pow(T_base_object_z - pos_cyan_z, 2));*/
-
-        // std::cout<<"Pos x obj azul: "<<pos_cyan_x<<std::endl;
-        // std::cout<<"Pos y obj azul: "<<pos_cyan_y<<std::endl;
-        // std::cout<<"Pos z obj azul: "<<pos_cyan_z<<std::endl;
-
-        /*double pos_yellow_x = pos_palete_yellow.trans.x;
-        double pos_yellow_y = pos_palete_yellow.trans.y;
-        double pos_yellow_z = pos_palete_yellow.trans.z;
-        double dist_yellow = std::sqrt(std::pow(T_base_object_x - pos_yellow_x, 2) + std::pow(T_base_object_y - pos_yellow_y, 2) + std::pow(T_base_object_z - pos_yellow_z, 2));*/
-
-        // std::cout<<"Pos x obj yellow: "<<pos_yellow_x<<std::endl;
-        // std::cout<<"Pos y obj verde: "<<pos_yellow_y<<std::endl;
-        // std::cout<<"Pos z obj verde: "<<pos_yellow_z<<std::endl;
-
-        /*double pos_magenta_x = pos_palete_magenta.trans.x;
-        double pos_magenta_y = pos_palete_magenta.trans.y;
-        double pos_magenta_z = pos_palete_magenta.trans.z;
-        double dist_magenta = std::sqrt(std::pow(T_base_object_x - pos_magenta_x, 2) + std::pow(T_base_object_y - pos_magenta_y, 2) + std::pow(T_base_object_z - pos_magenta_z, 2));*/
-
-        // std::cout<<"Pos x obj rojo: "<<pos_magenta_x<<std::endl;
-        // std::cout<<"Pos y obj rojo: "<<pos_magenta_y<<std::endl;
-        // std::cout<<"Pos z obj rojo: "<<pos_magenta_z<<std::endl;
-
-        // std::cout<<"La distancia a azul es: "<<dist_cyan<<std::endl;
-        // std::cout<<"La distancia a verde es: "<<dist_yellow<<std::endl;
-        // std::cout<<"La distancia a rojo es: "<<dist_magenta<<std::endl;
-
-        // Choose the closer one
-        /*if (dist_cyan < dist_magenta && dist_cyan < dist_yellow && dist_cyan < 0.13)
-            brushColour = 1;
-        if (dist_yellow < dist_magenta && dist_yellow < dist_cyan && dist_yellow < 0.13)
-            brushColour = 2;
-        if (dist_magenta < dist_yellow && dist_magenta < dist_cyan && dist_magenta < 0.13)
-            brushColour = 3;
-
-        std::cout << "El color con el que estoy pintando es: " << brushColour << std::endl;*/
+        for(unsigned int objKinBodyIdx = 1; objKinBodyIdx<2; objKinBodyIdx++)
+        {
+            yCInfo(ORYPS) << _objKinBodyPtrs[objKinBodyIdx]->GetName();
+            //yCInfo(ORYPS) << _objKinBodyPtrs[objKinBodyIdx]->GetLinks().size(); // 1
+            OpenRAVE::KinBody::LinkPtr objLinkPtr = _objKinBodyPtrs[objKinBodyIdx]->GetLinks()[0];
+            OpenRAVE::Transform t = objLinkPtr->GetGeometry(0)->GetTransform();
+            yCInfo(ORYPS) << t.trans.x << t.trans.y << t.trans.z << "| TCP" << tcp.trans.x << tcp.trans.y << tcp.trans.z;
+            //double dist = std::sqrt(std::pow(T_base_object.trans.x - tcp.trans.x, 2) + std::pow(T_base_object.trans.y - tcp.trans.y, 2) + std::pow(T_base_object.trans.z - tcp.trans.z, 2));
+
+            //if(objKinBodyIdx == 0)
+            //    yCInfo(ORYPS) << T_base_object.trans.x << T_base_object.trans.y << T_base_object.trans.z << "| TCP" << tcp.trans.x << tcp.trans.y << tcp.trans.z;
+        }
 
         // Update psqpainted to the new values
         /*for (int i = 0; i < (sqPainted.size()); i++)
@@ -323,9 +279,6 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase,
     std::vector<OpenRAVE::KinBodyPtr> _objKinBodyPtrs;
     OpenRAVE::RobotBase::ManipulatorPtr _pRobotManip;
 
-    OpenRAVE::Transform T_base_object;
-    OpenRAVE::KinBodyPtr _objPtr;
-    OpenRAVE::KinBodyPtr _wall;
 };
 
 #if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 105, 0)