Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Closed
mj-hwang opened this issue Oct 30, 2024 · 1 comment · Fixed by #705
Closed

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

mj-hwang opened this issue Oct 30, 2024 · 1 comment · Fixed by #705

Comments

@mj-hwang
Copy link

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
Copy link
Member

StoneT2000 commented Nov 1, 2024 •
edited
Loading

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

@StoneT2000 StoneT2000 mentioned this issue Nov 20, 2024
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging a pull request may close this issue.

[BugFix] Fix kinematics that sometimes will skip an active joint haosulab/ManiSkill
2 participants
@StoneT2000 @mj-hwang