infra: add contact force sensor cfg; isaaclab: support it

[Fisher-Wang]

This PR add contact force sensor cfg and can be supported by IsaacLab

Please test using

from __future__ import annotations

import logging
import os

import rootutils
from loguru import logger as log
from rich.logging import RichHandler

logging.addLevelName(5, "TRACE")
log.configure(handlers=[{"sink": RichHandler(), "format": "{message}"}])
rootutils.setup_root(__file__, pythonpath=True)

from metasim.cfg.scenario import ScenarioCfg
from metasim.cfg.sensors import ContactForceSensorCfg
from metasim.constants import SimType
from metasim.utils.demo_util import get_traj
from metasim.utils.setup_util import get_sim_env_class


def get_actions(all_actions, action_idx: int, num_envs: int):
    envs_actions = all_actions[:num_envs]
    actions = [
        env_actions[action_idx] if action_idx < len(env_actions) else env_actions[-1] for env_actions in envs_actions
    ]
    return actions


def main():
    scenario = ScenarioCfg(
        task="stack_cube",
        robot="franka",
        cameras=[],
        sim="isaaclab",
        num_envs=4,
        try_add_table=False,
    )
    scenario.sensors.append(
        ContactForceSensorCfg(
            name="contact_force_sensor",
            base_link=("franka", "panda_leftfinger"),
            source_link="cube",
        ),
    )

    num_envs: int = scenario.num_envs
    env_class = get_sim_env_class(SimType(scenario.sim))
    env = env_class(scenario)
    assert os.path.exists(scenario.task.traj_filepath), (
        f"Trajectory file: {scenario.task.traj_filepath} does not exist."
    )
    init_states, all_actions, all_states = get_traj(scenario.task, scenario.robot, env.handler)

    obs, extras = env.reset(states=init_states[:num_envs])
    ## Main loop
    step = 0
    while True:
        log.debug(f"Step {step}")

        actions = get_actions(all_actions, step, num_envs)
        obs, reward, success, time_out, extras = env.step(actions)

        step += 1

        print("Contact force:")
        print(obs.sensors["contact_force_sensor"].force)

    env.close()


if __name__ == "__main__":
    main()

After step 60~72, when is cube is graspped, the contact force turn positive:

Contact force:
tensor([[0., 0., 0.],
        [0., 0., 0.],
        [0., 0., 0.],
        [0., 0., 0.]], device='cuda:0')
                    DEBUG    Step 60                                                                                                             tmp.py:58
Contact force:
tensor([[ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-3.5135e-01, -5.2015e-01, -3.8012e-05]], device='cuda:0')
                    DEBUG    Step 61                                                                                                             tmp.py:58
Contact force:
tensor([[ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0539e+01, -1.5600e+01,  2.4767e-03]], device='cuda:0')
                    DEBUG    Step 62                                                                                                             tmp.py:58
Contact force:
tensor([[ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0564e+01, -1.5630e+01,  4.5136e-04]], device='cuda:0')
                    DEBUG    Step 63                                                                                                             tmp.py:58
Contact force:
tensor([[-1.6675e-01, -7.1762e-01, -1.3159e-05],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0568e+01, -1.5627e+01, -2.0238e-03]], device='cuda:0')
                    DEBUG    Step 64                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2313e+00, -1.8348e+01,  3.5335e-03],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0573e+01, -1.5624e+01, -5.2357e-03]], device='cuda:0')
                    DEBUG    Step 65                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2339e+00, -1.8356e+01,  1.1037e-03],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0578e+01, -1.5619e+01, -9.2572e-03]], device='cuda:0')
                    DEBUG    Step 66                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2346e+00, -1.8355e+01, -2.2646e-03],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [-1.0584e+01, -1.5614e+01, -1.4162e-02]], device='cuda:0')
                    DEBUG    Step 67                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2354e+00, -1.8355e+01, -6.6503e-03],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 6.3459e-02, -7.8889e-01,  1.5294e-05],
        [-1.0591e+01, -1.5608e+01, -1.9894e-02]], device='cuda:0')
                    DEBUG    Step 68                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2359e+00, -1.8353e+01, -1.2066e-02],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 1.5463e+00, -1.8814e+01,  5.8881e-03],
        [-1.0598e+01, -1.5601e+01, -2.6505e-02]], device='cuda:0')
                    DEBUG    Step 69                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2364e+00, -1.8352e+01, -1.8495e-02],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 1.5524e+00, -1.8894e+01,  6.3026e-03],
        [-1.0605e+01, -1.5594e+01, -3.3967e-02]], device='cuda:0')
                    DEBUG    Step 70                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2370e+00, -1.8351e+01, -2.5913e-02],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00],
        [ 1.5527e+00, -1.8894e+01,  6.9620e-03],
        [-1.0613e+01, -1.5586e+01, -4.2356e-02]], device='cuda:0')
                    DEBUG    Step 71                                                                                                             tmp.py:58
Contact force:
tensor([[-4.2375e+00, -1.8350e+01, -3.4303e-02],
        [-2.6814e-01, -6.3348e-01, -4.7896e-05],
        [ 1.5536e+00, -1.8893e+01,  7.9119e-03],
        [-1.0621e+01, -1.5578e+01, -5.1721e-02]], device='cuda:0')
                    DEBUG    Step 72                                                                                                             tmp.py:58