def edit_camera_xform(world_fn, xform=None, title=None):
"""Visual editor of the camera position
"""
world = WorldModel()
world.readFile(world_fn)
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
if xform is not None:
sensing.set_sensor_xform(sensor, xform)
vis.createWindow()
if title is not None:
vis.setWindowTitle(title)
vis.resizeWindow(1024, 768)
vis.add("world", world)
vis.add("sensor", sensor)
vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot))
vis.edit("sensor_xform")
def update_sensor_xform():
sensor_xform = vis.getItemConfig("sensor_xform")
sensor_xform = sensor_xform[:9], sensor_xform[9:]
sensing.set_sensor_xform(sensor, sensor_xform)
vis.loop(callback=update_sensor_xform)
sensor_xform = vis.getItemConfig("sensor_xform")
return sensor_xform[:9], sensor_xform[9:]
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 multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def visualize_pc_in_klampt_vis(self, pcloud_fname):
title = pcloud_fname + " klampt world"
vis_window_id = vis.createWindow(title)
vis.setWindowTitle(title)
world = WorldModel()
vis.add("world", world)
pcd = o3d.io.read_point_cloud(pcloud_fname)
print(pcd)
pc_xyzs = np.asarray(pcd.points)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
# pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0])
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
print(pcloud.numPoints())
vis.add("pcloud", pcloud)
# vis.setColor("pcloud", 0, 0, 1, a=1)
# vis.setAttribute("p1", "size", 5.0)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
coordinates.setWorldModel(world)
vis.add("coordinates", coordinates.manager())
vis.show()
while vis.shown():
vis.lock()
vis.unlock()
time.sleep(0.01)
vis.kill()
from klampt import WorldModel
from klampt import vis
import klampt
import time
world = WorldModel()
world_file = "../data/simulation_test_worlds/drc_rough_terrain_world.xml"
if not world.readFile(world_file):
raise RuntimeError("Unable to load terrain model")
robot_file = "../data/robot_model/robosimian_caesar_new.rob"
if not world.readFile(robot_file):
raise RuntimeError("Unable to load robot model")
vis_window_id = vis.createWindow(world_file)
vis.setWindow(vis_window_id)
vis.add("world", world)
vp = vis.getViewport()
vis.setViewport(vp)
vis.autoFitCamera()
world.robot(0).setConfig([
12.621508747630084,
1.3060978650033888,
0.7271994997360561,
-0.18389666460947365,
-0.2336561986984183,
0.23915345995072382,
0.0,
def launchwindow():
origwindow = vis.getWindow()
vis.createWindow("Pop up window")
vis.add("world2", world)
vis.show()
vis.setWindow(origwindow)
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)
from klampt import *
from klampt import vis
import time
print(
"Tests multiple simultaneous windows. This requires multithreaded visualization,"
)
print("which doesn't work on Mac")
world1 = WorldModel()
world1.readFile('../../data/objects/block.obj')
id1 = vis.createWindow('First')
def firsthello():
print("hello from First")
vis.addAction(firsthello, "First's action", "p")
vis.add('world1', world1)
vis.show()
world2 = WorldModel()
world2.readFile('../../data/robots/athlete.rob')
id2 = vis.createWindow('Second')
def secondhello():
print("hello from Second")
def vis_world(self):
world = WorldModel()
self.rposer = None
res = world.readFile(self.world_file)
if not res:
raise RuntimeError("Unable to load terrain model")
res = world.readFile(self.robot_file)
if not res:
raise RuntimeError("Unable to load robot model")
vis.createWindow(self.worldname)
vis.setWindowTitle(self.worldname + " visualization")
vis.add("world", world)
vp = vis.getViewport()
# vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.autoFitCamera()
vis.show()
q = [
0,
0.0,
0.5, # torso x y z
0,
0,
0, # torso roll pitch yaw
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # fr
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # br
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # bl
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # fl
]
world.robot(0).setConfig(q)
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)
if __name__ == "__main__":
world_name = "flatworld"
world_file = "../data/simulation_test_worlds/" + world_name + ".xml"
world = WorldModel()
if not world.readFile(world_file):
raise RuntimeError("Unable to load terrain model")
if not world.readFile("../data/robot_model/robosimian_caesar_new.rob"):
raise RuntimeError("Unable to load robot")
robot = world.robot(0)
vis.createWindow(world_name + " physics sim replay world")
vis.add("world", world)
vis.autoFitCamera()
vis.show()
time.sleep(2)
# print(len(robot.getConfig()))
q = [0] * 38
last_time = 0
count = 0
with open("../flatworld_sim_output.txt", "r") as f:
for line in f:
t = float(line.split(" [")[0])
str_ = line.split(" [")[1]
def grasp_edit_ui(gripper, object, grasp=None):
assert gripper.klampt_model is not None
world = WorldModel()
res = world.readFile(gripper.klampt_model)
if not res:
raise ValueError("Unable to load klampt model")
robot = world.robot(0)
base_link = robot.link(gripper.base_link)
base_xform = base_link.getTransform()
base_xform0 = base_link.getTransform()
parent_xform = se3.identity()
if base_link.getParent() >= 0:
parent_xform = robot.link(base_link.getParent()).getTransform()
if grasp is not None:
base_xform = grasp.ik_constraint.closestMatch(*base_xform)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
robot.setConfig(
gripper.set_finger_config(robot.getConfig(), grasp.finger_config))
q0 = robot.getConfig()
grob = gripper.get_subrobot(robot)
grob._links = [l for l in grob._links if l != gripper.base_link]
#set up visualizer
oldWindow = vis.getWindow()
if oldWindow is None:
oldWindow = vis.createWindow()
vis.createWindow()
vis.add("gripper", grob)
vis.edit("gripper")
vis.add("object", object)
vis.add("base_xform", base_xform)
vis.edit("base_xform")
def make_grasp():
return Grasp(ik.objective(base_link, R=base_xform[0], t=base_xform[1]),
gripper.finger_links,
gripper.get_finger_config(robot.getConfig()))
#add hooks
robot_appearances = [
robot.link(i).appearance().clone() for i in range(robot.numLinks())
]
robot_shown = [True]
def toggle_robot(arg=0, data=robot_shown):
vis.lock()
if data[0]:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().setDraw(False)
data[0] = False
else:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().set(robot_appearances[i])
data[0] = True
vis.unlock()
def randomize():
print("TODO")
def reset():
vis.lock()
robot.setConfig(q0)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform0))
vis.unlock()
vis.add("base_xform", base_xform0)
vis.edit("base_xform")
vis.setItemConfig("gripper", grob.getConfig())
def save():
fmt = gripper.name + "_" + object.getName() + '_grasp_%d.json'
ind = 0
while os.path.exists(fmt % (ind, )):
ind += 1
fn = fmt % (ind, )
g = make_grasp()
print("Saving grasp to", fn)
with open(fn, 'w') as f:
json.dump(g.toJson(), f)
vis.addAction(toggle_robot, 'Toggle show robot', 'v')
vis.addAction(randomize, 'Randomize', 'r')
vis.addAction(reset, 'Reset', '0')
vis.addAction(save, 'Save to disk', 's')
def loop_setup():
vis.show()
def loop_callback():
global base_xform
xform = vis.getItemConfig("base_xform")
base_xform = (xform[:9], xform[9:])
vis.lock()
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
vis.unlock()
def loop_cleanup():
vis.show(False)
vis.loop(setup=loop_setup, callback=loop_callback, cleanup=loop_cleanup)
# this works with Linux/Windows, but not Mac
# loop_setup()
# while vis.shown():
# loop_callback()
# time.sleep(0.02)
# loop_cleanup()
g = make_grasp()
#restore RobotModel
base_link.setParentTransform(*se3.mul(se3.inv(parent_xform), base_xform0))
vis.setWindow(oldWindow)
return g
