Gamepad Teleop: Flying gripper control mode

[hello-robot-shehab]

Brings https://github.com/hello-robot/stretch4_flying_gripper to stretch_gamepad_teleop.

This PR adds https://github.com/hello-robot/stretch4_flying_gripper as a dependency in pyproject.toml.

To Test

Update May 22: it's probably more useful to test flying gripper in #21 or #22 instead of this PR; they contain the commits in this PR.

1. Clone https://github.com/hello-robot/stretch4_flying_gripper:

cd ~/repos
git clone  https://github.com/hello-robot/stretch4_flying_gripper
pip install -e stretch4_flying_gripper

Note: if you already have it, make sure to checkout the latest main and pip install -e .
2. Run stretch_gamepad_teleop
3. Hold Select to change to the Flying Gripper Mapping and make sure it works in a similar manner to https://github.com/hello-robot/stretch4_flying_gripper/blob/main/stretch4_flying_gripper/stretch_gamepad_teleop_gripper.py, but with all the features in stretch_gamepad_teleop like changing speeds, handedness and self-collision vibration.

[hello-garv]

Would it be possible to make a util script with most of the flying gripper kinematics logic? If it can be abstracted into util functions that take position commands as their input, it can be used as a library for both gamepad control and the grasping demos. The gamepad script can handle the button press --> position change logic.

[hello-garv]

Can this be called directly from stretch4_urdf?

[hello-garv]

This is just a wrapper around existing stretch4_urdf utils. Maybe we need an additional util to return just the desired urdf_path, rather than having to find it from a list?

def _get_base_planar_ik_urdf_file():
    tmp_gamepad_folder = "/tmp/stretch_gamepad_teleop"
    setup_shared_directory(Path(tmp_gamepad_folder))
    robot = generate_robot_from_base_xacro()
    urdf_paths = generate_ik_urdfs(robot, "gamepad_teleop", tmp_gamepad_folder)
    urdf_path = [urdf_path for urdf_path in urdf_paths if "planar_ik" in urdf_path and not "fixed_wrist" in urdf_path][0]
    return urdf_path

[hello-robot-shehab]

Yah makes sense, I'll add it in, thanks Anna!

[hello-garv]

It looks like KinematicController already has a method for compute_ik_step, is there a reason why that can't be used in _map_flying_gripper_ik?

v_desired = np.array(cmd['v_desired']) * gamepad_speed_trans * speed_multiplier * dt
rot_change = np.array(cmd['rot_change']) * gamepad_speed_rot * speed_multiplier * dt
                
# v maps to joint displacements [delta_q_base_x, delta_q_base_y, delta_q_base_theta, delta_q_lift, delta_q_arm, delta_q_yaw, delta_q_ pitch, delta_q_roll]
v, _ = ikin.compute_ik_step(v_desired, rot_change, control_mode)
              
# Scale from displacement `v` back to continuous velocity by dividing by `dt`
v_vel = v / dt

[hello-robot-shehab]

It is being used here:

stretch4_body/stretch4_body/core/gamepad_control_mappings.py

Line 306 in 130da9c

v, _ = ikin.compute_ik_step(v_desired_vel, rot_change_vel, control_mode)

[hello-garv]

Ah I see it now. It seemed like a lot of computation inside the mapping function, but maybe that's unavoidable. I see it pretty much mirrors what was done in the gamepad teleop script that was in the flying gripper control package.