ComputeJointVelocityFromTwist fails on continuous joints

[mkoval]

ComputeJointVelocityFromTwist prevents the robot from moving outside of the limits returned by GetActiveDOFLimits. Unfortunately, GetActiveDOFLimits returns (0, 0) on joints with IsContinuous set to True. This causes ComputeJointVelocityFromTwist to immediately fail.

There are a few ways to fix this. The simplest change is, I think, to set the respective min and max limits to infinity when a joint is continuous:

q_min, q_max = robot.GetActiveDOFLimits()
for index, dof_index in xrange(robot.GetActiveDOFIndices()):
    joint = robot.GetJointFromDOFIndex(dof_index)
    if joint.IsCircular(dof_index - joint.GetDOFIndex()):
        q_min[index] = -numpy.inf
        q_max[index] = numpy.inf

This is pretty atrocious code, but I don't know of better way of doing the IsContinuous check that can work on multi-DOF joints. (Note: This is also untested. I wrote it inline.)

@siddhss5 @psigen Thoughts?

Tagging @sjavdani, @Stefanos19, and @herlant since this is important on ADA.

[cdellin]

Random code comments: (a) index -> q_index, (b) xrange -> enumerate.

[cdellin]

Also (c) IsContinuous() -> IsCircular().

[psigen]

Agree with the philosophy, Inf on continuous makes sense to me.

Will look at code prettification in depth later. Think you can probably make a pretty boolean array and slice it like:

is_continuous = [robot.GetJointFromIndex(i).IsCircular() for i in
robot.GetActiveDOFIndices()]
q_min[is_continuous] = -numpy.inf
q_max[is_continuous] = numpy.inf

On Wed, Feb 25, 2015 at 4:26 PM, cdellin notifications@github.com wrote:

Random code comments: (a) index -> q_index, (b) xrange -> enumerate.

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/prpy/issues/61#issuecomment-76063630 .

[mkoval]

@cdellin Thanks!

@psigen That's not possible. IsCircular can theoretically be different for each DOF of a multi-DOF joint. E.g. you could have a 3-DOF spherical joint where only a subset of the DOFs are continuous. I don't think we can do much better than what I listed above if we want to handle this case.

[siddhss5]

LGTM. We should run a contest for the prettiest list comprehension of the above code :).

[cdellin]

Best I could do:

q_min, q_max = robot.GetActiveDOFLimits()

jointof = robot.GetJointFromDOFIndex
iscirc = lambda d: jointof(d).IsCircular(d-jointof(d).GetDOFIndex())
q_circ = [iscirc(d) for d in robot.GetActiveDOFIndices()]
q_min[q_circ] = -numpy.inf
q_max[q_circ] =  numpy.inf

[sjavdani]

Shouldn't this be changed higher in the chain? That is, shouldn't GetActiveDOFLimits() return +-numpy.inf when IsContinuous is true?

On Wed, Feb 25, 2015 at 5:31 PM cdellin notifications@github.com wrote:

Best I could do:

q_min, q_max = robot.GetActiveDOFLimits()

jointof = robot.GetJointFromDOFIndex iscirc = lambda d: jointof(d).IsCircular(d-jointof(d).GetDOFIndex()) q_circ = [iscirc(d) for d in robot.GetActiveDOFIndices()] q_min[q_circ] = -numpy.inf q_max[q_circ] = numpy.inf

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/prpy/issues/61#issuecomment-76075328 .

[psigen]

That's an interesting point, @sjavdani. This seems like a case where changing to the intuitive behavior in our OpenRAVE fork might be The Right Thing™.