ComputeJointVelocityFromTwist is incorrect when outside of joint limits

[mkoval]

Joint limits are currently enforced by this (admittedly beautiful) list comprehension:

dq_bounds = [(0, max) if (numpy.isclose(q_curr[i], q_min[i])) else
             (min, 0) if (numpy.isclose(q_curr[i], q_max[i])) else
             (min, max) for i, (min, max) in enumerate(bounds)]

If q_curr < q_min or q_curr > q_max, this code allows the robot to move further outside of joint limits. I think we should treat this as if we are at the limit. This is equivalent to changing the code to:

dq_bounds = [(0, max) if (q_curr[i] <= q_min[i]) else
             (min, 0) if (q_curr[i] >= q_max[i]) else
             (min, max) for i, (min, max) in enumerate(bounds)]

@siddhss5 Thoughts?

[siddhss5]

I agree this is totally idiot proof and the better way to go than what I have. I made the assumption that a robot's DOF cannot be set to below its limits because: a) I think openrave clamps at the DOF limit b) you know, a real robot has limits :)

[mkoval]

(a) is true by default. However, you can override this behavior by passing a CheckLimitsAction enum to SetDOFValues. In fact, we pass CLA_Nothing in or_owd_controller and or_ros_controller to disable clamping. This is important to avoid "first point of traj is not current position" errors when executing trajectories in OWD.

I agree with (b) in theory, but that would make HERB not a real robot. :smile:

In any case, I'll send a PR.

[mkoval]

This was fixed in 76a92db256d03aa39be6ca611b711c8a8c6e5bae.