Could add safe distance in grasp_bottle.py ?

[chuangwang1991]

In the example "grasp_bottle.py", the robot plans path without safety distance.

######### setup OMPL ##########
        space = ob.RealVectorStateSpace(self.n_qs)
        bounds = ob.RealVectorBounds(self.n_qs)

        for i_q in range(self.n_qs):
            bounds.setLow(i_q, q_limit_lower[i_q])
            bounds.setHigh(i_q, q_limit_upper[i_q])
        space.setBounds(bounds)
        ss = og.SimpleSetup(space)
        if ignore_collision:
            ss.setStateValidityChecker(ob.StateValidityCheckerFn(lambda state: True))
        else:
            ss.setStateValidityChecker(ob.StateValidityCheckerFn(self._is_ompl_state_valid))

def _is_ompl_state_valid(self, state):
       self.set_qpos(self._ompl_state_to_tensor(state))
       collision_pairs = self.detect_collision()

       if len(collision_pairs) > 0:
           return False
       else:
           return True

Could add safe distance during planning path in the example, and show how to get minimum distance between robot and env objects ?

[zhouxian]

Do you think you can send a PR? Would be helpful!

[chuangwang1991]

I found the "contact data" definition in "/genesis/engine/solvers/rigid/collider.py" row 99-115, I will try return and check the penetration value in detection function

############## narrow phase ##############
        struct_contact_data = ti.types.struct(
            geom_a=gs.ti_int,
            geom_b=gs.ti_int,
            penetration=gs.ti_float,
            normal=gs.ti_vec3,
            pos=gs.ti_vec3,
            friction=gs.ti_float,
            sol_params=gs.ti_vec7,
            force=gs.ti_vec3,
            link_a=gs.ti_int,
            link_b=gs.ti_int,
        )
        self.contact_data = struct_contact_data.field(
            shape=self._solver._batch_shape(max(1, self._max_contact_pairs)),
            layout=ti.Layout.SOA,
        )