haosulab / ManiSkill

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

[Question]The bounding box of the articulations seems incorrect #645

Open DevinQiao opened 1 week ago

DevinQiao commented 1 week ago

I want to load some URDF or MJCF models in ManiSkill, and the positions of these models are calculated. In my calculations, I assumed the position of the model is the center of its bounding box (bbox), but when loading the model, it doesn't appear in the desired position. I printed some information using the code below.

mesh = articulation.get_first_collision_mesh()
min_bbox, max_bbox = mesh.bounds
bbox = max_bbox - min_bbox
center = (max_bbox + min_bbox) / 2
print(f"{urdf_loader.name=}")
print(f"{pos=}")
print(f"{max_bbox=}")
print(f"{min_bbox=}")
print(f"{bbox=}")
print(f"{center=}")

For the scene "ReplicaCAD", output is below:

 urdf_loader.name='scs-[0]_fridge-0'
pos=[-2.1782121658325195, 0.9755649566650391, 3.2299728393554688]
max_bbox=array([0.384, 0.962, 0.354])
min_bbox=array([-0.331, -0.962, -0.358])
bbox=array([0.715, 1.923, 0.712])
center=array([ 0.026, -0.   , -0.002])
urdf_loader.name='scs-[0]_kitchen_counter-1'
pos=[-2, 0.04, 1.2346]
max_bbox=array([0.166, 0.941, 1.543])
min_bbox=array([-0.49 , -0.011, -1.488])
bbox=array([0.656, 0.952, 3.03 ])
center=array([-0.162,  0.465,  0.028])
urdf_loader.name='scs-[0]_kitchenCupboard_01-2'
pos=[-2.3, 1.54, 1.2346]
max_bbox=array([0.267, 1.526, 0.997])
min_bbox=array([-0.23 , -1.52 , -0.002])
bbox=array([0.497, 3.046, 0.998])
center=array([0.019, 0.003, 0.498])
urdf_loader.name='scs-[0]_chestOfDrawers_01-3'
pos=[-2.318626880645752, 0, -1.3263527154922485]
max_bbox=array([0.179, 0.445, 1.277])
min_bbox=array([-0.16 , -0.43 , -0.004])
bbox=array([0.339, 0.875, 1.281])
center=array([0.01 , 0.007, 0.637])
urdf_loader.name='scs-[0]_cabinet-4'
pos=[0.4406132400035858, 0.04022252857685089, -0.3957761228084564]
max_bbox=array([0.764, 0.764, 0.215])
min_bbox=array([-0.84 ,  0.165, -0.228])
bbox=array([1.604, 0.599, 0.443])
center=array([-0.038,  0.465, -0.006])
urdf_loader.name='scs-[0]_door2-5'
pos=[-0.35, 2.4, -2.65]
max_bbox=array([ 0.023, -0.05 ,  0.905])
min_bbox=array([-0.012, -2.35 ,  0.055])
bbox=array([0.035, 2.3  , 0.85 ])
center=array([ 0.005, -1.2  ,  0.48 ])
Observation space Dict()
Action space Box(-1.0, 1.0, (13,), float32)
Control mode pd_joint_delta_pos
Reward mode normalized_dense
urdf_loader.name='scs-[0]_fridge-0'
pos=[-2.1782121658325195, 0.9755649566650391, 3.2299728393554688]
max_bbox=array([0.384, 0.962, 0.354])
min_bbox=array([-0.331, -0.962, -0.358])
bbox=array([0.715, 1.923, 0.712])
center=array([ 0.026, -0.   , -0.002])
urdf_loader.name='scs-[0]_kitchen_counter-1'
pos=[-2, 0.04, 1.2346]
max_bbox=array([0.166, 0.941, 1.543])
min_bbox=array([-0.49 , -0.011, -1.488])
bbox=array([0.656, 0.952, 3.03 ])
center=array([-0.162,  0.465,  0.028])
urdf_loader.name='scs-[0]_kitchenCupboard_01-2'
pos=[-2.3, 1.54, 1.2346]
max_bbox=array([0.267, 1.526, 0.997])
min_bbox=array([-0.23 , -1.52 , -0.002])
bbox=array([0.497, 3.046, 0.998])
center=array([0.019, 0.003, 0.498])
urdf_loader.name='scs-[0]_chestOfDrawers_01-3'
pos=[-2.318626880645752, 0, -1.1]
max_bbox=array([0.179, 0.445, 1.277])
min_bbox=array([-0.16 , -0.43 , -0.004])
bbox=array([0.339, 0.875, 1.281])
center=array([0.01 , 0.007, 0.637])
urdf_loader.name='scs-[0]_cabinet-4'
pos=[0.5838254690170288, 0.04, 7.1580729484558105]
max_bbox=array([0.764, 0.764, 0.215])
min_bbox=array([-0.84 ,  0.165, -0.228])
bbox=array([1.604, 0.599, 0.443])
center=array([-0.038,  0.465, -0.006])
urdf_loader.name='scs-[0]_door2-5'
pos=[-0.35, 2.4, -2.65]
max_bbox=array([ 0.023, -0.05 ,  0.905])
min_bbox=array([-0.012, -2.35 ,  0.055])
bbox=array([0.035, 2.3  , 0.85 ])
center=array([ 0.005, -1.2  ,  0.48 ])

