From 3a535a5b34763454f256d9f6262f17bd770fdbf5 Mon Sep 17 00:00:00 2001 From: jgvictores Date: Fri, 8 Sep 2023 10:50:01 +0200 Subject: [PATCH] tcp in thread --- .../OpenraveYarpIroning.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp index c91c53cc..fbd1e9a7 100644 --- a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp @@ -135,15 +135,6 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase, probot->SetActiveManipulator("rightArm"); _pRobotManip = probot->GetManipulator("rightArm"); - OpenRAVE::Transform ee = _pRobotManip->GetEndEffector()->GetTransform(); - OpenRAVE::Transform tool; - //tool.trans = Vector(0.0,0.0,1.3); - tool.rot = OpenRAVE::geometry::quatFromAxisAngle(OpenRAVE::Vector(0,0,0)); //-- Converts an axis-angle rotation into a quaternion. - 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; - { // lock the environment! #if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 68, 0) @@ -197,7 +188,16 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase, void run() override { - yCInfo(ORYPS) << "thread"; + //yCInfo(ORYPS) << "thread"; + + OpenRAVE::Transform ee = _pRobotManip->GetEndEffector()->GetTransform(); + OpenRAVE::Transform tool; + //tool.trans = Vector(0.0,0.0,1.3); + tool.rot = OpenRAVE::geometry::quatFromAxisAngle(OpenRAVE::Vector(0,0,0)); //-- Converts an axis-angle rotation into a quaternion. + 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; // Get new object (pen) position /*T_base_object = _objPtr->GetTransform();