Click-to-Pregrasp

[hello-amal]

Description

This PR implements a feature where the user is able to click a point on the Realsense stream, and if the point is within the robot's reach, it will align with the point with either a horizontal or vertical gripper (user-specified). The aim of this feature is to prevent the iterative and time-consuming movements required for an operator to properly align the gripper with an object to grasp it.

Upgrade Info

See README.md for information on installing the additional requirements and generating the specialized URDF.

Testing procedure

• Stretch RE3 with Dex Wrist and D405:
  • Launch the interface, add a Realsense Camera with buttons displayed below.
  • Click on an object on the floor that is within the graspable region with a horizontal grasp, verify it works.
  • Click on an object on the floor that is within the graspable region with a vertical grasp, verify it works.
  • Click on an object on a table that is within the graspable region with a horizontal grasp, verify it works.
  • Click on an object on a table that is within the graspable region with a vertical grasp, verify it works.
  • Click on an object outside the graspable region with a horizontal grasp, verify it fails.
  • Click on an object outside the graspable region with a vertical grasp, verify it fails.
• Stretch3 with tablet:
  • Verify that the click-to-pregrasp buttons don't appear.
• Stretch RE2 with Dex Wrist and D405:
  • Repeat tests as above.
• Stretch RE2 with Dex Wrist and Beta Teleop Kit:
  • Repeat tests as above.
• Stretch RE2 without Dex Wrist:
  • Verify that the "Select Object" and "Align Gripper with Object" buttons do not appear.

Before opening a pull request

From the top-level of this repository, run:

• pre-commit run --all-files

To merge

• Squash & Merge

[hello-amal]

@hello-cpaxton This is ready for a review of the functionality (move_to_pregrasp.py, stretch_ik_control.py). I'll be working on the UX this week.

[hello-cpaxton]

we can probably tweak the formatting rules so this isnt 3 lines

[hello-binit]

@hello-amal jFYI, I tried running this branch and saw the following errors. You may need to add the constants and helpers python packages as ROS deps in package.xml. Or they can be added here.

[hello-amal]

Thanks for surfacing that @hello-binit . I was able to recreate it when rebuilding from scratch, and the latest commit should address it -- try it out when you get a chance.

[hello-amal]

Thanks for the comments @hello-cpaxton ! I addressed them and cleaned up th code. @hello-vinitha , you can review it now when you get a chance. This has been thoroughly tested on the Stretch 3 (with the dex wrist and with the tablet), not yet on earlier configurations.

[hello-vinitha]

@hello-amal I tested this PR on a Stretch 2 with the upgraded teleop kit and dex wrist. The arm collides with the base when the (horizontal) pre-grasp z position is below the base.

[hello-vinitha]

Tested on the various Stretch 2 configurations. All tests pass!