further name undos

include/openrave/kinbody.h

@@ -1366,7 +1366,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
 public:
         std::string _grabbedname; ///< the name of the body to grab
         std::string _robotlinkname;  ///< the name of the body link that is grabbing the body
-        Transform _trelative; ///< transform of first link of body relative to _bodylinkName's transform. In other words, grabbed->GetTransform() == bodylink->GetTransform()*trelative
+        Transform _trelative; ///< transform of first link of body relative to _robotlinkname's transform. In other words, grabbed->GetTransform() == bodylink->GetTransform()*trelative
         std::set<int> _setRobotLinksToIgnore; ///< links of the body to force ignoring because of pre-existing collions at the time of grabbing. Note that this changes depending on the configuration of the body and the relative position of the grabbed body.
     };
     typedef boost::shared_ptr<GrabbedInfo> GrabbedInfoPtr;

include/openrave/robot.h

@@ -847,19 +847,32 @@ class OPENRAVE_API RobotBase : public KinBody
         @{
      */
 
-    /** \brief Grabs the body with the active manipulator's end effector.
+    /** \brief Grab the body with the specified link.
 
         \param[in] body the body to be grabbed
+        \param[in] pRobotLinkToGrabWith the link of this robot that will perform the grab
         \param[in] setRobotLinksToIgnore Additional robot link indices that collision checker ignore
         when checking collisions between the grabbed body and the robot.
-        \return true if successful and body is grabbed
+        \return true if successful and body is grabbed.
      */
-    virtual bool Grab(KinBodyPtr body, const std::set<int>& setRobotLinksToIgnore);
+    virtual bool Grab(KinBodyPtr body, LinkPtr pRobotLinkToGrabWith, const std::set<int>& setRobotLinksToIgnore);
+
+    /** \brief Grab a body with the specified link.
 
+        \param[in] body the body to be grabbed
+        \param[in] pRobotLinkToGrabWith the link of this robot that will perform the grab
+        \return true if successful and body is grabbed/
+     */
     virtual bool Grab(KinBodyPtr body, LinkPtr pRobotLinkToGrabWith);
 
-    virtual bool Grab(KinBodyPtr pbody, LinkPtr pRobotLinkToGrabWith,
-                      const std::set<int>& setRobotLinksToIgnore);
+    /** \brief Grabs the body with the active manipulator's end effector.
+
+        \param[in] body the body to be grabbed
+        \param[in] setRobotLinksToIgnore Additional robot link indices that collision checker ignore
+        when checking collisions between the grabbed body and the robot.
+        \return true if successful and body is grabbed
+     */
+    virtual bool Grab(KinBodyPtr body, const std::set<int>& setRobotLinksToIgnore);
 
     /** \brief Grabs the body with the active manipulator's end effector.
 

python/bindings/openravepy_kinbody.cpp

@@ -2899,13 +2899,13 @@ class GrabbedInfo_pickle_suite : public pickle_suite
 public:
     static boost::python::tuple getstate(const PyKinBody::PyGrabbedInfo& r)
     {
-        return boost::python::make_tuple(r._grabbedname, r._bodylinkName, r._trelative, r._setBodyLinksToIgnore);
+        return boost::python::make_tuple(r._grabbedname, r._robotlinkname, r._trelative, r._setRobotLinksToIgnore);
     }
     static void setstate(PyKinBody::PyGrabbedInfo& r, boost::python::tuple state) {
         r._grabbedname = state[0];
-        r._bodylinkName = state[1];
+        r._robotlinkname = state[1];
         r._trelative = state[2];
-        r._setBodyLinksToIgnore = state[3];
+        r._setRobotLinksToIgnore = state[3];
     }
 };
 
@@ -3072,9 +3072,9 @@ void init_openravepy_kinbody()
 
     object grabbedinfo = class_<PyKinBody::PyGrabbedInfo, boost::shared_ptr<PyKinBody::PyGrabbedInfo> >("GrabbedInfo", DOXY_CLASS(KinBody::GrabbedInfo))
                          .def_readwrite("_grabbedname",&PyKinBody::PyGrabbedInfo::_grabbedname)
-                         .def_readwrite("_bodylinkName",&PyKinBody::PyGrabbedInfo::_bodylinkName)
+                         .def_readwrite("_robotlinkname",&PyKinBody::PyGrabbedInfo::_robotlinkname)
                          .def_readwrite("_trelative",&PyKinBody::PyGrabbedInfo::_trelative)
-                         .def_readwrite("_setBodyLinksToIgnore",&PyKinBody::PyGrabbedInfo::_setBodyLinksToIgnore)
+                         .def_readwrite("_setRobotLinksToIgnore",&PyKinBody::PyGrabbedInfo::_setRobotLinksToIgnore)
                          .def_pickle(GrabbedInfo_pickle_suite())
     ;
 

python/bindings/openravepy_kinbody.h

@@ -95,32 +95,32 @@ class PyKinBody : public PyInterfaceBase
     }
     PyGrabbedInfo(const RobotBase::GrabbedInfo& info) {
         _grabbedname = ConvertStringToUnicode(info._grabbedname);
-        _bodylinkName = ConvertStringToUnicode(info._robotlinkname);
+        _robotlinkname = ConvertStringToUnicode(info._robotlinkname);
         _trelative = ReturnTransform(info._trelative);
         boost::python::list setRobotLinksToIgnore;
         FOREACHC(itindex, info._setRobotLinksToIgnore) {
             setRobotLinksToIgnore.append(*itindex);
         }
-        _setBodyLinksToIgnore = setRobotLinksToIgnore;
+        _setRobotLinksToIgnore = setRobotLinksToIgnore;
     }
 
     RobotBase::GrabbedInfoPtr GetGrabbedInfo() const
     {
         RobotBase::GrabbedInfoPtr pinfo(new RobotBase::GrabbedInfo());
         pinfo->_grabbedname = boost::python::extract<std::string>(_grabbedname);
-        pinfo->_robotlinkname = boost::python::extract<std::string>(_bodylinkName);
+        pinfo->_robotlinkname = boost::python::extract<std::string>(_robotlinkname);
         pinfo->_trelative = ExtractTransform(_trelative);
-        std::vector<int> v = ExtractArray<int>(_setBodyLinksToIgnore);
+        std::vector<int> v = ExtractArray<int>(_setRobotLinksToIgnore);
         pinfo->_setRobotLinksToIgnore.clear();
         FOREACHC(it,v) {
             pinfo->_setRobotLinksToIgnore.insert(*it);
         }
         return pinfo;
     }
 
-    object _grabbedname, _bodylinkName;
+    object _grabbedname, _robotlinkname;
     object _trelative;
-    object _setBodyLinksToIgnore;
+    object _setRobotLinksToIgnore;
 };
 typedef boost::shared_ptr<PyGrabbedInfo> PyGrabbedInfoPtr;
 

python/databases/linkstatistics.py

@@ -198,7 +198,7 @@ def generate(self,**kwargs):
     def _GetJointSpheresFromGrabbed(self, grabbedinfo):
         for testgrabbedinfo, testjointspheres in self.grabbedjointspheres:
             if len(testgrabbedinfo) == len(grabbedinfo):
-                if all([(grabbedinfo[i]._grabbedname == testgrabbedinfo[i]._grabbedname and grabbedinfo[i]._bodylinkName == testgrabbedinfo[i]._bodylinkName and sum(abs(grabbedinfo[i]._trelative-testgrabbedinfo[i]._trelative)) <= 1e-7 and grabbedinfo[i]._setBodyLinksToIgnore == testgrabbedinfo[i]._setBodyLinksToIgnore) for i in range(len(grabbedinfo))]):
+                if all([(grabbedinfo[i]._grabbedname == testgrabbedinfo[i]._grabbedname and grabbedinfo[i]._robotlinkname == testgrabbedinfo[i]._robotlinkname and sum(abs(grabbedinfo[i]._trelative-testgrabbedinfo[i]._trelative)) <= 1e-7 and grabbedinfo[i]._setRobotLinksToIgnore == testgrabbedinfo[i]._setRobotLinksToIgnore) for i in range(len(grabbedinfo))]):
                     return testjointspheres
 
         log.debug('adding new linkstatistic for grabbed bodies: %r', [g._grabbedname for g in grabbedinfo])

src/libopenrave/kinbody.cpp

@@ -214,7 +214,7 @@ void KinBody::KinBodyStateSaver::_RestoreKinBody(boost::shared_ptr<KinBody> pbod
                         RAVELOG_WARN(str(boost::format("body %s is not similar across environments")%pbody->GetName()));
                     }
                     else {
-                        GrabbedPtr pnewgrabbed(new Grabbed(pnewbody,pbody->GetLinks().at(KinBody::LinkPtr(pgrabbed->_plinkbody)->GetIndex())));
+                        GrabbedPtr pnewgrabbed(new Grabbed(pnewbody,pbody->GetLinks().at(KinBody::LinkPtr(pgrabbed->_plinkrobot)->GetIndex())));
                         pnewgrabbed->_troot = pgrabbed->_troot;
                         pnewgrabbed->_listNonCollidingLinks.clear();
                         FOREACHC(itlinkref, pgrabbed->_listNonCollidingLinks) {
@@ -4433,10 +4433,10 @@ void KinBody::_UpdateGrabbedBodies()
         GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
         KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
         if( !!pbody ) {
-            Transform t = pgrabbed->_plinkbody->GetTransform();
+            Transform t = pgrabbed->_plinkrobot->GetTransform();
             pbody->SetTransform(t * pgrabbed->_troot);
             // set the correct velocity
-            std::pair<Vector, Vector> velocity = pgrabbed->_plinkbody->GetVelocity();
+            std::pair<Vector, Vector> velocity = pgrabbed->_plinkrobot->GetVelocity();
             velocity.first += velocity.second.cross(t.rotate(pgrabbed->_troot.trans));
             pbody->SetVelocity(velocity.first, velocity.second);
             ++itgrabbed;
@@ -4852,16 +4852,16 @@ bool KinBody::Grab(KinBodyPtr pbody, LinkPtr plink)
     velocity.first += velocity.second.cross(tbody.trans - t.trans);
     if( !!pPreviousGrabbed ) {
         dReal disterror = TransformDistance2(t*pPreviousGrabbed->_troot, tbody);
-        if( pPreviousGrabbed->_plinkbody == plink && disterror <= g_fEpsilonLinear ) {
+        if( pPreviousGrabbed->_plinkrobot == plink && disterror <= g_fEpsilonLinear ) {
             // links and transforms are the same, so no worries
             return true;
         }
         RAVELOG_VERBOSE_FORMAT("Body %s: body %s already grabbed, but transforms differ by %f \n", GetName()%pbody->GetName()%disterror);
         _RemoveAttachedBody(pbody);
         CallOnDestruction destructigonhook(boost::bind(&RobotBase::_AttachBody,this,pbody));
-        pPreviousGrabbed->_plinkbody = plink;
+        pPreviousGrabbed->_plinkrobot = plink;
         pPreviousGrabbed->_troot = t.inverse() * tbody;
-        pPreviousGrabbed->_ProcessCollidingLinks(pPreviousGrabbed->_setBodyLinksToIgnore);
+        pPreviousGrabbed->_ProcessCollidingLinks(pPreviousGrabbed->_setRobotLinksToIgnore);
         pbody->SetVelocity(velocity.first, velocity.second);
         return true;
     }
@@ -4991,7 +4991,7 @@ void KinBody::RegrabAll()
         if( !!pbody ) {
             _RemoveAttachedBody(pbody);
             CallOnDestruction destructionhook(boost::bind(&RobotBase::_AttachBody,this,pbody));
-            pgrabbed->_ProcessCollidingLinks(pgrabbed->_setBodyLinksToIgnore);
+            pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
         }
     }
 }
@@ -5006,7 +5006,7 @@ void KinBody::_Regrab(UserDataPtr _pgrabbed)
         CollisionOptionsStateSaver colsaver(collisionchecker,0); // have to reset the collision options
         _RemoveAttachedBody(pgrabbedbody);
         CallOnDestruction destructionhook(boost::bind(&RobotBase::_AttachBody,this,pgrabbedbody));
-        pgrabbed->_ProcessCollidingLinks(pgrabbed->_setBodyLinksToIgnore);
+        pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
     }
 }
 
@@ -5015,7 +5015,7 @@ KinBody::LinkPtr KinBody::IsGrabbing(KinBodyConstPtr pbody) const
     FOREACHC(itgrabbed, _vGrabbedBodies) {
         GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
         if( KinBodyConstPtr(pgrabbed->_pgrabbedbody) == pbody ) {
-            return pgrabbed->_plinkbody;
+            return pgrabbed->_plinkrobot;
         }
     }
     return LinkPtr();
@@ -5040,9 +5040,9 @@ void KinBody::GetGrabbedInfo(std::vector<KinBody::GrabbedInfoPtr>& vgrabbedinfo)
         GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(_vGrabbedBodies[i]);
         vgrabbedinfo[i].reset(new GrabbedInfo());
         vgrabbedinfo[i]->_grabbedname = pgrabbed->_pgrabbedbody.lock()->GetName();
-        vgrabbedinfo[i]->_robotlinkname = pgrabbed->_plinkbody->GetName();
+        vgrabbedinfo[i]->_robotlinkname = pgrabbed->_plinkrobot->GetName();
         vgrabbedinfo[i]->_trelative = pgrabbed->_troot;
-        vgrabbedinfo[i]->_setRobotLinksToIgnore = pgrabbed->_setBodyLinksToIgnore;
+        vgrabbedinfo[i]->_setRobotLinksToIgnore = pgrabbed->_setRobotLinksToIgnore;
         FOREACHC(itlink, _veclinks) {
             if( find(pgrabbed->_listNonCollidingLinks.begin(), pgrabbed->_listNonCollidingLinks.end(), *itlink) == pgrabbed->_listNonCollidingLinks.end() ) {
                 vgrabbedinfo[i]->_setRobotLinksToIgnore.insert((*itlink)->GetIndex());

src/libopenrave/libopenrave.cpp

@@ -2070,11 +2070,11 @@ void TriMesh::serialize(std::ostream& o, int options) const
 
 void Grabbed::_ProcessCollidingLinks(const std::set<int>& setRobotLinksToIgnore)
 {
-    _setBodyLinksToIgnore = setRobotLinksToIgnore;
+    _setRobotLinksToIgnore = setRobotLinksToIgnore;
     _listNonCollidingLinks.clear();
     _mapLinkIsNonColliding.clear();
     KinBodyPtr pgrabbedbody(_pgrabbedbody);
-    KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkbody->GetParent());
+    KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkrobot->GetParent());
     EnvironmentBasePtr penv = pbody->GetEnv();
     CollisionCheckerBasePtr pchecker = pbody->GetSelfCollisionChecker();
     if( !pchecker ) {
@@ -2112,7 +2112,7 @@ void Grabbed::_ProcessCollidingLinks(const std::set<int>& setRobotLinksToIgnore)
         std::vector<KinBody::LinkPtr > vbodyattachedlinks;
         FOREACHC(itgrabbed, pbody->_vGrabbedBodies) {
             boost::shared_ptr<Grabbed const> pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
-            bool bsamelink = find(_vattachedlinks.begin(),_vattachedlinks.end(), pgrabbed->_plinkbody) != _vattachedlinks.end();
+            bool bsamelink = find(_vattachedlinks.begin(),_vattachedlinks.end(), pgrabbed->_plinkrobot) != _vattachedlinks.end();
             KinBodyPtr pothergrabbedbody(pgrabbed->_pgrabbedbody);
             if( bsamelink ) {
                 pothergrabbedbody->GetLinks().at(0)->GetRigidlyAttachedLinks(vbodyattachedlinks);

src/libopenrave/libopenrave.h

@@ -278,34 +278,34 @@ void subtractstates(std::vector<dReal>& q1, const std::vector<dReal>& q2);
 class Grabbed : public UserData, public boost::enable_shared_from_this<Grabbed>
 {
 public:
-    Grabbed(KinBodyPtr pgrabbedbody, KinBody::LinkPtr plinkbody) : _pgrabbedbody(pgrabbedbody), _plinkbody(plinkbody) {
+    Grabbed(KinBodyPtr pgrabbedbody, KinBody::LinkPtr plinkrobot) : _pgrabbedbody(pgrabbedbody), _plinkrobot(plinkrobot) {
         _enablecallback = pgrabbedbody->RegisterChangeCallback(KinBody::Prop_LinkEnable, boost::bind(&Grabbed::UpdateCollidingLinks, this));
-        _plinkbody->GetRigidlyAttachedLinks(_vattachedlinks);
+        _plinkrobot->GetRigidlyAttachedLinks(_vattachedlinks);
     }
     virtual ~Grabbed() {
     }
     KinBodyWeakPtr _pgrabbedbody;         ///< the grabbed body
-    KinBody::LinkPtr _plinkbody;         ///< body link that is grabbing the body
+    KinBody::LinkPtr _plinkrobot;         ///< robot link that is grabbing the body
     std::list<KinBody::LinkConstPtr> _listNonCollidingLinks;         ///< links that are not colliding with the grabbed body at the time of Grab
-    Transform _troot;         ///< root transform (of first link of body) relative to plinkbody's transform. In other words, pbody->GetTransform() == plinkbody->GetTransform()*troot
-    std::set<int> _setBodyLinksToIgnore; ///< original links of the body to force ignoring
+    Transform _troot;         ///< root transform (of first link of body) relative to plinkrobot's transform. In other words, pbody->GetTransform() == plinkrobot->GetTransform()*troot
+    std::set<int> _setRobotLinksToIgnore; ///< original links of the robot to force ignoring
 
     /// \brief check collision with all links to see which are valid.
     ///
-    /// Use the body's self-collision checker if possible
+    /// Use the robot's self-collision checker if possible
     /// resets all cached data and re-evaluates the collisions
-    /// \param setBodyLinksToIgnore indices of the body links to always ignore, in other words remove from non-colliding list
-    void _ProcessCollidingLinks(const std::set<int>& setBodyLinksToIgnore);
+    /// \param setRobotLinksToIgnore indices of the robot links to always ignore, in other words remove from non-colliding list
+    void _ProcessCollidingLinks(const std::set<int>& setRobotLinksToIgnore);
 
     inline const std::vector<KinBody::LinkPtr>& GetRigidlyAttachedLinks() const {
         return _vattachedlinks;
     }
 
-    void AddMoreIgnoreLinks(const std::set<int>& setBodyLinksToIgnore)
+    void AddMoreIgnoreLinks(const std::set<int>& setRobotLinksToIgnore)
     {
-        KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkbody->GetParent());
-        FOREACHC(itignoreindex, setBodyLinksToIgnore) {
-            _setBodyLinksToIgnore.insert(*itignoreindex);
+        KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkrobot->GetParent());
+        FOREACHC(itignoreindex, setRobotLinksToIgnore) {
+            _setRobotLinksToIgnore.insert(*itignoreindex);
             KinBody::LinkPtr plink = pbody->GetLinks().at(*itignoreindex);
             _mapLinkIsNonColliding[plink] = 0;
             _listNonCollidingLinks.remove(plink);
@@ -324,10 +324,10 @@ class Grabbed : public UserData, public boost::enable_shared_from_this<Grabbed>
 
     /// \brief updates the non-colliding info while reusing the cache data from _ProcessCollidingLinks
     ///
-    /// note that Regrab here is *very* dangerous since the body could be a in a bad self-colliding state with the body. therefore, update the non-colliding state based on _mapLinkIsNonColliding
+    /// note that Regrab here is *very* dangerous since the robot could be a in a bad self-colliding state with the body. therefore, update the non-colliding state based on _mapLinkIsNonColliding
     void UpdateCollidingLinks()
     {
-        KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkbody->GetParent());
+        KinBodyPtr pbody = RaveInterfaceCast<KinBody>(_plinkrobot->GetParent());
         if( !pbody ) {
             return;
         }
@@ -348,7 +348,7 @@ class Grabbed : public UserData, public boost::enable_shared_from_this<Grabbed>
         std::vector<KinBody::LinkPtr > vbodyattachedlinks;
         FOREACHC(itgrabbed, pbody->_vGrabbedBodies) {
             boost::shared_ptr<Grabbed const> pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
-            bool bsamelink = find(_vattachedlinks.begin(),_vattachedlinks.end(), pgrabbed->_plinkbody) != _vattachedlinks.end();
+            bool bsamelink = find(_vattachedlinks.begin(),_vattachedlinks.end(), pgrabbed->_plinkrobot) != _vattachedlinks.end();
             KinBodyPtr pothergrabbedbody(pgrabbed->_pgrabbedbody);
             if( !!pothergrabbedbody && pothergrabbedbody != pgrabbedbody && pothergrabbedbody->GetLinks().size() > 0 ) {
                 if( bsamelink ) {