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 svg_path_to_polygon(p, dt=0.1):
"""Produces a Klampt polygon geometry from an SVG Path."""
traj = svg_path_to_trajectory(p)
if isinstance(traj, HermiteTrajectory):
traj = traj.discretize(dt)
verts = sum(traj.milestones, [])
g = GeometricPrimitive()
g.setPolygon(verts)
return g
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 debug_stable_faces(obj, faces):
from klampt import vis, Geometry3D, GeometricPrimitive
from klampt.math import se3
import random
vis.createWindow()
obj.setTransform(*se3.identity())
vis.add("object", obj)
for i, f in enumerate(faces):
gf = GeometricPrimitive()
gf.setPolygon(np.stack(f).flatten())
color = (1, 0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(),
0.5)
vis.add("face{}".format(i),
Geometry3D(gf),
color=color,
hide_label=True)
vis.setWindowTitle("Stable faces")
vis.dialog()
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 add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
def sphere(radius,
center=None,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a sphere with the given radius
Args:
radius (float): radius of the sphere
center (list of 3 floats, optional): if None (typical), the *geometry*
of the sphere is centered at 0. Otherwise, the *geometry* of
the sphere is shifted relative to the sphere's local coordinate system.
R,t (se3 transform, optional): if given, the sphere's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the sphere 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 'sphere'.
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 sphere. 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.setSphere(center, radius)
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 = 'sphere'
if mass != float('inf'):
bmass = Mass()
bmass.setMass(mass)
bmass.setCom(center)
bmass.setInertia([0.4 * mass * radius**2] * 3)
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
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.),