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.

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

[Bug Report] Invalid behavior of joints reset functions #980

Open
2 of 4 tasks
johnBuffer opened this issue Sep 12, 2024 · 2 comments · May be fixed by #1416
Open
2 of 4 tasks

[Bug Report] Invalid behavior of joints reset functions #980

johnBuffer opened this issue Sep 12, 2024 · 2 comments · May be fixed by #1416
Labels
bug Something isn't working

Comments

@johnBuffer
Copy link
Contributor

Describe the bug

The mdp.reset_joints_by_offset and mdp.reset_joints_by_scale functions cannot target specific joints and cannot be combined.

Steps to reproduce

When defining multiple reset events (as done in the Isaac-Cartpole-v0 task for example):

@configclass
class EventCfg:
    """Configuration for events."""

    # reset
    reset_cart_position = EventTerm(
        func=mdp.reset_joints_by_offset,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]),
            "position_range": (-1.0, 1.0),
            "velocity_range": (-0.5, 0.5),
        },
    )

    reset_pole_position = EventTerm(
        func=mdp.reset_joints_by_offset,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"]),
            "position_range": (-0.25 * math.pi, 0.25 * math.pi),
            "velocity_range": (-0.25 * math.pi, 0.25 * math.pi),
        },
    )

each EventTerm overrides previous ones and applies to all joints, unless I am missing something.

I think this happens because the starting point is always the default joints position / velocity and no filter is done, ignoring the joint_names attribute of the SceneEntityCfg parameter:

def reset_joints_by_scale(...):
    ...
    # get default joint state
    joint_pos = asset.data.default_joint_pos[env_ids].clone()
    joint_vel = asset.data.default_joint_vel[env_ids].clone()
    # bias these values randomly
    joint_pos += math_utils.sample_uniform(*position_range, joint_pos.shape, joint_pos.device)
    joint_vel += math_utils.sample_uniform(*velocity_range, joint_vel.shape, joint_vel.device)
    ...
    # set into the physics simulation
    asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)

To overcome this problem I defined a reset_joints_to_default function

def reset_joints_to_default(
    env: ManagerBasedEnv,
    env_ids: torch.Tensor,
    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
):
    """Reset the robot joints to default position and velocity"""

    # extract the used quantities (to enable type-hinting)
    asset: Articulation = env.scene[asset_cfg.name]

    # get default joint state
    joint_pos = asset.data.default_joint_pos[env_ids].clone()
    joint_vel = asset.data.default_joint_vel[env_ids].clone()

    # set into the physics simulation
    asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)

that I use in the first EventTerm and then use a modified reset_joints_by_offset in subsequent EventTerm.

def reset_joints_by_offset(...):
    ...
    # get current joint state
    joint_pos_all = asset.data.joint_pos[env_ids]
    joint_vel_all = asset.data.joint_vel[env_ids]

    # fetch targeted joints
    joint_pos = joint_pos_all[:, asset_cfg.joint_ids]
    joint_vel = joint_vel_all[:, asset_cfg.joint_ids]

    # bias these values randomly
    joint_pos += math_utils.sample_uniform(*position_range, joint_pos.shape, joint_pos.device)
    joint_vel += math_utils.sample_uniform(*velocity_range, joint_vel.shape, joint_vel.device)

    # update joint state
    joint_pos_all[:, asset_cfg.joint_ids] = joint_pos
    joint_vel_all[:, asset_cfg.joint_ids] = joint_vel
    ...
    # set into the physics simulation
    asset.write_joint_state_to_sim(joint_pos_all, joint_vel_all, env_ids=env_ids)

System Info

Additional context

If you think the solution I currently use is valid, I can create a pull request with this implementation (it seems to be working well on my side).

Checklist

  • I have checked that there is no similar issue in the repo (required)
  • I have checked that the issue is not in running Isaac Sim itself and is related to the repo

Acceptance Criteria

Add the criteria for which this task is considered done. If not known at issue creation time, you can add this once the issue is assigned.

  • It is possible to target specific joints in the reset_joints_by_offset or reset_joints_by_scale functions.
  • It is possible to combine multiple EventTerm objects that use the reset_joints_by_offset or reset_joints_by_scale functions.
@kellyguo11
Copy link
Contributor

Thanks for reporting this! Would be great if you could create a PR for the fix!

@kellyguo11 kellyguo11 added the bug Something isn't working label Oct 4, 2024
@johnBuffer
Copy link
Contributor Author

Yes sure, I will create it soon.

@johnBuffer johnBuffer linked a pull request Nov 13, 2024 that will close this issue
6 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

Successfully merging a pull request may close this issue.

2 participants