Determine correct hand DOF indices and Open/Close behavior

[ClintLiddick]

During the ros_control upgrade I noticed hand_tests::test_getIndices_ReturnsIndices fails, so I changed the order of spread/fingers in Hand::GetIndices (intentionally highlighted line 112 below) to make the test pass. This has broken some other code, and seems to not match the Barrett (and others'?) convention of spread being last.

This commit so far is just to make OpenHand move to the expected open position, with spread at 0.0.

@mkoval @psigen @cdellin Can we use this PR to determine the correct way this should all work?

[cdellin]

Barrett's UI, APIs, and on-the-wire protocols use an order of [F1, F2, F3, Spread], and OWD used the same convention [0], (e.g. for the MoveHand service call). The original OpenRAVE HERB model [1] used the same order for the hand DOFs (right hand dofs had indices [7,8,9,10] with 10 the spread, and left hand dofs had indices [18,19,20,21] with 21 the spread).

My understanding is that the new HERB model uses a hand URDF [2] which defines joints by name, not index (as is the convention in ROS). The names for the joints are: ["j01","j11","j21","j00"] (using the same index as above; the strings /right/ and /left/ are prefixed by xacro for the two hands), and I think or_urdf [3] alphabetizes the joint names in order to maintain a consistent joint order, since OpenRAVE uses joint indices regularly. So the index order ends up being [Spread, F1, F2, F3].

I'm of course happy with any resolution -- I'm sympathetic to the idea that all of our code should use joint names instead of indices as the primary interface to joint vectors. But we should make sure all the dependent code is happy with this.

[0] https://github.com/personalrobotics/owd/blob/master/owd/openwam/CANbus.cc#L2841 [1] https://gist.github.com/cdellin/ad9ca7b2911be25c7b604d847a4b649a [2] https://github.com/personalrobotics/herb_description/blob/master/robots/BHD280_URDF.URDF [3] https://github.com/personalrobotics/or_urdf

[gilwoolee]

Also, table_clearing demo hard-coded open/close/move_hand actions with hand.GetIndices()[0:3] to control the fingers, so we'll have to change them as well if we make any changes. I agree with @cdellin that we should use joint names.

[psigen]

Since we no longer have any deterministic control over the DOF ordering (there's simply no guarantee that order is preserved by URDF), I think we'd better make sure everything can handle arbitrary DOF orders.

[cdellin]

I'm adding a comment to this issue, because it affects several things. Clint et. al. may already have everything figured out, but there are still a couple of points I'm confused by.

1. @lgw903 in a recent table_clearing PR [0] is switching the hand referencing code in a lot of those actions. Before, it used to call manipulator.hand.GetIndices(), and assume that the order was [F1, F2, F3, Spread]. But we didn't like that because it imposed an order on the returned indices. Now, that is split into two separate calls, manipulator.hand.GetFingerIndices() and manipulator.hand.GetSpreadIndex(). But it seems the new way still has the problem -- GetFingerIndices() is assumed to return the indices in this order: [F1, F2, F3] (and not e.g. [F2, F3, F1]). It appears to me that the new solution is the worst of both worlds -- neither conforming to the standard order, nor immune to reordering problems. (For example, I would imagine that if someone changed the joint names to finger_one, finger_two, and finger_three, the or_urdf package would load them in this order: [F1, F3, F2], and we'd have this problem all over again.)
2. It seems to me that the documentation [1] for the above methods in herbpy used to be correct (that is, the docs provide that the returned indices are in a particular order), and there should never have been a reason for downstream software to change. As it is now, there seems to be an obvious inconsistency:

def GetIndices(self):
    """ Gets the DOF indices of this hand.
    For compatability with OWD, the indices are always returned in the
    order: [ finger 0, finger 1, finger 2, spread ].
    @return DOF indices of the hand
    """
    return [ self.GetSpreadIndex() ] + self.GetFingerIndices()

At the end of the day, I think we need a consistent plan for how this is to be handled. What do you guys think?

[0] https://github.com/personalrobotics/table_clearing/pull/22 [1] https://github.com/personalrobotics/herbpy/blob/4cd30baae12fc1489895b861cd197bdf3c2c1c99/src/herbpy/barretthand.py#L98

[gilwoolee]

I agree with @cdellin . I think we should first find out whether the changes to GetIndices was necessary. If GetIndices can stick to what its contract says, table_clearing doesn't have to change at all.

[ClintLiddick]

The BarrettHand::GetFingerIndices method is as such:

 def GetFingerIndices(self):
        """ Gets the DOF indices of the fingers.
        These are returned in the order: [ finger 0, finger 1, and finger 2 ].
        @return DOF indices of the fingers
        """
        names = [ 'j01', 'j11', 'j21' ]
        return [ self._GetJointFromName(name).GetDOFIndex() for name in names ]

And so deterministically returns the [f1, f2, f3] index ordering, and GetSpreadIndex returns only a single value so that is fine.

I changed the GetIndices code initially to make an--honestly very questionable--unit test pass. That change can be reverted, and I can handle proper reordering in other methods that need to be simulation-aware.

What I'm really worried about is the fact that the whole GetIndices idea seems to be a farce, since this is the prpy.base.EndEffector::GetIndices implementation:

    def GetIndices(self):
        """Gets the DOF indicies associated with this end-effector.
        @return list of DOF indices
        """
        indices = self.manipulator.GetChildDOFIndices()
        indices.sort()
        return indices

The indices are sorted. What kind of legacy meaning does this have?

[psigen]

I think it is important to note a distinction here between the requirements of herbpy specific functions, and inherited prpy functions.

It is reasonable for us to enforce ordering in a function like BarrettHand::GetFingerIndices(), since we are defining the contract for this function and also have control of the URDF model and loading code associated with it. In the situation that @cdellin described where fingers get renamed, it is fine to correspondingly do arbitrary reordering inside this function to correct for it.

It is not as reasonable for us to make this claim about inherited prpy functions, as prpy certainly cannot guarantee these things for arbitrary robots. It seems a little bit odd for us to make extra guarantees about prpy functions only when they are implemented from herbpy. We can do it, but it might lead to further situations like this in the future, or difficulties porting code upstream to prpy that depends on a property that only herbpy guarantees.

@clint I think the reason that the sort operation is in there, IIRC, is because OpenRAVE can really just return any order at all, and @mkoval and I ran into some siuation where it changed depending on where the call was made. (E.g. calling GetIndices() on an end-effector did not return the same ordering as calling GetChildDOFIndices() on the parent manipulator). I think this was an attempt to make some guarantee about consistent ordering, even if it was arbitrary w.r.t. the robot kinematic chain.

[cdellin]

Thanks @ClintLiddick! I agree with your assertion about GetFingerIndices() -- since it uses explicit name lookups, it should work fine even if the underlying DOFs change order.

But I'd argue that GetIndices() (as it was implemented in herbby) makes the same guarantee -- that is, that the spread index will always be returned last. So it should be just as correct. It seems to me that any code using the original GetIndices() should work fine no matter the comparative DOF values of the spread vs the fingers.

I think I agree that we should look again at what was causing that unit test to fail -- I think perhaps swapping in GetIndices() to correct that failure is actually masking a deeper bug somewhere else that's relying on a particular DOF order.

Regarding prpy.base.EndEffector::GetIndices() sorting its return value, that's a good question ... we should probably call in @mkoval here, since it looks like that code dates back to 2013 from AndyPy: https://github.com/personalrobotics/prpy/commit/a4d24618. I looked briefly at the implementation of Manipulator::GetChildDOFIndices() in OpenRAVE, and it looks like it should already return in sorted order. But @psigen is right, Mike might have just wanted to be sure, especially since the OpenRAVE docs don't make any guarantee on this point.

To the deeper point, I agree with @psigen though that it's perhaps a bit strange for users of herbbpy's GetIndices() to rely on the order advertised in its documentation even if the base class method would return a different order, as it certainly would in this case. From a semantic standpoint, there's actually a bit of a confusion here, since OpenRAVE's definition of an End Effector is simply a link (and of course links don't have indices), whereas PrPy's definition of an End Effector appears to mean a hand (which can have indices, where you might want them in some assumed order). Perhaps one solution would be to simply remove the PrPy's base class implementation of GetIndices() entirely (or throw a NotImplementedException), and rely on actual implementations of hands and such to provide this in the correct order. I'm curious, what would break in that case?

[mkoval]

I mostly agree with @cdellin and @psigen here:

1. the EndEffector functions GetIndices and GetFingerIndices may return indices in an arbitrary order (including sorted) because nothing is specified in the documentation
2. the BarrettHand functions GetIndices and GetFingerIndices functions must return indices in the order specified in their documentation: [ finger 0, finger 1, finger 2, spread ]
3. it's fine for us to rely on this order when using a BarrettHand, but not on a generic EndEffector

Therefore, I suggest we:

1. leave EndEffector's unchanged
2. revert BarrettHand to return DOFs in the documented order: [ finger 0, finger 1, finger 2, spread ]
3. fix any HerbPy tests that fail after making change (2)
4. revert any changes to table_clearing that broke compatibility with (2)

I don't think the sort() call in EndEffector is relevant because PrPy guarantees nothing about the order of the indices it returns. I am not sure why we originally added it, but @psigen's guess is good: we likely used this to make the order more robust to changes in OpenRAVE's XML parsing code, or_urdf, and our manipulation definition.

[cdellin]

By the way I re-reviewed the implementation of Manipulator::GetChildDOFIndices(), and my initial assessment (that it already returns indices in sorted order) was incorrect. It actually uses an ugly combination of link and joint order. So I can imagine why the sort() was added at some point.

[gilwoolee]

Table-clearing just has an open PR, so it just needs to be deleted. :)

[jeking04]

Here is an alternate set of fixes that I believe will work:

1. Change hand_tests.py to get indices via self._hand.GetIndices() here: https://github.com/personalrobotics/herbpy/blob/feature/hand_dof_indices/tests/hand_tests.py#L14
2. Revert MoveHand in barrett hand to not have the special case for sim: https://github.com/personalrobotics/herbpy/blob/feature/hand_dof_indices/src/herbpy/barretthand.py#L132
3. Revert GetIndices to return in the ordering specified in the comment: return self.GetFingerIndices() + [ self.GetSpreadIndex() ]
4. Revert OpenHand to default spread=None allowing the caller to indicate that we just want the fingers to open without changing the spread.

I believe by making fix #1, we make the test agnostic to any changes to the urdf or dof ordering. We rely on GetIndices to keep its contract that it will return finger values then spread value.

@lgw903 and I confirmed that the tests then pass.

[ClintLiddick]

@jeking04 that looks good, and I'll add that removing the special case from MoveHand means that the controllers in prpy need to depend on the ordering specified in BarrettHand. So I think, given what we've laid out here, we still need the sim/non-sim cases to do the correct reordering for the prpy controllers.

Edit: I might be wrong, the controllers might just conveniently use the "original" GetIndices ordering anyway.

[ClintLiddick]

Ok, as-is works in sim and real. This also makes it work with old code, so I'm merging the PR. Tests will continue to fail (now that they're actually running), so we'll have to fix them sometime, but the relevant DOF_Indices test passes now.