def gen_grasp_worlds(N=10):
if len(base_boxes) == 0:
#edit base boxes
for basefn in base_files:
world = WorldModel()
world.readFile(basefn)
base_name = os.path.splitext(os.path.basename(basefn))[0]
box = GeometricPrimitive()
box.setAABB([0, 0, 0], [1, 1, 1])
base_boxes.append(
resource.get(base_name + '.geom',
'GeometricPrimitive',
default=box,
world=world,
doedit='auto'))
output_file_pattern = "generated_worlds/world_%04d.xml"
#TODO: play around with me, problem 1a
num_objects = [3, 3, 4, 5, 6, 6]
for i in range(N):
nobj = random.choice(num_objects)
world = WorldModel()
base_world = random.choice(range(len(base_files)))
world.readFile(base_files[base_world])
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn, world)
obj_fns.append(fn)
bbox = base_boxes[base_world]
bmin, bmax = [v for v in bbox.properties[0:3]
], [v for v in bbox.properties[3:6]]
#TODO: Problem 1: arrange objects within box bmin,bmax
arrange_objects(world, obj_fns, bmin, bmax)
save_world(world, output_file_pattern % (i, ))
def grasp_plan_main():
world = WorldModel()
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
world.readFile("table.xml")
xform = resource.get("table_camera_00.xform",type='RigidTransform')
sensing.set_sensor_xform(sensor,xform)
box = GeometricPrimitive()
box.setAABB([0,0,0],[1,1,1])
box = resource.get('table.geom','GeometricPrimitive',default=box,world=world,doedit='auto')
bmin,bmax = [v for v in box.properties[0:3]],[v for v in box.properties[3:6]]
nobj = 5
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn,world)
#TODO: you might want to mess with colors here too
obj.appearance().setSilhouette(0)
obj_fns.append(fn)
for i in range(world.numTerrains()):
#TODO: you might want to mess with colors here too
world.terrain(i).appearance().setSilhouette(0)
problem1.arrange_objects(world,obj_fns,bmin,bmax)
intrinsics = dict()
w = int(sensor.getSetting('xres'))
h = int(sensor.getSetting('yres'))
xfov = float(sensor.getSetting('xfov'))
yfov = float(sensor.getSetting('yfov'))
intrinsics['cx'] = w/2
intrinsics['cy'] = h/2
intrinsics['fx'] = math.tan(xfov*0.5)*h*2
intrinsics['fy'] = math.tan(xfov*0.5)*h*2
print("Intrinsics",intrinsics)
planner = ImageBasedGraspPredictor()
def do_grasp_plan(event=None,world=world,sensor=sensor,planner=planner,camera_xform=xform,camera_intrinsics=intrinsics):
sensor.kinematicReset()
sensor.kinematicSimulate(world,0.01)
rgb,depth = sensing.camera_to_images(sensor)
grasps,scores = planner.generate(camera_xform,camera_intrinsics,rgb,depth)
for i,(g,s) in enumerate(zip(grasps,scores)):
color = (1-s,s,0,1)
g.add_to_vis("grasp_"+str(i),color=color)
def resample_objects(event=None,world=world,obj_fns=obj_fns,bmin=bmin,bmax=bmax):
problem1.arrange_objects(world,obj_fns,bmin,bmax)
vis.add("world",world)
vis.add("sensor",sensor)
vis.addAction(do_grasp_plan,'Run grasp planner','p')
vis.addAction(resample_objects,'Sample new arrangement','s')
vis.run()
def show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5))
else:
print("Nothing reachable?")
Tee = robot.link(ee_link).getTransform()
gpen.setCurrentTransform(*Tee)
box = GeometricPrimitive()
box.setAABB(lower_corner,upper_corner)
gbox = Geometry3D(box)
#show this if you want to debug the size of the grid domain
#vis.add("box",gbox,color=(1,1,1,0.2))
vis.add("pen tip",se3.apply(Tee,ee_local_pos))
vis.loop()
def box(width,
depth,
height,
center=None,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a box with dimensions width x depth x height. The box is centered
at (0,0,0) by default.
Args:
width,depth,height (float): x,y,z dimensions of the box
center (list of 3 floats, optional): if None (typical),
the *geometry* of the box is centered at 0. Otherwise,
the *geometry* of the box is shifted relative to the
box's local coordinate system.
R,t (se3 transform, optional): if given, the box's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the box will be a
RigidObjectModel or TerrainModel will be created in this world
name (str, optional): If world is given, this is the name of the object.
Default is 'box'.
mass (float, optional): If world is given and this is inf, then a
TerrainModel will be created. Otherwise, a RigidObjectModel
will be created with automatically determined inertia.
type (str, optional): the geometry type. Defaults to 'TriangleMesh',
but also 'GeometricPrimitive' and 'VolumeGrid' are accepted.
Returns:
Geometry3D, RigidObjectModel, or TerrainModel: A representation
of the box. If a world is given, then either a RigidObjectModel
or TerrainModel is added to the world and returned.
"""
if center is None:
center = [0, 0, 0]
prim = GeometricPrimitive()
prim.setAABB([
center[0] - width * 0.5, center[1] - depth * 0.5,
center[2] - height * 0.5
], [
center[0] + width * 0.5, center[1] + depth * 0.5,
center[2] + height * 0.5
])
geom = Geometry3D(prim)
if type != 'GeometricPrimitive':
geom = geom.convert(type)
if world is None:
if R is not None and t is not None:
geom.setCurrentTransform(R, t)
return geom
#want a RigidObjectModel or TerrainModel
if name is None:
name = 'box'
if mass != float('inf'):
bmass = Mass()
bmass.setMass(mass)
bmass.setCom(center)
bmass.setInertia([
mass * (depth**2 + height**2) / 12,
mass * (width**2 + height**2) / 12,
mass * (width**2 + height**2) / 12
])
robj = world.makeRigidObject(name)
robj.geometry().set(geom)
robj.setMass(bmass)
if R is not None and t is not None:
robj.setTransform(R, t)
return robj
else:
tobj = world.makeTerrain(name)
if R is not None and t is not None:
geom.transform(R, t)
tobj.geometry().set(geom)
return tobj
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.),