search

haosulab / ManiSkill

SAPIEN Manipulation Skill Framework, a GPU parallelized robotics simulator and benchmark
https://maniskill.ai/
Apache License 2.0
928 stars 168 forks source link

Incorrect initialization of `active_ancestor_joints` in mani_skill.agents.controllers.utils.kinematics.Kinematics #666

Closed mj-hwang closed 6 days ago

mj-hwang commented 3 weeks ago

Hello everyone! I was wondering if the current implementation of active_ancestor_joints is correct.

In the mani_skill.agents.controllers.utils.kinematics.py, active_ancestor_joints are computed by iterating through the parent link/join until the cur_link is None, where cur_link initialized with the joint of the parent link (see line 54 of kinematics.py: cur_link = self.end_link.joint.parent_link):

cur_link = self.end_link.joint.parent_link
active_ancestor_joints: List[ArticulationJoint] = []
while cur_link is not None:
    if cur_link.joint.active_index is not None:
        active_ancestor_joints.append(cur_link.joint)
    cur_link = cur_link.joint.parent_link
active_ancestor_joints = active_ancestor_joints[::-1]
self.active_ancestor_joints = active_ancestor_joints

I believe this initialization is incorrect since it should really start from the end_link, not its parent link. The current implementation works fine in case the end_link is not directly connected with one of the active joints (which is the case for most robot arms with end effectors).

However, say we are controlling a Franka robot, and we want to use panda_link7 as the end link. With the current implementation, panda_joint7gets ignored since it starts from the joint of the end link's parent link. In fact, if you set ee_link_name in panda as panda_link7, the kinematics class gives an error because of this issue because of the mismatch between self.active_joint_indices (i.e. 0-6 for 7 joints) and self.active_ancestor_joint_idxs (i.e. 0-5 for 6 joints since joint 7 is ignored). :

python -m mani_skill.examples.demo_random_action -e "PushCube-v1" --render-mode="human" -c pd_ee_delta_pose

Traceback (most recent call last):
  File "/home/mjhwang/miniconda3/envs/maniskill/lib/python3.10/runpy.py", line 196, in _run_module_as_main
    return _run_code(code, main_globals, None,
  File "/home/mjhwang/miniconda3/envs/maniskill/lib/python3.10/runpy.py", line 86, in _run_code
    exec(code, run_globals)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/examples/demo_random_action.py", line 127, in <module>
    main(parsed_args)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/examples/demo_random_action.py", line 82, in main
    env: BaseEnv = gym.make(
  File "/home/mjhwang/miniconda3/envs/maniskill/lib/python3.10/site-packages/gymnasium/envs/registration.py", line 802, in make
    env = env_creator(**env_spec_kwargs)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/utils/registration.py", line 182, in make
    env = env_spec.make(**kwargs)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/utils/registration.py", line 79, in make
    return self.cls(**_kwargs)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/tasks/tabletop/push_cube.py", line 68, in __init__
    super().__init__(*args, robot_uids=robot_uids, **kwargs)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/sapien_env.py", line 327, in __init__
    obs, _ = self.reset(seed=[2022 + i for i in range(self.num_envs)], options=dict(reconfigure=True))
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/sapien_env.py", line 819, in reset
    self._reconfigure(options)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/sapien_env.py", line 656, in _reconfigure
    self._load_agent(options)
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/tasks/tabletop/push_cube.py", line 106, in _load_agent
    super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
  File "/home/mjhwang/Desktop/maniskill/mani_skill/envs/sapien_env.py", line 411, in _load_agent
    agent: BaseAgent = agent_cls(
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/base_agent.py", line 110, in __init__
    self.set_control_mode()
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/base_agent.py", line 236, in set_control_mode
    self.controllers[control_mode] = CombinedController(
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/controllers/base_controller.py", line 199, in __init__
    self.controllers[uid] = config.controller_cls(
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/controllers/base_controller.py", line 62, in __init__
    self._initialize_joints()
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/controllers/pd_ee_pose.py", line 39, in _initialize_joints
    self.kinematics = Kinematics(
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/controllers/utils/kinematics.py", line 69, in __init__
    self.controlled_joints_idx_in_qmask = [
  File "/home/mjhwang/Desktop/maniskill/mani_skill/agents/controllers/utils/kinematics.py", line 70, in <listcomp>
    self.active_ancestor_joint_idxs.index(idx)
ValueError: tensor(6, dtype=torch.int32) is not in list

Here, 6 is not in the list of [0,1,2,3,4,5] (active_ancestor_joint_idxs), which is missing the joint 7.

This can be simply fixed by just replacing the line 54 with cur_link = self.end_link. That is, we just start iterating from the end link not its parent link :)).

I apologize if my explanation wasn't clear. Let me know if this is a desired behavior, or if we should apply the suggested fix.

StoneT2000 commented 3 weeks ago

I think you are correct. I just checked and it seems this error didn't surface up before because we often controlled invisible links (marked as the TCP) which is connected via a fixed joint to an actual link.

Happy to accept a PR! Otherwise I'll merge a fix for this in a few days