How to enable/disable collisions



In [1]:

    
import sys
sys.path.append('../build')
import libry as ry
import numpy as np
import time



In [2]:

    
C = ry.Config()
C.addFile("../scenarios/pandasTable.g")
V = ry.ConfigurationViewer()
V.setConfiguration(C)



In [3]:

    
#no collisions so far:
C.computeCollisions() #this calls broadphase collision detection
C.getCollisions()     #this reports collisions (in algorithms, use features instead)









    Out[3]:





[]



In [4]:

    
#bring into collision:
q = C.getJointState()

for t in range(10):
    [y,J] = C.evalFeature(ry.FS.positionDiff, ["R_gripperCenter", "L_gripperCenter"])
    vel = J.T @ np.linalg.inv(J@J.T + 1e-2*np.eye(y.shape[0])) @ (-y);

    q = q + .8*vel
    C.setJointState(q)
    V.setConfiguration(C)



In [5]:

    
C.computeCollisions() #this calls broadphase collision detection
C.getCollisions(belowMargin=1.)









    Out[5]:





[('L_frame', 'R_frame', 0.06561977608755995),
 ('L_panda_coll7', 'R_frame', 0.04685827145208207),
 ('L_gripper', 'R_frame', 0.06693238739998014),
 ('L_finger1', 'R_frame', 0.07271677669653415),
 ('L_frame', 'R_panda_coll7', 0.05791468160266697),
 ('L_panda_coll7', 'R_panda_coll7', -0.02988786580078831),
 ('L_gripper', 'R_panda_coll7', -0.01623162839226778),
 ('L_finger1', 'R_panda_coll7', 0.008563418986308732),
 ('L_frame', 'R_gripper', 0.09727276329523714),
 ('L_panda_coll7', 'R_gripper', 0.0236455457862735),
 ('L_gripper', 'R_gripper', -0.02),
 ('L_finger1', 'R_gripper', 0.0025749560240463706),
 ('L_finger2', 'R_gripper', 0.06869685467148554),
 ('L_panda_coll7', 'R_finger1', 0.06687111592144941),
 ('L_gripper', 'R_finger1', 0.0417058924117476),
 ('L_finger1', 'R_finger1', 0.05018380967751282),
 ('L_finger2', 'R_finger1', 0.05793201309129997),
 ('L_panda_coll7', 'R_finger2', 0.06268928253009355),
 ('L_gripper', 'R_finger2', 0.039854984947338926),
 ('L_finger1', 'R_finger2', 0.057931593611096205),
 ('L_finger2', 'R_finger2', 0.050184233531723035)]



In [6]:

    
#those are the real penetrations:
C.getCollisions(belowMargin=0.)









    Out[6]:





[('L_panda_coll7', 'R_panda_coll7', -0.02988786580078831),
 ('L_gripper', 'R_panda_coll7', -0.01623162839226778),
 ('L_gripper', 'R_gripper', -0.02)]



In [7]:

    
#deactivate a collision flag!
f = C.getFrame("R_gripper")
f.setContact(0)



In [8]:

    
C.computeCollisions()
C.getCollisions(belowMargin=0.)









    Out[8]:





[('L_panda_coll7', 'R_panda_coll7', -0.02988786580078831),
 ('L_gripper', 'R_panda_coll7', -0.01623162839226778)]



In [9]:

    
#Note: this is exactly the sum of the above:
C.evalFeature(ry.FS.accumulatedCollisions, [])









    Out[9]:





(array([ 0.04611949]),
 array([[ -3.34276873e-01,  -2.32319413e-01,  -5.43192598e-01,
           5.11465561e-01,  -8.59540334e-02,   1.69571803e-01,
          -3.02775954e-03,   0.00000000e+00,  -2.31544435e-01,
           5.73228417e-01,   4.52182786e-01,  -4.96589031e-01,
           9.38756857e-03,  -8.77189874e-02,   1.50296671e-07,
           0.00000000e+00]]))



In [11]:

    
-0.02988786580078831-0.01623162839226778









    Out[11]:





-0.04611949419305609

Notes:

The contact flag of frames is an integer: 0 = does not contribute to contact lists, 1 = always contributes, negative = only contributes to collision pairs that are topologically distant in the kinematic tree
WARNING: the broad phase collision engine is created at first call - You cannot enable collisions between objects that were disabled during first call of the collision engine
TODO: enable recreation of a fresh collision engine for a changed configuration

How to attach frames - faking grasps

Note, this is not real grasping. Just editing the kinematic tree in your configuration



In [ ]:

    
import os
os._exit(0)



In [1]:

    
import sys
sys.path.append('../build')
import libry as ry
import numpy as np
import time



In [2]:

    
C = ry.Config()
C.addFile("../scenarios/pandasTable.g")
V = ry.ConfigurationViewer()
V.setConfiguration(C)



In [3]:

    
C.attach("L_gripper", "R_gripper")



In [6]:

    
#move a bit around

q = C.getJointState()

for t in range(30):
    q[0] = np.sin(t/10)
    
    C.setJointState(q)
    V.setConfiguration(C)
    time.sleep(0.1)



In [ ]: