alberthli / frogger Goto Github PK

The user should have more control over how to handle collision constraints. We can implement this by exposing a callback to the user that takes in pairs of geometry names and returns the allowable signed distance between them (up to some perhaps global tolerance). This would offer much more fine-grained control over which geometries are allowed to be close and which aren't. It seamlessly integrates with our current situation, which is to assume a global allowable distance independent of the geom names. It could potentially also simplify the requirement that the URDF is edited with the substring "FROGGERCOL" to be compatible with this tool, which is an annoyance.

model.R_cf_O and model.n_O (fingertip contact frame in object frame)

Shapes:

model.R_cf_O.shape == (4, 3, 3)
model.n_O.shape == (3, 4)

4 is for 4 fingers

Here is an example grasp:

Here I visualize:

• world frame (far in the distance)
• object frame (bottom of object)
• wrist frame (in the image, behind the can and to the right slightly)
• fingertip frames (3 along the right of can, thumb on left of can)
  This all makes sense
  

Now I just visualize world frame and the R_cf_O (translated to be at each fingertip, rotated to world frame X_W_O[:3, :3] @ model.R_cf_O[i])
The second image shows that something is off. They are all pointed in almost the same direction. It should be z (blue) lines along the object normals, but they look off

Raw values:
model.R_cf_O = 
[[[ 0.77113781  0.32720713  0.54615197]
  [ 0.50889554 -0.83226419 -0.2199128 ]
  [ 0.38258569  0.44751738 -0.80830463]]

 [[ 0.76323557  0.38757045  0.51697254]
  [ 0.54062041 -0.82123916 -0.18247139]
  [ 0.35383758  0.41875457 -0.83632744]]

 [[ 0.76353962  0.43236811  0.47965099]
  [ 0.56236164 -0.81030788 -0.16477419]
  [ 0.31742187  0.39554894 -0.8618494 ]]

 [[ 0.66124914  0.32325077  0.67694794]
  [ 0.53843989 -0.83284574 -0.12825933]
  [ 0.52233328  0.44930714 -0.72476964]]]

All 4 transforms are very similar. The thumb one should almost certainly be quite different

I expected something like this, where the z-dirs of R_cf_O (3rd columns, which I checked is same direction as n_O, R_cf_O[finger_idx, :, 2] == n_O[:, finger_idx])

Valeus check

Some meshes give the following error:

RuntimeError: Spatial inertia fails SpatialInertia::IsPhysicallyValid().
mass = -0.0343898626546023 is not positive and finite.

In robot_core.py

    def _load_object(self) -> None:
        """Loads the object into the plant."""
        # registering object's visual/collision geoms with the plant
        assert None not in [self.obj.shape_visual, self.obj.shape_collision_list]
        self.obj_instance = self.plant.AddModelInstance("obj")
        I_o = self.obj.inertia / self.obj.mass
        self.obj_body = self.plant.AddRigidBody(
            "obj",
            self.obj_instance,
            SpatialInertia(
                self.obj.mass,
                np.zeros(3),
                UnitInertia(
                    I_o[0, 0], I_o[1, 1], I_o[2, 2], I_o[0, 1], I_o[0, 2], I_o[1, 2]
                ),
            ),
        )

This tries to compute mass/inertia through the mesh, but if the mesh has issues, these could be very wrong. mass does not directly affect grasp planning, so this shouldn't change the behavior. Adding a flag for this would allow a user to change mass to avoid bugs and not assume constant density.