From e4dd0eea8807e672869673ba80563cab883f0897 Mon Sep 17 00:00:00 2001
From: jgvictores <jcgvicto@gmail.com>
Date: Tue, 6 Jun 2017 15:37:03 +0200
Subject: [PATCH] add jointPtr->GetLimits

recover joint movement
---
 .../YarpOpenraveControlboard/DeviceDriverImpl.cpp   | 13 +++++++++++++
 .../YarpOpenraveControlboard.hpp                    |  2 +-
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp
index 2a9d66fd..c0162116 100644
--- a/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp
+++ b/yarpplugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp
@@ -34,6 +34,19 @@ bool roboticslab::YarpOpenraveControlboard::open(yarp::os::Searchable& config) {
 
     axes = manipulatorIDs.size();
 
+    for(int i=0; i<axes; i++)
+    {
+        OpenRAVE::RobotBase::JointPtr jointPtr = probot->GetJointFromDOFIndex(manipulatorIDs[i]);
+
+        std::vector<OpenRAVE::dReal> vLowerLimit;
+        std::vector<OpenRAVE::dReal> vUpperLimit;
+        jointPtr->GetLimits(vLowerLimit,vUpperLimit);
+
+        CD_INFO("Limits %d: [%f,%f]\n",i,vLowerLimit[0]*180.0/M_PI,vUpperLimit[0]*180.0/M_PI);
+
+        vectorOfJointPtr.push_back(jointPtr);
+    }
+
     return true;
 }
 
diff --git a/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp b/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp
index f15ccd8d..6156e4c6 100644
--- a/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp
+++ b/yarpplugins/YarpOpenraveControlboard/YarpOpenraveControlboard.hpp
@@ -773,7 +773,7 @@ class YarpOpenraveControlboard : public yarp::dev::DeviceDriver, public yarp::de
                 OpenRAVE::RobotBasePtr probot;
                 std::vector< int > manipulatorIDs;
                 std::vector<OpenRAVE::dReal> dEncRaw;
-
+                std::vector<OpenRAVE::RobotBase::JointPtr> vectorOfJointPtr;
 
         };