Minor improvement on GetNonAdjacentLinks #1527

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft

Puttichai wants to merge 7 commits into production from adjacent_computation_20250819

+55 −34

src/libopenrave/kinbody.cpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -2703,8 +2703,13 @@ bool KinBody::GetChain(int linkindex1, int linkindex2, std::vector<LinkPtr>& vli
  
    bool KinBody::IsDOFInChain(int linkindex1, int linkindex2, int dofindex) const

    {

        CHECK_INTERNAL_COMPUTATION0;

        int jointindex = _vDOFIndices.at(dofindex);

        return (DoesAffect(jointindex,linkindex1)==0) != (DoesAffect(jointindex,linkindex2)==0);

        const int jointindex = _vDOFIndices.at(dofindex);

        const int numlinks = _veclinks.size();

        OPENRAVE_ASSERT_FORMAT(jointindex >= 0 && jointindex < (int)_vecjoints.size(), "body %s jointindex %d invalid (num joints %d)", GetName()%jointindex%_vecjoints.size(), ORE_InvalidArguments);

        OPENRAVE_ASSERT_FORMAT(linkindex1 >= 0 && linkindex1 < numlinks, "body %s linkindex %d invalid (num links %d)", GetName()%linkindex1%numlinks, ORE_InvalidArguments);

        OPENRAVE_ASSERT_FORMAT(linkindex2 >= 0 && linkindex2 < numlinks, "body %s linkindex %d invalid (num links %d)", GetName()%linkindex2%numlinks, ORE_InvalidArguments);

        const int jointindexoffset = jointindex*numlinks;

        return (_vJointsAffectingLinks.at(jointindexoffset + linkindex1) == 0) != (_vJointsAffectingLinks.at(jointindexoffset + linkindex2) == 0);

    }

    int KinBody::GetJointIndex(const std::string& jointname) const

    @@ -5648,21 +5653,21 @@ void KinBody::_ResetInternalCollisionCache()
  
    bool CompareNonAdjacentFarthest(int pair0, int pair1)

    {

        // order so that farthest links are first. if equal, then prioritize links that are furthest down the chain.

        int pair0link0 = (pair0&0xffff);

        int pair0link1 = ((pair0>>16)&0xffff);

        int dist0 = pair0link1 - pair0link0; // link1 > link0

        int pair1link0 = (pair1&0xffff);

        int pair1link1 = ((pair1>>16)&0xffff);

        int dist1 = pair1link1 - pair1link0; // link1 > link0

        if( dist0 == dist1 ) {

            if( pair0link1 == pair1link1 ) {

                return pair0link0 > pair1link0;

            }

            else {

                return pair0link1 > pair1link1;

            }

        }

        return dist0 > dist1;

        const uint32_t p0 = static_cast<uint32_t>(pair0);

        const uint32_t p1 = static_cast<uint32_t>(pair1);

        const uint32_t pair0link0 = p0 & 0xffffu;

        const uint32_t pair0link1 = (p0 >> 16) & 0xffffu;

        const uint32_t pair1link0 = p1 & 0xffffu;

        const uint32_t pair1link1 = (p1 >> 16) & 0xffffu;

        const uint32_t dist0 = pair0link1 - pair0link0; // ok because linkindex1 > linkindex0

        const uint32_t dist1 = pair1link1 - pair1link0;

        // Pack everything into one 64-bit key: (dist, link1, link0).

        const uint64_t key0 = (static_cast<uint64_t>(dist0) << 32) | (pair0link1 << 16) | pair0link0;

        const uint64_t key1 = (static_cast<uint64_t>(dist1) << 32) | (pair1link1 << 16) | pair1link0;

        return key0 > key1;

    }

    const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const

    @@ -5719,10 +5724,11 @@ const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
  
            // find out what needs to computed

            if( requestedoptions & AO_Enabled ) {

                _vNonAdjacentLinks.at(AO_Enabled).resize(0);

                FOREACHC(itset, _vNonAdjacentLinks[0]) {

                    KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));

                    if( plink1->IsEnabled() && plink2->IsEnabled() ) {

                        _vNonAdjacentLinks[AO_Enabled].push_back(*itset);

                for( const int pair : _vNonAdjacentLinks[0] ) {

                    const int linkindex1 = pair & 0xffff;

                    const int linkindex2 = pair >> 16;

                    if( _veclinks.at(linkindex1)->IsEnabled() && _veclinks.at(linkindex2)->IsEnabled() ) {

                        _vNonAdjacentLinks[AO_Enabled].push_back(pair);

                    }

                }

                _nNonAdjacentLinkCache |= AO_Enabled;

src/libopenrave/robot.cpp

-Original file line number
+Diff line change
@@ Expand Up @@
             // compute it
             if( compute.at(AO_Enabled) ) {
                 _vNonAdjacentLinks.at(AO_Enabled).resize(0);
-                FOREACHC(itset, _vNonAdjacentLinks[0]) {
-                    KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));
-                    if( plink1->IsEnabled() && plink2->IsEnabled() ) {
-                        _vNonAdjacentLinks[AO_Enabled].push_back(*itset);
+                for( const int pair : _vNonAdjacentLinks[0] ) {
+                    const int linkindex1 = pair & 0xffff;
+                    const int linkindex2 = pair >> 16;
+                    if( _veclinks.at(linkindex1)->IsEnabled() && _veclinks.at(linkindex2)->IsEnabled() ) {
+                        _vNonAdjacentLinks[AO_Enabled].push_back(pair);
                     }
                 }
                 std::sort(_vNonAdjacentLinks[AO_Enabled].begin(), _vNonAdjacentLinks[AO_Enabled].end(), CompareNonAdjacentFarthest);
             }
             if( compute.at(AO_ActiveDOFs) ) {
                 _vNonAdjacentLinks.at(AO_ActiveDOFs).resize(0);
-                FOREACHC(itset, _vNonAdjacentLinks[0]) {
-                    FOREACHC(it, GetActiveDOFIndices()) {
-                        if( IsDOFInChain(*itset&0xffff,*itset>>16,*it) ) {
-                            _vNonAdjacentLinks[AO_ActiveDOFs].push_back(*itset);
-                            break;
+                {
+                    // Expand the content of ISDOFInChain to avoid redundant checks
+                    // CHECK_INTERNAL_COMPUTATION; // don't need this since KinBody::GetNonAdjacentLinks already calls it.
+                    for( const int pair : _vNonAdjacentLinks[0] ) {
+                        const int linkindex1 = pair & 0xffff;
+                        const int linkindex2 = pair >> 16;
+                        const int numlinks = _veclinks.size();
+                        OPENRAVE_ASSERT_FORMAT(linkindex1 >= 0 && linkindex1 < numlinks, "body %s linkindex %d invalid (num links %d)", GetName()%linkindex1%numlinks, ORE_InvalidArguments);
+                        OPENRAVE_ASSERT_FORMAT(linkindex2 >= 0 && linkindex2 < numlinks, "body %s linkindex %d invalid (num links %d)", GetName()%linkindex2%numlinks, ORE_InvalidArguments);
+                        for( const int dofindex : GetActiveDOFIndices() ) {
+                            const int jointindex = _vDOFIndices.at(dofindex);
+                            OPENRAVE_ASSERT_FORMAT(jointindex >= 0 && jointindex < (int)_vecjoints.size(), "body %s jointindex %d invalid (num joints %d)", GetName()%jointindex%_vecjoints.size(), ORE_InvalidArguments);
+                            const int jointindexoffset = jointindex*numlinks;
+                            if( (_vJointsAffectingLinks.at(jointindexoffset + linkindex1) == 0) != (_vJointsAffectingLinks.at(jointindexoffset + linkindex2) == 0) ) {
+                                _vNonAdjacentLinks[AO_ActiveDOFs].push_back(pair);
+                                break;
+                            }
                         }
                     }
                 }
                 std::sort(_vNonAdjacentLinks[AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_ActiveDOFs].end(), CompareNonAdjacentFarthest);
             }
             if( compute.at(AO_Enabled|AO_ActiveDOFs) ) {
                 _vNonAdjacentLinks.at(AO_Enabled|AO_ActiveDOFs).resize(0);
-                FOREACHC(itset, _vNonAdjacentLinks[AO_ActiveDOFs]) {
-                    KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));
-                    if( plink1->IsEnabled() && plink2->IsEnabled() ) {
-                        _vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].push_back(*itset);
+                for( const int pair : _vNonAdjacentLinks[AO_ActiveDOFs] ) {
+                    const int linkindex1 = pair & 0xffff;
+                    const int linkindex2 = pair >> 16;
+                    if( _veclinks.at(linkindex1)->IsEnabled() && _veclinks.at(linkindex2)->IsEnabled() ) {
+                        _vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].push_back(pair);
                     }
                 }
                 std::sort(_vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].end(), CompareNonAdjacentFarthest);
@@ Expand Down @@

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Minor improvement on GetNonAdjacentLinks #1527

Diff view

Diff view

There are no files selected for viewing

Minor improvement on GetNonAdjacentLinks #1527

Are you sure you want to change the base?

Minor improvement on GetNonAdjacentLinks #1527

Uh oh!

Uh oh!

Diff view

Diff view

There are no files selected for viewing