Add clamping for SO3 and SE3

[adlarkin]

Closes #91

Add clamping to SO3 and SE3, which can be used to clamp pose targets for things like a frame task to emulate constraints on the end-effector.

[kevinzakka]

Hi @adlarkin, started an internship and am pretty slammed. Will review on the weekend!

[adlarkin]

Hey @kevinzakka, just checking in - would you still be interested in giving this a look when you have the time?

[kevinzakka]

Hi @adlarkin, apologies for the delay! I've been at RSS demoing my work and attending the conference so got super busy. I’ll review this in the next few days. Thanks so much for your patience—and again, sorry for the wait!

[adlarkin]

Thanks Kevin, no rush (it slipped my mind as well) - hope you enjoy RSS!

[stephane-caron]

Just two cents coming long after the initial PR 😊 but if I understand the task correctly it is the target pose that is constrained, with clamped translation and rotation? If so, maybe "ConstrainedPoseTask" or "ClampedPoseTask" could be more accurate names than "PoseConstraintTask"?

One concern with the expression "constraint task" is that it combines the words constraint and task, which in this context can also be used to distinguish between enforcing a (hard) constraint (QP equality constraint) versus a soft one (task expressed in the QP cost function).

[adlarkin]

Hey @stephane-caron, thanks for chiming in! Your concern makes sense, I think it would be worth renaming this to ConstrainedPoseTask.

However, after taking another look at this, I think there is even a simpler approach - currently, the task proposed in this PR is the same as the RelativeFrameTask, but with clamping that is performed in the task's set_target method. To reduce code duplication, what if we instead added a clamp_pose method (or something similarly named) that users can call before calling the task's set_target? I see a few benefits here:

• free function is simpler than another class
• this free function can be used with any task (users just need to get the transform first, then call clamp_pose followed by set_target)

@kevinzakka thoughts on this? I can follow-up here with this change if you think it's worthwhile.

[adlarkin]

However, after taking another look at this, I think there is even a simpler approach - currently, the task proposed in this PR is the same as the RelativeFrameTask, but with clamping that is performed in the task's set_target method. To reduce code duplication, what if we instead added a clamp_pose method (or something similarly named) that users can call before calling the task's set_target?

I implemented this approach in 3976f0f. If we want to keep this new approach (it's a lot simpler!), I will update the PR title/description and mark #96 (comment) as resolved.

[kevinzakka]

Thank you @adlarkin, new changes look wonderful.

[kevinzakka]

Hi @adlarkin, can you rebase to the latest main? Should fix the test errors.

[adlarkin]

Done! Looks like all tests are passing now 🙂

[kevinzakka]

Woohoo, nice @adlarkin !