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.),