def _is_collision_free(self, gcc, item_name, graspable, grasp, pose):
"""
Makes sure a particular graspable is collision free
"""
gripper_pose = [pose[0][0], pose[0][1], GRIPPER_Z_POS]
rot_obj = autolab_core.RigidTransform.rotation_from_quaternion(pose[1])
rot_grip = autolab_core.RigidTransform.rotation_from_quaternion(
self.gripper_rotation)
world_to_obj = autolab_core.RigidTransform(rot_obj, pose[0], 'world',
'obj')
world_to_grip = autolab_core.RigidTransform(rot_grip, gripper_pose,
'world', 'gripper')
obj_to_world = world_to_obj.inverse()
obj_to_grip = world_to_grip.dot(obj_to_world)
# now have RigidTransform of gripper wrt object, so can pass to collision check
gcc.set_graspable_object(graspable)
grasp_collide = gcc.grasp_in_collision(obj_to_grip.inverse(),
item_name)
approach_collide = gcc.collides_along_approach(grasp,
self.cc_approach_dist,
self.cc_delta_approach,
item_name)
if grasp_collide or approach_collide:
#print "collision detected " + item_name
return False
return True
def grasp_to_world(objectInWorld, graspToObj):
rot_obj = autolab_core.RigidTransform.rotation_from_quaternion(
objectInWorld[1])
world_to_obj = autolab_core.RigidTransform(rot_obj, objectInWorld[0],
'world', 'obj')
obj_to_world = world_to_obj.inverse()
g_to_w = obj_to_world.dot(graspToObj)
return g_to_w
def _create_grasp_collision_checker(self, state):
gcc = dexnet.grasping.GraspCollisionChecker(self.gripper)
# Add all objects to the world frame
item_names = state['item_names']
poses = state['poses']
graspables = {}
for item_id, pose in poses.items():
graspable = self.dn.dataset.graspable(item_names[item_id])
rot_obj = autolab_core.RigidTransform.rotation_from_quaternion(
pose[1])
world_to_obj = autolab_core.RigidTransform(rot_obj, pose[0],
'world', 'obj')
obj_to_world = world_to_obj.inverse()
gcc.add_graspable_object(graspable, obj_to_world)
graspables[item_id] = graspable
return gcc, graspables
from kin_func_skeleton import rotation_3d
####
if __name__ == '__main__':
## Defining the transform between the box and the camera ##
rotation = np.array([[1, 0, 0], # 'random' values, they will need to be changed for the final setup
[0, -0.1979, 0.9802],
[0, -0.9802, -0.1979]])
translation = np.array([0.2, -0.8, 0.4])
rigid_transfo = autolab_core.RigidTransform(rotation=rotation, translation=translation, from_frame='camera', to_frame='world')
pawn = trimesh.exchange.load.load('pawn.obj')
pawn.fix_normals()
xs = []
ys = []
zs = []
## Plot the object in 3D ##
for vertex in pawn.vertices:
pos_in_world = np.matmul(rigid_transfo.matrix, np.array([vertex[0], vertex[1], vertex[2], 1]))
xs.append(pos_in_world[0])
ys.append(pos_in_world[1])