class testingWorldBuilder(): def __init__(self, floor_length=20, floor_width=20): self.w = WorldModel() self.floor = Geometry3D() self.floor.loadFile(model_path + "cube.off") self.floor.transform( [floor_length, 0, 0, 0, floor_width, 0, 0, 0, 0.01], [-floor_length / 2.0, -floor_width / 2.0, -0.01]) floor_terrain = self.w.makeTerrain("floor") floor_terrain.geometry().set(self.floor) floor_terrain.appearance().setColor(0.4, 0.3, 0.2, 1.0) ###colors self.light_blue = [3.0 / 255.0, 140.0 / 255.0, 252.0 / 255.0, 1.0] self.wall_green = [50.0 / 255.0, 168.0 / 255.0, 143.0 / 255.0, 1.0] ###sizes self.table_length = 1.0 self.table_width = 0.5 self.table_top_thickness = 0.03 self.table_height = 0.6 self.leg_width = 0.05 self.cube_width = 0.05 def getWorld(self): return self.w def addTableTopScenario(self, x=0, y=0): """ add a table with objects on top, the center of table can be set Parameters: -------------- x,y: floats, the position of the table center """ self.addTable(x, y) # add some cubes ''' self.addCube((so3.from_axis_angle(([0,0,1],0.5)),[x/2,y/2,self.table_height]), self.cube_width, [1.0,0,0,1],1) # add one mesh self.addRandomMesh([0+x,0.2+y,self.table_height],1) self.addRandomMesh([0+x,-0.2+y,self.table_height],2) self.addRandomMesh([0.2+x,0+y,self.table_height],3) self.addRandomMesh([-0.2+x,-0.2+y,self.table_height],4) ''' ##Functions below add individual objects def addCube(self, T, side_length, color, ID, object_mass=0.1): """ Add a cube to the world. Parameters: -------------- T:world transform of the cube side_length: float, size of the cube color: RGBA values, (0-1) ID: int, cannot duplicate mass:object mass added at the object geometric center """ self._addRigidObject(model_path + "cube.off",([side_length,0,0,0,side_length,0,0,0,side_length,],[0,0,0]),T,\ color,object_mass,[side_length/2.0,side_length/2.0,side_length/2.0],"cube"+str(ID)) def addMesh(self, path, T, color, mass, ID): """ Add a mesh to the world. Parameters: -------------- path: path to the mesh file T:world transform of the mesh color: RGBA values, (0-1) mass:object mass added at the object geometric center ID: int, cannot duplicate """ mesh = trimesh.load(path) mesh_center = mesh.centroid.tolist() # load the rigid object in the world self._addRigidObject(path,([1,0,0,0,1,0,0,0,1],[0]*3),T,\ color,mass,mesh_center,"item"+str(ID)) def addRandomMesh(self, t, ID=1): """ Add a household item to the world, randonmly selected from the library. Color is also determined randomly. Mass set to 1kg arbitrarily Parameters: -------------- t:world position of the mesh ID: int, cannot duplicate """ meshpaths = [] for file in os.listdir(mesh_model_path): if file.endswith(".ply"): meshpaths.append(os.path.join(mesh_model_path, file)) meshpath = random.choice(meshpaths) mesh = trimesh.load(meshpath) mesh_center = mesh.centroid.tolist() #Z_min = np.min(mesh.vertices[:,2]) #t[2] = t[2]+mesh_center[2]-Z_min # load the rigid object in the world self._addRigidObject(meshpath,([1,0,0,0,1,0,0,0,1],[0]*3),([1,0,0,0,1,0,0,0,1],t),\ (random.random(),random.random(),random.random(),1.0),0.1,mesh_center,"item"+str(ID)) def addTable(self, x=0, y=0): """ Add a table to the world Parameters: -------------- x,y: floats, the x,y position of the center of the table """ table_top = Geometry3D() table_leg_1 = Geometry3D() table_leg_2 = Geometry3D() table_leg_3 = Geometry3D() table_leg_4 = Geometry3D() table_top.loadFile(model_path + "cube.off") table_leg_1.loadFile(model_path + "cube.off") table_leg_2.loadFile(model_path + "cube.off") table_leg_3.loadFile(model_path + "cube.off") table_leg_4.loadFile(model_path + "cube.off") table_top.transform([self.table_length,0,0,0,self.table_width,0,0,0,\ self.table_top_thickness],[0,0,self.table_height - self.table_top_thickness]) table_leg_1.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height\ - self.table_top_thickness],[0,0,0]) table_leg_2.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height - \ self.table_top_thickness],[self.table_length-self.leg_width,0,0]) table_leg_3.transform([ self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0, self.table_height - self.table_top_thickness ], [ self.table_length - self.leg_width, self.table_width - self.leg_width, 0 ]) table_leg_4.transform([ self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0, self.table_height - self.table_top_thickness ], [0, self.table_width - self.leg_width, 0]) table_geom = Geometry3D() table_geom.setGroup() for i, elem in enumerate( [table_top, table_leg_1, table_leg_2, table_leg_3, table_leg_4]): g = Geometry3D(elem) table_geom.setElement(i, g) #table_geom.transform([1,0,0,0,1,0,0,0,1],[x-self.table_length/2.0,y-self.table_width/2.0,0]) table_geom.transform( so3.from_rpy([0, 0, math.pi / 2]), [x - self.table_length / 2.0, y - self.table_width / 2.0, 0]) table = self.w.makeTerrain("table") table.geometry().set(table_geom) table.appearance().setColor(self.light_blue[0], self.light_blue[1], self.light_blue[2], self.light_blue[3]) def addRobot(self, path, T): """ Add a robot to the world. You can directly use Klampt functions to add to the world as well Parameters: ------------ path: path to the robot model T: transform of the base of the robot """ self.w.loadRobot(path) rob = self.w.robot(0) #rob.link(3).setTransform(T[0], T[1]) def _addRigidObject(self, path, T_g, T_p, color, object_mass, Com, name): item_1_geom = Geometry3D() item_1_geom.loadFile(path) item_1_geom.transform(T_g[0], T_g[1]) item_1 = self.w.makeRigidObject(name) item_1.geometry().set(item_1_geom) item_1.appearance().setColor(color[0], color[1], color[2], color[3]) bmass = item_1.getMass() bmass.setMass(object_mass) bmass.setCom(Com) item_1.setMass(bmass) item_1.setTransform(T_p[0], T_p[1]) return item_1 def _addTerrain(self, path, T, color, name): item_1_geom = Geometry3D() item_1_geom.loadFile(path) item_1_geom.transform(T[0], T[1]) item_1 = self.w.makeTerrain(name) item_1.geometry().set(item_1_geom) item_1.appearance().setColor(color[0], color[1], color[2], color[3]) return item_1
def browse_database(gripper, dbfile=None): from known_grippers import GripperInfo g = GripperInfo.get(gripper) if g is None: print("Invalid gripper, valid names are", list(GripperInfo.all_grippers.keys())) exit(1) db = GraspDatabase(g) if dbfile is not None: if dbfile.endswith('.json'): db.load(dbfile) else: db.loadfolder(dbfile) data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '')) FIND_PATTERNS = [ data_dir + "/objects/%s.obj", data_dir + "/objects/ycb-select/%s/textured.obj" ] def _find_object(name): for pat in FIND_PATTERNS: fn = pat % (name, ) if os.path.exists(fn): return fn return None w = WorldModel() if not w.readFile(g.klampt_model): print("Can't load gripper robot model", g.klampt_model) exit(1) robot = w.robot(0) for o in db.objects: fn = _find_object(o) if fn is None: print("Can't find object", o, "in usual paths...") continue if w.numRigidObjects() == 0: obj = w.makeRigidObject(o) if not obj.loadFile(fn): if not obj.geometry().loadFile(fn): print("Couldn't load object", o, "from", fn) exit(1) obj.setTransform(*se3.identity()) if len(db.objects) == 0: print("Can't show anything, no objects") print("Try adding some grasps to the database using grasp_edit.py") exit(0) data = dict() data['cur_object'] = 0 data['cur_grasp'] = -1 data['shown_grasps'] = [] vis.add(db.objects[data['cur_object']], w.rigidObject(0)) def shift_object(amt, data=data): vis.remove(db.objects[data['cur_object']]) data['cur_object'] += amt if data['cur_object'] >= len(db.objects): data['cur_object'] = 0 elif data['cur_object'] < 0: data['cur_object'] = len(db.objects) - 1 if data['cur_object'] >= w.numRigidObjects(): for i in range(w.numRigidObjects(), data['cur_object'] + 1): o = db.objects[i] fn = _find_object(o) obj = w.makeRigidObject(o) if not obj.loadFile(fn): if not obj.geometry().loadFile(fn): print("Couldn't load object", o, "from", fn) exit(1) obj.setTransform(*se3.identity()) obj = w.rigidObject(data['cur_object']) vis.add(db.objects[data['cur_object']], obj) shift_grasp(None) def shift_grasp(amt, data=data): for i, grasp in data['shown_grasps']: grasp.remove_from_vis("grasp" + str(i)) data['shown_grasps'] = [] all_grasps = db.object_to_grasps[db.objects[data['cur_object']]] if amt == None: data['cur_grasp'] = -1 else: data['cur_grasp'] += amt if data['cur_grasp'] >= len(all_grasps): data['cur_grasp'] = -1 elif data['cur_grasp'] < -1: data['cur_grasp'] = len(all_grasps) - 1 if data['cur_grasp'] == -1: for i, grasp in enumerate(all_grasps): grasp.ik_constraint.robot = robot grasp.add_to_vis("grasp" + str(i)) data['shown_grasps'].append((i, grasp)) print("Showing", len(data['shown_grasps']), "grasps") else: grasp = all_grasps[data['cur_grasp']] grasp.ik_constraint.robot = robot grasp.add_to_vis("grasp" + str(data['cur_grasp'])) Tbase = grasp.ik_constraint.closestMatch(*se3.identity()) g.add_to_vis(robot, animate=False, base_xform=Tbase) robot.setConfig(grasp.set_finger_config(robot.getConfig())) data['shown_grasps'].append((data['cur_grasp'], grasp)) if grasp.score is not None: vis.addText("score", "Score %.3f" % (grasp.score, ), position=(10, 10)) else: vis.addText("score", "", position=(10, 10)) vis.addAction(lambda: shift_object(1), "Next object", '.') vis.addAction(lambda: shift_object(-1), "Prev object", ',') vis.addAction(lambda: shift_grasp(1), "Next grasp", '=') vis.addAction(lambda: shift_grasp(-1), "Prev grasp", '-') vis.addAction(lambda: shift_grasp(None), "All grasps", '0') vis.add("gripper", w.robot(0)) vis.run()
for i,elem in enumerate([left,right,back,bottom,top]): g = Geometry3D(elem) shelfgeom.setElement(i,g) shelf = world.makeTerrain("shelf") shelf.geometry().set(shelfgeom) shelf.appearance().setColor(0.2,0.6,0.3,1.0) return shelf w = WorldModel() if not w.readFile("myworld.xml"): raise RuntimeError("Couldn't read the world file") shelf = make_shelf(w,*shelf_dims) shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height)) obj = w.makeRigidObject("point_cloud_object") #Making Box obj.geometry().loadFile(KLAMPT_Demo+"/data/objects/apc/genuine_joe_stir_sticks.pcd") #set up a "reasonable" inertial parameter estimate for a 200g object m = obj.getMass() m.estimate(obj.geometry(),0.200) obj.setMass(m) #we'll move the box slightly forward so the robot can reach it obj.setTransform(so3.identity(),[shelf_offset_x-0.05,shelf_offset_y-0.3,shelf_height+0.01]) vis.add("world",w) vis.add("ghost",w.robot(0).getConfig(),color=(0,1,0,0.5)) vis.edit("ghost") from klampt import Simulator sim = Simulator(w)
#add a floor # floor = Geometry3D() # floor.loadFile("cube.off") # floor_length = 0.2 # floor_width = 0.2 # floor.transform([floor_length,0,0,0,floor_width,0,0,0,0.01],[-floor_length/2.0,-floor_width/2.0,-0.01]) # floor_terrain = w.makeTerrain("floor") # floor_terrain.geometry().set(floor) # floor_terrain.appearance().setColor(0.4,0.3,0.2,1.0) #add the probe item_1_geom = Geometry3D() item_1_geom.loadFile('Contact_Probe_linear2.STL') #item_1_geom.transform(T_g[0],T_g[1]) item_1 = w.makeRigidObject('probe') item_1.geometry().set(item_1_geom) item_1.appearance().setColor(1, 0, 0, 0.2) ###################Load data for line probe################################################# exp_path = '../data_final/exp_' + str(exp_N) + '/' exp_path_2 = '../data_final/exp_' + str(exp_N) + '_debiased/' #load line geometry data lineStarts, lineEnds, lineNormals, lineTorqueAxes = cal_line_data(exp_path) #load visual model pcd = load_pcd(exp_path_2 + 'originalPcd.txt', pcdtype='return_lines') points = np.array(pcd) ##calculate the range max_range = max([(np.max(points[:, 0]) - np.min(points[:, 0])), (np.max(points[:, 1]) - np.min(points[:, 1])),
if __name__ == '__main__': import sys import os if len(sys.argv) < 3: print("USAGE: python grasp_edit gripper_name OBJECT_FILE [grasp file]") exit(1) from known_grippers import * g = GripperInfo.get(sys.argv[1]) w = WorldModel() res = w.readFile(sys.argv[2]) if not res: basename = os.path.splitext(os.path.basename(sys.argv[2]))[0] obj = w.makeRigidObject(basename) if not obj.loadFile(sys.argv[2]): if not obj.geometry().loadFile(sys.argv[2]): print("Unable to read object", sys.argv[2]) exit(1) obj = w.rigidObject(0) grasp = None if len(sys.argv) >= 4: with open(sys.argv[3], 'r') as f: jsonobj = json.load(f) grasp = Grasp() grasp.fromJson(jsonobj) grasp = grasp_edit_ui(g, obj, grasp) print("Resulting grasp:") print(json.dumps(grasp.toJson(), indent=2))
fn = pat%(name,) if os.path.exists(fn): return fn return None w = WorldModel() if not w.readFile(g.klampt_model): print("Can't load gripper robot model",g.klampt_model) exit(1) robot = w.robot(0) for o in db.objects: fn = _find_object(o) if fn is None: print("Can't find object",o,"in usual paths...") continue obj = w.makeRigidObject(o) if not obj.loadFile(fn): if not obj.geometry().loadFile(fn): print("Couldn't load object",o,"from",fn) exit(1) if len(db.objects)==0: print("Can't show anything, no objects") print("Try adding some grasps to the database using grasp_edit.py") exit(0) cur_object = 0 cur_grasp = -1 shown_grasps = [] vis.add(db.objects[cur_object],w.rigidObject(cur_object)) def shift_object(amt): global cur_object,cur_grasp,shown_grasps,db
from klampt import WorldModel, Geometry3D, GeometricPrimitive, VolumeGrid, PointCloud, Appearance from klampt.vis import GLProgram, camera, gldraw from klampt.io import povray, povray_animation import klampt.math.vectorops as op import klampt.math.se3 as se3 import klampt.math.so3 as so3 import math, os, random, vapory as vp world = WorldModel() #example Floor floor = world.makeRigidObject("Floor") prim = GeometricPrimitive() prim.setAABB((-.1, -.1, -0.1), (.2, 1.6, 0.)) floor.geometry().setGeometricPrimitive(prim) floor.appearance().setColor(.5, .5, .5) #example Point point = world.makeRigidObject("Point") prim = GeometricPrimitive() prim.setPoint((0.05, 0.0, 0.05)) point.geometry().setGeometricPrimitive(prim) point.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.), random.uniform(0., 1.), 1.) #example Sphere point = world.makeRigidObject("Sphere") prim = GeometricPrimitive() prim.setSphere((0.05, 0.1, 0.05), 0.02) point.geometry().setGeometricPrimitive(prim) point.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),