Anomaly in Allegro Hand's Joint Velocity When Mounted on Panda Arm

[hesic73]

Hello,

I previously mounted a left Allegro Hand on an xArm, and it worked as expected in ManiSkill (https://github.com/haosulab/ManiSkill/issues/482). Recently, I attempted to mount a right Allegro Hand on a Franka Panda arm using a custom URDF franka.zip based on the following definitions:

• https://github.com/haosulab/ManiSkill/blob/main/mani_skill/assets/robots/panda/panda_v2.urdf
• https://github.com/haosulab/ManiSkill/blob/main/mani_skill/assets/robots/allegro/allegro_hand_right_glb.urdf
• https://github.com/Msornerrrr/sope-dex/blob/main/assets/urdf/franka_description/robots/franka_panda_allegro.urdf

However, I'm encountering an unexpected issue: the joint velocities for the Allegro Hand are significantly higher than anticipated when I attempt to control it using a reinforcement learning policy. This behavior only occurs when the Allegro Hand is mounted on the Panda arm.

Could someone help me identify the cause of this velocity anomaly or suggest potential solutions? Thank you very much!

Agent class code:

import numpy as np
import torch

from typing import Dict

from mani_skill.envs.scene import ManiSkillScene
import sapien
from mani_skill.agents.base_agent import BaseAgent, Keyframe
from mani_skill.agents.registration import register_agent
from mani_skill.agents.base_agent import DictControllerConfig

from mani_skill.agents.controllers import PDEEPoseControllerConfig, deepcopy_dict, PDJointPosControllerConfig
import os



@register_agent()
class FrankaAllegoRight(BaseAgent):
    uid = "franka_allegro_right"
    urdf_path = os.path.join(
        ASSET_DIR, "urdf/franka/franka_allegro_right.urdf")

    # for debug
    keyframes = dict(
        rest=Keyframe(
            qpos=np.array(
                [
                    0.0,
                    np.pi / 8,
                    0,
                    -np.pi * 5 / 8,
                    0,
                    np.pi * 3 / 4,
                    np.pi / 4,
                ]+[0.0 for _ in range(16)]
            ),
            pose=sapien.Pose(p=np.array([-0.5, 0.0, 0.0])),
        )
    )

    arm_joint_names = [
        "panda_joint1",
        "panda_joint2",
        "panda_joint3",
        "panda_joint4",
        "panda_joint5",
        "panda_joint6",
        "panda_joint7",
    ]

    qmin_hand = torch.tensor([
        -0.470,
        -0.470,
        -0.470,
        0.8,  # 0.263,

        0.5,  # -0.196,
        0.5,  # -0.196,
        0.5,  # -0.196,
        -0.105,

        -0.174,
        -0.174,
        -0.174,
        -0.189,

        -0.227,
        -0.227,
        -0.227,
        -0.162,
    ])
    qmax_hand = torch.tensor([
        0.470,
        0.470,
        0.470,
        1.396,

        1.610,
        1.610,
        1.610,
        1.163,

        1.709,
        1.709,
        1.709,
        1.644,

        1.618,
        1.618,
        1.618,
        1.719,
    ])

    hand_joint_names = [
        'joint_0.0',
        'joint_4.0',
        'joint_8.0',
        'joint_12.0',
        'joint_1.0',
        'joint_5.0',
        'joint_9.0',
        'joint_13.0',
        'joint_2.0',
        'joint_6.0',
        'joint_10.0',
        'joint_14.0',
        'joint_3.0',
        'joint_7.0',
        'joint_11.0',
        'joint_15.0',
    ]

    def __init__(self, scene: ManiSkillScene, control_freq: int, control_mode: str = None, agent_idx: int = None):

        self.arm_stiffness = 1e3
        self.arm_damping = 1e2
        self.arm_force_limit = 100

        self.hand_stiffness = 4e2
        self.hand_damping = 1e1
        self.hand_force_limit = 5e1

        self.ee_link_name = 'base_link'

        super().__init__(scene, control_freq, control_mode, agent_idx)

    def _make_pd_joint_pos_hand_config(self):

        hand_pd_pos = PDJointPosControllerConfig(
            self.hand_joint_names,
            self.qmin_hand,
            self.qmax_hand,
            self.hand_stiffness,
            self.hand_damping,
            self.hand_force_limit,
            use_delta=False,
        )
        return hand_pd_pos

    def _make_pd_joint_pos_config(self):
        """
        Absolute joint position control for both arm and hand
        """
        # min_arm = torch.tensor(
        #     [
        #         -2.8973,
        #         -1.7628,
        #         -2.8973,
        #         -3.0718,
        #         -2.8973,
        #         -0.0175,
        #         -2.8973,
        #     ])
        # max_arm = torch.tensor(
        #     [
        #         2.8973,
        #         1.7628,
        #         2.8973,
        #         -0.0698,
        #         2.8973,
        #         3.7525,
        #         2.8973,
        #     ])

        min_arm = torch.tensor(
            [
                -1.0,
                -1.0,
                -1.0,
                -3.0718,
                -1.0,
                -0.0175,
                0.0,
            ])
        max_arm = torch.tensor(
            [
                1.0,
                1.0,
                1.0,
                -0.0698,
                1.0,
                3.7525,
                2.8973,
            ])

        arm_pd_pos = PDJointPosControllerConfig(
            self.arm_joint_names,
            min_arm,
            max_arm,
            self.arm_stiffness,
            self.arm_damping,
            self.arm_force_limit,
            use_delta=False,
        )

        hand_pd_pos = self._make_pd_joint_pos_hand_config()

        return dict(
            arm=arm_pd_pos, gripper=hand_pd_pos
        )

    def _make_pd_arm_ee_delta_pose_hand_joint_pos_config(self):
        """
        EE delta pose control for arm and absolute joint position control for hand
        """
        arm_pos_lower = torch.tensor([-0.05, -0.05, -0.05])
        arm_pos_upper = torch.tensor([0.05, 0.05, 0.05])
        arm_rot_lower = -0.25
        arm_rot_upper = 0.25

        arm_pd_ee_delta_pose = PDEEPoseControllerConfig(
            joint_names=self.arm_joint_names,
            pos_lower=arm_pos_lower,
            pos_upper=arm_pos_upper,
            rot_lower=arm_rot_lower,
            rot_upper=arm_rot_upper,
            stiffness=self.arm_stiffness,
            damping=self.arm_damping,
            force_limit=self.arm_force_limit,
            ee_link=self.ee_link_name,
            urdf_path=self.urdf_path,
        )

        hand_pd_pos = self._make_pd_joint_pos_hand_config()
        return dict(
            arm=arm_pd_ee_delta_pose, gripper=hand_pd_pos
        )

    @ property
    def _controller_configs(
        self,
    ) -> Dict[str,  DictControllerConfig]:

        controller_configs = dict(
            pd_joint_pos=self._make_pd_joint_pos_config(),
            pd_arm_ee_delta_pose_hand_joint_pos=self._make_pd_arm_ee_delta_pose_hand_joint_pos_config(),
        )

        # Make a deepcopy in case users modify any config
        return deepcopy_dict(controller_configs)