isaac-sim / IsaacLab

Unified framework for robot learning built on NVIDIA Isaac Sim
https://isaac-sim.github.io/IsaacLab
Other
2.33k stars 966 forks source link

[Bug Report] Removing a link in a USD causes the Articulation to confuse links (?) #389

Closed dhajnes closed 1 month ago

dhajnes commented 7 months ago

Describe the bug

I have a quadruped robot. I deleted some links from front left leg, I left in the xform for the attachement point of the deleted part, just there is no mesh and no following links. I was using this XForm to apply force to end of the leg for benchmarking purposes. This worked when I was on the previous version 2023.1.0-hotfix.1, and commit of Orbit repo: 620ce2b0 Fixes source frame indexing in FrameTransfomer sensor (#350).

Steps to reproduce

1) Delete foot and lower link of robot leg, while keeping only the xform where the lower link was attached. (the robot is with a fixed joint - I tested putting the fixed joint inside of base xform or outside the base xform with no change of behavior) 2) spawn the robot as described in source/standalone/demos/quadrupeds.py 3) apply external force as shown below 4) wrong leg lifts - right middle link, instead of the left (in previous commit and IsaacSim version, the correct leg lifted)

# APPLY IN A LOOP

body_ids, body_names = robot.find_bodies("front_left_link_C") 
print(f"body_ids: {body_ids}")
>> body_ids: [9]
print(f"body_names: {body_names}")
>> body_names: ['front_left_link_C']

# you can see that the correct body_id should be collected

# Sample a large force
external_wrench_b = torch.zeros(robot.num_instances, len(body_ids), 6, device=sim.device)
external_wrench_b[..., 0] = FORCE
robot.set_external_force_and_torque(
external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
)
...
# write data to sim
robot.write_data_to_sim()

System Info

Describe the characteristic of your environment:

Additional context

Upon further investigation of what the robot variable from quadrupeds.py holds, I noticed where the problem might be: This is what robot.root_physx_view.dof_paths sees:

NOTE numbers were added for clarity, its a list in the implementation

00:'/World/Origin0/Robot_0/base',
01:'/World/Origin0/Robot_0/front_left_link_A',
02:'/World/Origin0/Robot_0/front_right_link_A',
03:'/World/Origin0/Robot_0/rear_left_link_A',
04:'/World/Origin0/Robot_0/rear_right_link_A',
05:'/World/Origin0/Robot_0/front_left_link_B',
06:'/World/Origin0/Robot_0/front_right_link_B',
07:'/World/Origin0/Robot_0/rear_left_link_B',
08:'/World/Origin0/Robot_0/rear_right_link_B',
09:'/World/Origin0/Robot_0/front_left_link_C',
10:'/World/Origin0/Robot_0/front_right_link_C',
11:'/World/Origin0/Robot_0/rear_left_link_C',
12:'/World/Origin0/Robot_0/rear_right_link_C',
13:'/World/Origin0/Robot_0/front_right_link_D',
14:'/World/Origin0/Robot_0/rear_left_link_D'
15:'/World/Origin0/Robot_0/rear_right_link_D'

This seems correct. However, robot._body_view_ordering says:

tensor([ 0,  1,  4,  8, 12,  2,  5,  9, 13,  3,  6, 10, 14,  7, 11, 15],
device='cuda:0')

This suggests, that the simulation sees the articulation tree as:

/World/Origin0/Robot_0/base

/World/Origin0/Robot_0/front_left_link_A
/World/Origin0/Robot_0/rear_right_link_A
/World/Origin0/Robot_0/rear_right_link_B
/World/Origin0/Robot_0/rear_right_link_C

/World/Origin0/Robot_0/front_right_link_A
/World/Origin0/Robot_0/front_left_link_B
/World/Origin0/Robot_0/front_left_link_C
/World/Origin0/Robot_0/front_right_link_D

/World/Origin0/Robot_0/rear_left_link_A
/World/Origin0/Robot_0/front_right_link_B
/World/Origin0/Robot_0/front_right_link_C
/World/Origin0/Robot_0/rear_left_link_D

/World/Origin0/Robot_0/rear_left_link_B
/World/Origin0/Robot_0/rear_left_link_C
/World/Origin0/Robot_0/rear_right_link_D

Which is not correct in multiple ways. The right articulations would be something like:

[base, 
FLA, FLB, FLC,
FRA, FRB, FRC, FRD, 
RLA, RLB, RLC, RLD, 
RRA, RRB, RRC, RRD]`

Also, another possibly related error/warning: [Error] [omni.isaac.orbit.assets.articulation.articulation] The attribute 'body_physx_view' will be removed in v0.4.0. Please use 'root_physx_view' instead. I am not accessing this myself, so I suppose it relates to the source code of Articulation?

Checklist

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.

Dhoeller19 commented 1 month ago

Hi @dhajnes, Could you check if this is fixed with the latest Isaac Lab and Sim?