Hand teleop and policy

This example will walk through setting up the Inspire Hand using an extension for a custom processing step that remaps the finger joints to control signals that can be interpreted by the inspire hand. Furthermore, the integration of the hand with the incar system will be explained. The complete Inspire Hand implementation can be found here

Setting up another hand will follow a very similar path, with just a different joint remapping. The Inspire hand has six degrees of freedom, one for each finger and an additional one for the thumb. The incar system provides cartesian positions for 26 tracked points according to the Unity Hand Model. Thus, we need to remap the joints so that the Inspire Hand can interpret the commands

Processing steps

Creating and installing the extension

First we create an extension package for the processing step:

LinuxWindows

# ensure that your python venv is sourced
incar create_pkg --name inspire_hand --processing [path]

# ensure that your python venv is sourced
incar create_pkg --name inspire_hand --processing [path]

Then install the extension in editable mode, so we can hot reload it:

LinuxWindows

pip install -e [path_to_package]

pip install -e [path_to_package]

Processing step code

We create a processingstep that can take the [left/right].commands.hand.joints.position features provided by the Incar System, and create a new feature [left/right].commands.hand.inspire. We hook this into the TELEOP_COMMAND hook.

from dataclasses import dataclass, field
import numpy as np
from incar.extensions.processing_step import ProcessStep, ProcessHook
from .retargeting import Retargeter

@ProcessStep.register_subclass("inspire_hand_remapping")
@dataclass
class InspireHandRemapping(ProcessStep):
    hooks: list[ProcessHook] = field(default_factory = lambda: [ProcessHook.TELEOP_COMMAND])
    hand_features: list[str] = field(default_factory = lambda: ["left.commands.hand.joints.position", "right.commands.hand.joints.position"])
    processed_features: list[str] = field(default_factory = lambda: ["left.commands.hand.inspire", "right.commands.hand.inspire"])

    def __post_init__(self):
        self.retargeter = Retargeter()

    def process_single_frame(self, frame: dict):
        # prevent foreach loop where keys of dict change during loop
        keys: list[str] = []
        for key in frame.keys():
            if not key in self.hand_features:
                continue 
            keys.append(key)

        for key in keys:
            goal_angles = self.retargeter.map_joints_to_goal_angles(frame[key])

            # Now, some magic numbers for offsets and mapping it into the expected range for the Inspire Hand (0 - 1000)
            goal_angles = ...

            corresponding_processed_key = self.processed_features[self.hand_features.index(key)]
            frame[corresponding_processed_key] = goal_angles.tolist()

Make sure to import the right class in inspire_hand_processing/__init__.py:

inspire_hand_processing/__init__.py

from .steps import InspireHandRemapping

If you were already running the incar system, this requires a restart.

Using the processing step

Now we need to add the retargeting in the workspace_config.json. Furthermore, we want to be able to engage/disengage our control. Since we don't have controllers with buttons, we suggest connecting bluetooth pedals to the headset, which can be configured with sending A, B, ... keys. Then we can use these to engage and disengage control.

workspace_config.json

{
    "command_processing": [
        {
            "type": "inspire_hand_remapping",
        },
        {
            "type": "require_active_button",
            "features": ["left.commands.hand.inspire"],
            "button_feature": "pedals",
            "button_name": "A"
        },
        {
            "type": "require_active_button",
            "features": ["right.commands.hand.inspire"],
            "button_feature": "pedals",
            "button_name": "B"
        }
        ...
    ],
    ...
}

Now, you should be able to see the [left/right].commands.hand.inspire features in the outgoing commands in the live data panel.

[left/right].commands.hand.inspire not showing

Are you not seeing the features? Make sure that you have enabled 'hand-tracking' mode within the XR view, and that you are using the pedals to engage control!

Teleoperating the robot

We can now use the [left/right].commands.hand.inspire command hook for the inspire hand:

import traceback

from inspire_interface import openSerial, write6, read6
from incar_networking.robot_interface import IncarRobotInterface

class InspireHand:
    def __init__(self):
        self.serial = openSerial('/dev/ttyUSB0', 115200)

    def move_joints(self, joint_positions: list[float]):
        command = [
            int(joint_positions[0]),
            int(joint_positions[1]),
            int(joint_positions[2]),
            int(joint_positions[3]),
            int(joint_positions[4]),
            int(joint_positions[5])
        ]
        write6(self.serial, 1, 'angleSet', command)

    def publish_state(self, interface: IncarRobotInterface):
        currentAngles = read6(self.serial, 1, 'angleAct')
        currentForces = read6(self.serial, 1, 'forceAct')
        interface.set_robot_state("hand", joint_pos=currentAngles, joint_effort=currentForces)
        interface.publish_state()

if __name__ == "__main__":
    robot = InspireHand()
    interface = IncarRobotInterface(
        0.1,
        command_hooks = {
            "left.commands.hand.inspire": robot.move_joints
        },
        loop_callbacks = [
            robot.publish_state
        ]
    )
    interface.start("127.0.0.1")

And voila! We can teleoperate the hand! Note that we also are publishing state back to the incar system, so that we can collect our dataset!

Todo

include GIF

Training a model and running inference

We can collect a dataset with the following features:

• wrist_cam
• left.commands.hand.inspire
• hand.joints.position
• hand.joints.efforts
• pedals

Note how we can use our mapped feature directly in the dataset. This way, when we train the network, it can output the inspire commands right away. Then, we can train a policy on this data just like any other policy, using the [left/right].commands.hand.inspire feature as an output feature.

{
    "policy": {
        "input_features": {
            "wrist_cam": {
                "type": "VISUAL",
                "shape": [3, 240, 320]
            },
            "hand.joints.position": {
                "type": "STATE",
                "shape": [6]
            }
        },
        "output_features": {
            "left.commands.hand.inspire": {
                "type": "ACTION",
                "shape": [6]
            }
        },

        "preprocessing": {
            "steps": [
                ...,
                {
                    # This step can filter the data based on whether the hand teleoperation was engaged, using the feet pedals.
                    "type": "filter_by_buttons",
                    "button_feature_names": ["pedals"],
                    "button_names": ["A"]
                },
                ...
            ],
            ...
        },
        ...
    },
    ...
}

After training, just load the policy and voila! You now have an autonomous hand completing your task!

Todo

GIF

Cross-Hand-Type policy

If you would want to have policies running cross-hand type, you would need to record the richer full hand joint data and use this as action output for the model. Then, make sure that the preprocessing-step for the joint remapping also hooks into the INFERENCE_COMMAND hook:

workspace_config.json

{
    "command_processing": [
        {
            "type": "inspire_hand_remapping",
            "hooks": ["TELEOP_COMMAND", "INFERENCE_COMMAND"]
        }
    ],
    ...
}