traj6 = trajectory.HermiteTrajectory()
traj6.makeBezier([0,3,6,9],traj.milestones)
for m in traj6.milestones:
m[1] = 1
vis.add("point bezier",traj6.discretize(0.05),color=(0.8,0.8,1,1))
#which one to animate?
vis.animate("point",traj2)
#add a transform to the visualizer and animate it
xform = vis.add("xform",se3.identity())
traj = trajectory.SE3Trajectory()
x = 0
for i in range(10):
rrot = so3.sample()
rpoint = [x + random.uniform(0.1,0.3),0,random.uniform(-1,1)]
x = rpoint[0]
traj.milestones.append(rrot+rpoint)
traj.times = list(range(len(traj.milestones)))
vis.add("xform_milestones",traj,color=(1,1,0,1))
traj2 = trajectory.SE3HermiteTrajectory()
traj2.makeSpline(traj)
for m in traj2.milestones:
m[10] = 0.25
vis.add("xform spline",traj2.discretize(0.05),color=(1,0.8,0,1))
vis.add("xform2",se3.identity())
vis.animate("xform2",traj2)
traj3 = trajectory.SE3HermiteTrajectory()
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,
visualize=False,verbose=0):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Side effect: the positions of objects in world are modified
Returns:
(tuple): (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:",objects_bound)
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = _get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in range(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
if verbose:
print("Sampled position of",object.getName(),object.getTransform()[1])
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
if verbose:
print("Failed to place object",object.getName())
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
if verbose:
print("Beginning to simulate")
#start letting everything fall
for firstfall in range(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,visualize=False):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Arguments:
- world: a WorldModel
- container: the container RigidObject / Terrain in world over which objects should be spawned. Assumed axis-aligned and with an open top.
- objects: a list of RigidObjects in the world, at arbitrary locations. They are placed in order.
- container_wall_thickness: a margin subtracted from the container's outer dimensions into which the objects are spawned.
- randomize_orientation: if True, the orientation of the objects are completely randomized. If 'z', only the z orientation is randomized.
If False or None, the orientation is unchanged
Side effect: the positions of objects in world are modified
Return value (world,sim):
- world: the original world
- sim: the Simulator instance at the state used to obtain the stable placement of the objects.
Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy()
to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
print "Spawn area",spawn_area
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in xrange(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = get_bound(objects[:index])
print "Existing objects bound:",objects_bound
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in xrange(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
print "Sampled position of",object.getName(),object.getTransform()[1]
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in xrange(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
print "Terminated lowering at",lower,"cm lower"
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
print "Failed to place object",object.getName()
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
print "Beginning to simulate"
#start letting everything fall
for firstfall in xrange(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
def make_object_pile(world,
container,
objects,
container_wall_thickness=0.01,
randomize_orientation=True,
visualize=False,
verbose=0):
"""For a given container and a list of objects in the world, drops the
objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Returns:
tuple: (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
.. note::
If you wish to make multiple worlds with piles of the same objects, you
may wish to randomize the object ordering using
``random.shuffle(objects)`` between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],
[container_wall_thickness] * 3),
vectorops.sub(container_outer_bb[1],
[container_wall_thickness] * 3))
spawn_area = (container_inner_bb[0][:], container_inner_bb[1][:])
collision_margin = 0.0025
"""
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
"""
"""
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
if oldwindow == None:
vis.createWindow()
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
visworld = world.copy()
vis.add("world",visworld)
vis.addText("time","Time: 0",position=(20,20))
config.setConfig(visworld,config.getConfig(world))
vis.show()
"""
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:", objects_bound)
zshift = max(0.0, objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
R0, t0 = object.getTransform()
object.setTransform(R0, [0, 0, 0])
obb = _get_bound(object)
zmin = obb[0][2]
feasible = False
for sample in range(1000):
R, t = R0[:], t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + collision_margin + 0.2
object.setTransform(R, t)
xy_randomize(object, spawn_area[0], spawn_area[1])
if verbose:
print("Sampled position of", object.getName(),
object.getTransform()[1])
if not randomize_orientation:
_, t = object.getTransform()
object.setTransform(R, t)
#object spawned, now settle
feasible = True
break
"""
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
"""
if not feasible:
if verbose:
print("Failed to place object", object.getName())
return None
"""
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
"""
if verbose:
print("Beginning to simulate")
from klampt.sim import settle
for i, obj in enumerate(objects):
if i > 0:
objects_bound = _get_bound(objects[:i])
zothers = objects_bound[1][2]
else:
zothers = container_inner_bb[0][2]
R, t = obj.getTransform()
obj.setTransform(R, [0, 0, 0])
obb = _get_bound(obj)
zmin = obb[0][2]
t[2] = zothers - zmin + collision_margin
obj.setTransform(R, t)
print("Simulating object", obj.getName())
xform, touched = settle.settle(world, obj, debug=visualize)
obj.setTransform(*xform)
return (world, None)
"""
vis.lock()
t0 = time.time()
T = settle.settle(world,
world.robot(0).link(7),
forcedir=(0, 0, -2),
perturb=0.0,
settletol=1e-3,
debug=False)
t1 = time.time()
vis.unlock()
print("Settling robot link took time", t1 - t0)
input("Press enter to continue")
vis.lock()
for i in range(world.numRigidObjects()):
obj = world.rigidObject(i)
obj.setTransform(so3.sample(), obj.getTransform()[1])
vis.unlock()
for i in range(world.numRigidObjects()):
vis.lock()
obj = world.rigidObject(i)
t0 = time.time()
T, touched = settle.settle(world,
obj,
perturb=0.0,
orientationDamping=0,
settletol=1e-3,
debug=False)
obj.setTransform(*T)
t1 = time.time()