[CoMPS-users] DoGeneralIK ignores repeating indices for active DOF indices.

[Bener S. <ben...@gm...>]

Hi,

my robot model has two 10 DOF manipulators and the first three joints of
these manipulators are the same. To be more specific:

leftArm indices:  0,1,2,3,4,5,6,7,8,9
rightArm indices: 0,1,2,10,11,12,13,14,15,16

In OpenRAVE (0.8.2 Stable Release), I do the following to activate my two
manipulators so I can run DoGeneralIK for the end effectors:

for( size_t m=0; m < manipIdx.size(); m++)
    {
      activedofs.insert( activedofs.end(),

probot->GetManipulators()[manipIdx[m]]->GetArmIndices().begin(),

probot->GetManipulators()[manipIdx[m]]->GetArmIndices().end()
                 );
    }

The block above gives me the vector<int> activedofs as

{0,1,2,3,4,5,6,7,8,9,0,1,2,10,11,12,13,14,15,16}

And then I do

probot->SetActiveDOFs(activedofs);

as usual.

To confirm that the indices are actually active I do:

vector<int> dbug = probot->GetActiveDOFIndices();
cout << "checking which indices are active: " << endl;
// debug
for( size_t i=0; i < dbug.size(); i++)
  {
     cout << dbug[i] << endl;
   }

And this prints out the same indices.

So far so good...

Now when I make a call to DoGeneralIK with this set of active indices, the
joints that overlap between manipulators (namely 0, 1 and 2) behave as if
they are not active (that is, they don't move).

*However*, if I eliminate the repeating indices with the following two lines

sort(activedofs.begin(), activedofs.end());
activedofs.erase(unique(activedofs.begin(), activedofs.end()),
activedofs.end());

so that, my vector of activedofs is

{0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}

In this case DoGeneralIK behaves as expected.

I'm wondering if this is a SetActiveDOFs issue, or if this is related to
the way DoGeneralIK method handles GetActiveDOFIndices()... Curious.

I'd be happy to contribute with the two-liner "remove repeating indices"
block if that really is the appropriate / elegant solution for the issue.

Thanks for your attention,
Ben Suay
wpi.edu/~benersuay