add jointPtr->GetLimits

recover joint movement
roboticslab-uc3m · Jun 6, 2017 · e4dd0ee · e4dd0ee
1 parent 3f40266
commit e4dd0ee
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git 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
@@ -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;
 
         };