Exemple #1
0
def init_ground_truth(world):
    loaded_item_infos = {}
    items = []
    for i in xrange(world.numRigidObjects()):
        n = world.rigidObject(i).getName()
        name,binname = n.split(',')
        if name not in loaded_item_infos:
            loaded_item_infos[name] = apc.load_item(n)
            load_item_infos[name].geometry = world.rigidObject(i).geometry()
        item = ItemInBin(loaded_item_infos[n],binname)
        item.xform = world.rigidObject(i).getTransform()
        items.append(item)
    ground_truth_shelf_xform = ([6.123233995736766e-17, -1.0, 0.0, 0.0, 0.0, 1.0, -1.0, -6.123233995736766e-17, 0.0], [1.4, 0.0, 0.01])
    perception.set_ground_truth(ground_truth_shelf_xform,items)
#load gripper modules
sys.path.append('../klampt_models')
import rethink_electric_gripper,reflex_col

grippermodules = {'reflex':reflex_col,
                  'rethink_electric_gripper':rethink_electric_gripper,
                  }
grippermodule = grippermodules[grippername]

world = WorldModel()
world.readFile(os.path.join('../klampt_models/',grippermodule.klampt_model_name))
robot = world.robot(0)

#load the items and spawn one in the world
apc.load_item(objectname,gripper=grippername)
item = apc.item_info[objectname]
item.geometry = apc.load_item_geometry(item)
itemposed = apc.ItemInBin(item,'blah')
itemposed.xform = se3.identity()
apc.spawn_item_in_world(itemposed,world)

#set the directory
print "Resouce directory is",os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0])
resource.setDirectory(os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0]))

#edit the default configurations
default_open_config = resource.get('default_open.config',
                                   description='Default hand open configuration',
                                   doedit='auto',
                                   world=world)