So, if I want the URDF model to appear in the desired location, how should I set it up? How can I calculate their offset?

StoneT2000 commented 1 week ago

For MJCF I am not completely sure if it will work 100% correctly. I am currently importing the robocasa dataset and found a few bugs in the MJCF loader as a result

For URDF it should work fine. How did you retrieve the bboxes in code?

DevinQiao commented 1 week ago

I use the following code to create each articulation in the scene, then use get_first_collision_mesh() to obtain its mesh. Using mesh.bounds, I get min_bbox and max_bbox, and then subtract min_bbox from max_bbox to obtain bbox. However, I think the values of max_bbox and min_bbox are not quite correct. I think their values should be close to position + bbox/2 and position - bbox/2, respectively.

for art in arts:
            urdf_path = osp.join('data/dataset', str(art['path']), 'mobility.urdf')
            rot = np.array(art["rotation"])
            angles_radians = np.radians(rot)
            quat = euler2quat(angles_radians[0], angles_radians[1], angles_radians[2] - np.pi / 2)
            pos = art["position"]
            if rot[2] == 180:
                offset = np.array([-art["offset"][1], -art["offset"][0], art["offset"][2]])
            elif rot[2] == 270:
                offset = np.array([-art["offset"][0], -art["offset"][1], art["offset"][2]])
            elif rot[2] == 0:
                offset = np.array([art["offset"][1], art["offset"][0], art["offset"][2]])
            else:
                offset = np.array(art["offset"])
            pos -= offset
            urdf_loader = self.scene.create_urdf_loader()
            urdf_loader.name = art["id"]
            urdf_loader.scale = art["scale"]
            articulation = urdf_loader.load(urdf_path)
            articulation.set_pose(sapien.Pose(pos, quat))
            mesh = articulation.get_first_collision_mesh()
            min_bbox, max_bbox = mesh.bounds
            bbox = max_bbox - min_bbox
            center = (max_bbox + min_bbox) / 2
            print(f"{urdf_loader.name=}")
            print(f"{pos=}")
            print(f"{max_bbox=}")
            print(f"{min_bbox=}")
            print(f"{bbox=}")
            print(f"{center=}")

Perhaps you can load /data/dataset/47391/mobility.urdf in partnet_mobility. The related info I used is as follows.

loader = self.scene.create_urdf_loader()
loader.scale = 0.743
articulation = loader.load(fp)
articulation.set_pose(sapien.Pose([5.121, 5.41 , 2.47], [1, 0, 0, 0]))
mesh = articulation.get_first_collision_mesh()
min_bbox, max_bbox = mesh.bounds
bbox = max_bbox - min_bbox
center = (max_bbox + min_bbox) / 2
print(f"{max_bbox=}")
print(f"{min_bbox=}")
print(f"{bbox=}")
print(f"{center=}")

And it outputs:

max_bbox=array([10.42297376, 11.41342718,  5.3782044 ])
min_bbox=array([10.00989236, 10.20651197,  4.51612149])
bbox=array([0.4130814 , 1.20691521, 0.86208291])
center=array([10.21643306, 10.80996957,  4.94716294])

The max_bbox and min_bbox and larger than the pos.

StoneT2000 commented 6 days ago

Is the code you shared in the def _load_scene function? And you are testing in the GPU sim?

If it is the GPU sim, the bounding box info generated in the load_scene function will likely not be centered at the right place since we initialize the GPU poses based on the articulation initial pose.