def shutdown(u_input=None):
if u_input:
u_input.shutdown()
try:
if vis.shown():
Logger.log("Exiting when vis window closed", "")
while vis.shown():
time.sleep(0.01)
Logger.log("Shutting down", "FAIL")
vis.kill()
except TypeError:
Logger.log("Shutting down", "FAIL")
try:
sys.exit(0)
except SystemExit:
os._exit(0)
def animate_trajectories(trajs, times, endWaitTime=5.0, speed=0.2):
global world
active = 0
for i in range(len(trajs)):
vis.add(
"traj" + str(i), trajs[i].getLinkTrajectory(
END_EFFECTOR_LINK,
0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION))
vis.hide("traj" + str(i))
vis.hideLabel("traj" + str(i))
vis.show()
t0 = time.time()
if len(times) == 1:
tnext = float('inf')
else:
tnext = times[active + 1]
while vis.shown():
t = time.time()
if t - t0 > tnext:
nextactive = (active + 1) % len(times)
vis.hide("traj" + str(active))
vis.hide("traj" + str(nextactive), False)
active = nextactive
if nextactive + 1 == len(times):
tnext += endWaitTime
else:
tnext += times[active + 1] - times[active]
vis.lock()
world.robot(0).setConfig(trajs[active].eval((t - t0) * speed,
endBehavior='loop'))
vis.unlock()
time.sleep(0.01)
print("Done.")
def play_with_trajectory(traj, configs=[3]):
vis.add("trajectory", traj)
names = []
for i, x in enumerate(traj.milestones):
if i in configs:
print("Editing", i)
names.append("milestone " + str(i))
vis.add(names[-1], x[:])
vis.edit(names[-1])
#vis.addPlot("distance")
vis.show()
while vis.shown():
vis.lock()
t0 = time.time()
updated = False
for name in names:
index = int(name.split()[1])
qi = vis.getItemConfig(name)
if qi != traj.milestones[index]:
traj.milestones[index] = qi
updated = True
if updated:
vis.add("trajectory", traj)
xnew = trajcache.trajectoryToState(traj)
ctest2.setx(xnew)
res = ctest2.minvalue(xtraj)
print(res)
ctest2.clearx()
vis.unlock()
t1 = time.time()
#vis.logPlot("timing","opt",t1-t0)
time.sleep(max(0.001, 0.025 - (t1 - t0)))
def edit_template(world):
"""Shows how to pop up a visualization window with a world in which the robot configuration and a transform can be edited"""
#add the world to the visualizer
vis.add("world",world)
xform = se3.identity()
vis.add("transform",xform)
robotPath = ("world",world.robot(0).getName()) #compound item reference: refers to robot 0 in the world
vis.edit(robotPath)
vis.edit("transform")
#This prints how to get references to items in the visualizer
print("Visualization items:")
vis.listItems(indent=2)
vis.setWindowTitle("Visualization editing test")
if not MULTITHREADED:
vis.loop(setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here.
vis.unlock()
time.sleep(0.01)
print("Resulting configuration",vis.getItemConfig(robotPath))
print("Resulting transform (config)",vis.getItemConfig("transform")) # this is a vector describing the item parameters
xform = list(xform) #convert se3 element from tuple to list
config.setConfig(xform,vis.getItemConfig("transform"))
print("Resulting transform (se3)",xform)
#quit the visualization thread nicely
vis.kill()
def plugin_template(world):
"""Demonstrates the GLPluginInterface functionality"""
#create a subclass of GLPluginInterface
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin) #put the plugin on top of the standard visualization functionality.
#vis.setPlugin(plugin) #use this to completely replace the standard visualization functionality with your own.
vis.add("world",world)
vis.setWindowTitle("GLPluginInterface template")
#run the visualizer
if not MULTITHREADED:
def callback(plugin=plugin):
if plugin.quit:
vis.show(False)
vis.loop(callback=callback,setup=vis.show)
else:
#if plugin.quit is True
vis.show()
while vis.shown() and not plugin.quit:
vis.lock()
#TODO: you may modify the world here
vis.unlock()
#changes to the visualization must be done outside the lock
time.sleep(0.01)
if plugin.quit:
#if you want to do other stuff after the window quits, the window needs to be hidden
vis.show(False)
print("Waiting for 2 s...")
time.sleep(2.0)
#quit the visualization thread nicely
vis.kill()
def launch_xform_viewer(robotname):
"""Launches a very simple program that spawns an object from one of the
databases.
It launches a visualization of the mvbb decomposition of the object, and corresponding generated poses.
It then spawns a hand and tries all different poses to check for collision
"""
world = WorldModel()
robot = make_moving_base_robot(robotname, world)
set_moving_base_xform(robot, se3.identity()[0], se3.identity()[1])
xform = resource.get("default_initial_%s.xform" % robotname,
description="Initial hand transform",
default=se3.identity(),
world=world,
doedit=False)
print "Transform:", xform
program = XFormVisualizer(world, xform)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.01)
vis.kill()
return
def Robot_Config_Plot(world,
DOF,
state_ref,
contact_link_dictionary,
convex_edges_list,
delta_t=0.5):
# This function is used to plot the robot motion
# The optimized solution is used to plot the robot motion and the contact forces
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
sim_robot.setConfig(state_ref[0:DOF])
contact_link_list = contact_link_dictionary.keys()
while vis.shown():
# This is the main plot program
vis.lock()
sim_robot.setConfig(state_ref[0:DOF])
# Convex_Edges_Plot(sim_robot, convex_edges_list, vis)
# Robot_COM_Plot(sim_robot, vis)
vis.unlock()
time.sleep(delta_t)
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None):
"""Launches a very simple program that spawns a box with dimensions specified in boxes_db.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform",
default=se3.identity(), world=world, doedit=False)
for box_dims, poses in box_db.db.items():
if world.numRigidObjects() > 0:
world.remove(world.rigidObject(0))
obj = make_box(world,
box_dims[0],
box_dims[1],
box_dims[2])
poses_filtered = []
R,t = obj.getTransform()
obj.setTransform(R, [0, 0, box_dims[2]/2.])
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p_T_h = np.array(se3.homogeneous(xform))
p_T_h[2][3] += 0.02
for pose in poses:
w_T_p = w_T_o.dot(pose)
w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h))
if CollisionTestPose(world, robot, obj, w_T_h_des_se3):
pose_pp = se3.from_homogeneous(pose)
t_pp = pose_pp[1]
q_pp = so3.quaternion(pose_pp[0])
q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]]
print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims
elif w_T_p[2][3] <= 0.:
print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table"
else:
poses_filtered.append(pose)
print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses"
# embed()
# create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
# emulator takes the robot index (0), start link index (6), and start driver index (6)
program = FilteredMVBBTesterVisualizer(box_dims,
poses_filtered,
world,
p_T_h,
module,
box_db,
links_to_check)
vis.setPlugin(None)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.1)
return
def vis_interaction_test(world):
"""Run some tests of visualization module interacting with the resource module"""
print("Showing robot in modal dialog box")
vis.add("robot", world.robot(0))
vis.add("ee", world.robot(0).link(11).getTransform())
vis.dialog()
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
import time
if MULTITHREADED:
print(
"Showing threaded visualization (this will fail on GLUT or Mac OS)"
)
vis.show()
for i in range(3):
vis.lock()
q = world.robot(0).getConfig()
q[9] = 3.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
vis.lock()
q = world.robot(0).getConfig()
q[9] = -1.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
print("Hiding visualization window")
vis.show(False)
else:
print("Showing single-threaded visualization for 5s")
vis.spin(5.0)
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
def run(self):
while vis.shown():
vis.lock()
vis.unlock()
# changes to the visualization must be done outside the lock
vis.clearText()
print "Ending klampt.vis visualization."
def main():
print("============================================================")
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print("USAGE: simtest.py [world_file] [controller files (.py)]")
print("============================================================")
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
sys.path.append(os.path.abspath("../control"))
import trajectory_controller
#it's a path file, try to load it
controller = trajectory_controller.make(world.robot(i), c)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)
def PIP_Config_Plot(world, DOF, state_ref, pips_list, CPFlag, delta_t=0.5):
# This function is used to plot the robot motion
# The optimized solution is used to plot the robot motion and the contact forces
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
sim_robot.setConfig(state_ref[0:DOF])
InfeasiFlag = 0
while vis.shown():
# This is the main plot program
vis.lock()
sim_robot.setConfig(state_ref[0:DOF])
PIPs_Number = len(pips_list[0])
COM_Pos = sim_robot.getCom()
EdgeAList = pips_list[0]
EdgeBList = pips_list[1]
EdgeCOMList = pips_list[2]
EdgexList = pips_list[3]
EdgeyList = pips_list[4]
EdgezList = pips_list[5]
for i in range(0, PIPs_Number):
EdgeA = EdgeAList[i]
EdgeB = EdgeBList[i]
EdgeCOM = EdgeCOMList[i]
Edgey = EdgexList[i]
Edgez = EdgeyList[i]
Edgex = EdgezList[i]
PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos,
vis)
Robot_COM_Plot(sim_robot, vis)
if CPFlag is 1:
try:
h = ConvexHull(EdgeAList)
except:
InfeasiFlag = 1
if InfeasiFlag is 0 and CPFlag is 1:
h = ConvexHull(EdgeAList)
hrender = draw_hull.PrettyHullRenderer(h)
vis.add("blah", h)
vis.setDrawFunc("blah", my_draw_hull)
vis.unlock()
time.sleep(delta_t)
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def run_ex3():
world = WorldModel()
res = world.readFile("ex3_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle("Pick and place test, use a/b/c/d to select target")
vis.pushPlugin(GLPickAndPlacePlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def testCartesianDrive():
w = WorldModel()
#w.readFile("../../data/tx90scenario0.xml")
w.readFile("../../data/robots/jaco.rob")
r = w.robot(0)
solver = CartesianDriveSolver(r)
#set a non-singular configuration
q = r.getConfig()
q[3] = 0.5
r.setConfig(q)
solver.start(q, 6)
vis.add("world", w)
vis.addPlot("timing")
vis.addPlot("info")
vis.show()
time.sleep(0.1)
dt = 0.01
t = 0
while t < 20 and vis.shown():
vis.lock()
if t < 2:
v = [0, 0, 0.25]
elif t < 3:
v = [0, 0, -0.1]
elif t < 3.2:
v = [0, 0, -1]
elif t < 8:
v = [0, 0, 0]
elif t < 10:
v = [-1, 0, 0]
else:
v = [1, 0, 0]
if t < 4:
w = [0, 0, 0]
elif t < 10:
w = [0, -0.25, 0]
else:
w = None
t0 = time.time()
progress, qnext = solver.drive(q, w, v, dt)
t1 = time.time()
vis.addText("debug", "Vel %s" % (str(v), ))
vis.logPlot("timing", "t", t1 - t0)
vis.logPlot("info", "progress", progress)
vis.logPlot("info", "adj", solver.driveSpeedAdjustment)
r.setConfig(qnext)
q = qnext
vis.unlock()
vis.add("tgt", solver.driveTransforms[0])
t += dt
time.sleep(max(0.005 - (t1 - t0), 0))
vis.show(False)
vis.clear()
def Robot_Config_Plot(world, DOF, config_init):
robot_viewer = MyGLPlugin(world)
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
# Here we would like to read point cloud for visualization of planning.
# 1. All Reachable Points
# IdealReachableContacts_data = ContactDataLoader("IdealReachableContact")
# # 2. Active Reachable Points
# ReachableContacts_data = ContactDataLoader("ReachableContacts")
# # 3. Contact Free Points
# CollisionFreeContacts_data = ContactDataLoader("CollisionFreeContacts")
# # 4. Supportive Points
# SupportiveContacts_data = ContactDataLoader("SupportiveContacts")
OptimalContact_data = ContactDataLoader("OptimalContact")
OptimalContactWeights_data = ContactDataLoader("OptimalContactWeights")
OptimalContact_data, OptimalContactWeights_data = ContactDataRefine(
OptimalContact_data, OptimalContactWeights_data)
# # 6.
# TransitionPoints_data = ContactDataLoader("TransitionPoints")
# import ipdb; ipdb.set_trace()
# 7.
InitialTransitionPoints_data = ContactDataLoader("InitialPathWayPoints")
# 8.
ShiftedTransitionPoints_data = ContactDataLoader("ShiftedPathWayPoints")
# 9.
# FineShiftedPathWayPoints_data = ContactDataLoader("FineShiftedPathWayPoints")
#
# ReducedOptimalContact_data = ContactDataLoader("ReducedOptimalContact")
ContactChoice = ShiftedTransitionPoints_data
# ContactChoice = SupportiveContacts_data
SimRobot = world.robot(0)
SimRobot.setConfig(config_init)
while vis.shown():
# This is the main plot program
vis.lock()
SimRobot.setConfig(config_init)
WeightedContactDataPlot(vis, OptimalContact_data,
OptimalContactWeights_data)
ContactDataPlot(vis, ContactChoice)
vis.unlock()
time.sleep(0.1)
import ipdb
ipdb.set_trace()
WeightedContactDataUnPlot(vis, OptimalContact_data)
ContactDataUnplot(vis, ContactChoice)
def run_ex2():
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.pushPlugin(GLTransferPlanPlugin(world))
vis.setWindowTitle(
"Transfer plan test, press r/f to plan with right/forward target")
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run_ex1():
world = WorldModel()
res = world.readFile("ex1_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle(
"Transit plan test, press l/r to plan with left/right arm")
vis.pushPlugin(GLTransitPlanPlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def animation_template(world):
"""Shows how to animate a robot."""
#first, build a trajectory with 10 random configurations
robot = world.robot(0)
times = range(10)
milestones = []
for t in times:
robot.randomizeConfig()
milestones.append(robot.getConfig())
traj = trajectory.RobotTrajectory(robot, times, milestones)
vis.add("world", world)
robotPath = ("world", world.robot(0).getName()
) #compound item reference: refers to robot 0 in the world
#we're also going to visualize the end effector trajectory
#eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05)
#vis.add("end effector trajectory",eetraj)
#uncomment this to automatically visualize the end effector trajectory
vis.add("robot trajectory", traj)
vis.setAttribute("robot trajectory", "endeffectors", [13, 20])
vis.setWindowTitle("Animation test")
MANUAL_ANIMATION = False
if not MANUAL_ANIMATION:
#automatic animation, just call vis.animate
vis.animate(robotPath, traj)
if not MULTITHREADED:
#need to set up references to function-local variables manually, and the easiest way is to use a default argument
def callback(robot=robot):
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
pass
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def update_simulation(world, sim):
# this code manually updates the visualization
vis.add("world", world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
return
def basic_template(world):
"""Shows how to pop up a visualization window with a world"""
#add the world to the visualizer
vis.add("world", world)
#adding a point
vis.add("point", [1, 1, 1])
vis.setColor("point", 0, 1, 0)
vis.setAttribute("point", "size", 5.0)
#adding lines is currently not super convenient because a list of lists is treated as
#a Configs object... this is a workaround to force the vis module to treat it as a polyline.
vis.add("line", trajectory.Trajectory([0, 1], [[0, 0, 0], [1, 1, 1]]))
vis.setAttribute("line", "width", 1.0)
sphere = klampt.GeometricPrimitive()
sphere.setSphere([1.5, 1, 1], 0.2)
vis.add("sphere", sphere)
vis.setColor("sphere", 0, 0, 1, 0.5)
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)
vis.setWindowTitle("Basic visualization test")
if not MULTITHREADED:
print("Running vis loop in single-threaded mode with vis.loop()")
#single-threaded code
def callback():
#TODO: you may modify the world here.
pass
vis.loop(setup=vis.show, callback=callback)
else:
print("Running vis loop in multithreaded mode")
#multithreaded code
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here. Do not modify the internal state of any
#visualization items outside of the lock
vis.unlock()
#outside of the lock you can use any vis.X functions, including vis.setItemConfig()
#to modify the state of objects
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def main():
#creates a world and loads all the items on the command line
world = WorldModel()
res = world.readFile("./HRP2_Robot.xml")
if not res:
raise RuntimeError("Unable to load model")
robot = world.robot(0)
# coordinates.setWorldModel(world)
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin)
#add the world to the visualizer
vis.add("world", world)
vis.add("robot", world.robot(0))
vis.show()
# ipdb.set_trace()
Robotstate_Traj, Contact_Force_Traj = Traj_Loader()
h = 0.0068 # 0.0043: vert
# 0.0033: flat
playspeed = 2.5
norm = 500
while vis.shown():
# This is the main plot program
for i in range(0, Robotstate_Traj.shape[1]):
vis.lock()
Robotstate_Traj_i = Robotstate_Traj[:, i]
Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i)
# Now it is the plot of the contact force at the contact extremities
robot.setConfig(Robotstate_Traj_Full_i)
Contact_Force_Traj_i = Contact_Force_Traj[:, i]
left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec(
robot, Contact_Force_Traj_i, norm)
vis.add("left foot force",
Trajectory([0, 1], [left_ft, left_ft_end]))
vis.add("right foot force",
Trajectory([0, 1], [rght_ft, rght_ft_end]))
vis.add("left hand force",
Trajectory([0, 1], [left_hd, left_hd_end]))
vis.add("right hand force",
Trajectory([0, 1], [rght_hd, rght_hd_end]))
COMPos_start = robot.getCom()
COMPos_end = COMPos_start
COMPos_end[2] = COMPos_end[2] + 100
vis.add("Center of Mass",
Trajectory([0, 1], [COMPos_start, COMPos_end]))
# ipdb.set_trace()
vis.unlock()
time.sleep(h * playspeed)
def run(self):
"""Standardized interface for running program"""
vp = vis.getViewport()
#Square screen
#vp.w,vp.h = 800,800
#For saving HD quality movies
vp.w, vp.h = 1024, 768
vp.clippingplanes = self.clippingplanes
vis.setViewport(vp)
#vis.run(program)
vis.setPlugin(self)
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def visualize(self):
world = robotsim.WorldModel()
vis.add("world", world)
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("PointCloud World")
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
# vis.autoFitCamera()
vis.show()
vis.lock()
vis.unlock()
while vis.shown():
time.sleep(0.01)
vis.kill()
def run(self):
"""Standardized interface for running program"""
if KLAMPT_VERSION >= 0.7:
vp = vis.getViewport()
#Square screen
#vp.w,vp.h = 800,800
#For saving HD quality movies
vp.w, vp.h = 1024, 768
vp.clippingplanes = self.clippingplanes
vis.setViewport(vp)
#vis.run(program)
vis.setPlugin(self)
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
else:
#Square screen
#self.width,self.height = 800,800
#For saving HD quality movies
self.width, self.height = 1024, 768
GLBaseClass.run(self)
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()
def Robot_Traj_Plot(world,
DOF,
state_traj,
contact_link_dictionary,
delta_t=0.5):
# Initialize the robot motion viewer
robot_viewer = MyGLPlugin(world)
# Here it is to unpack the robot optimized solution into a certain sets of the lists
vis.pushPlugin(robot_viewer)
vis.add("world", world)
vis.show()
sim_robot = world.robot(0)
contact_link_list = contact_link_dictionary.keys()
while vis.shown():
# This is the main plot program
for config_i in state_traj:
vis.lock()
sim_robot.setConfig(config_i[0:DOF])
# Robot_COM_Plot(sim_robot, vis)
vis.unlock()
time.sleep(delta_t)
from sensor_msgs.msg import CameraInfo
def oncamerainfo(ci):
vp = vis.getViewport()
kros.from_CameraInfo(ci, vp)
vis.setViewport(vp)
camera_sub = rospy.Subscriber('camera_info',
CameraInfo,
oncamerainfo,
queue_size=10)
visworld = w.copy()
vis.add("world", visworld)
vis.show()
while not rospy.is_shutdown() and vis.shown():
if TEST_TF_LISTENER:
kros.listen_tf(listener, w)
vis.lock()
for i in range(w.robot(0).numLinks()):
visworld.robot(0).link(i).setTransform(
*w.robot(0).link(i).getTransform())
vis.unlock()
else:
vis.lock()
visworld.robot(0).setConfig(w.robot(0).getConfig())
vis.unlock()
rospy.sleep(0.01)
def idle(self):
if not self.running:
return
if self.world.numRigidObjects() > 0:
self.obj = self.world.rigidObject(0)
else:
return
if not self.is_simulating:
if len(self.poses) > 0:
self.curr_pose = self.poses.pop(0)
print "\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "Simulating Next Pose Grasp"
print "Dims:\n", self.box_dims
print "Pose:\n", self.curr_pose
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n"
else:
print "Done testing all", len(
self.poses), "poses for object", self.box_dims
print "Quitting"
self.running = False
vis.show(hidden=True)
return
w_T_o_se3 = se3.from_homogeneous(self.w_T_o)
self.obj.setTransform(w_T_o_se3[0], w_T_o_se3[1])
w_T_h_des_se3 = se3.from_homogeneous(
self.w_T_o.dot(self.curr_pose).dot(self.p_T_h))
self.robot.setConfig(self.q_0)
set_moving_base_xform(self.robot, w_T_h_des_se3[0],
w_T_h_des_se3[1])
if self.sim is None:
self.sim = SimpleSimulator(self.world)
self.hand = self.module.HandEmulator(self.sim, 0, 6, 6)
self.sim.addEmulator(0, self.hand)
# the next line latches the current configuration in the PID controller...
self.sim.controller(0).setPIDCommand(
self.robot.getConfig(),
vectorops.mul(self.robot.getVelocity(), 0.0))
obj_b = self.sim.body(self.obj)
obj_b.setVelocity([0., 0., 0.], [0., 0., 0.])
# setup the preshrink
visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(self.robot.numLinks()):
self.sim.body(
self.robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(self.world.numRigidObjects()):
self.sim.body(self.world.rigidObject(
l)).setCollisionPreshrink(visPreshrink)
self.object_com_z_0 = getObjectGlobalCom(self.obj)[2]
self.object_fell = False
self.t_0 = self.sim.getTime()
self.is_simulating = True
if self.is_simulating:
t_lift = 1.3 # when to lift
d_lift = 1.0 # duration
# print "t:", self.sim.getTime() - self.t_0
object_com_z = getObjectGlobalCom(self.obj)[2]
w_T_h_curr_se3 = get_moving_base_xform(self.robot)
w_T_h_des_se3 = se3.from_homogeneous(
self.w_T_o.dot(self.curr_pose).dot(self.p_T_h))
if self.sim.getTime() - self.t_0 == 0:
# print "Closing hand"
self.hand.setCommand([1.0])
elif (self.sim.getTime() - self.t_0) >= t_lift and (
self.sim.getTime() - self.t_0) <= t_lift + d_lift:
# print "Lifting"
t_i = w_T_h_des_se3[1]
t_f = vectorops.add(t_i, (0, 0, 0.2))
u = np.min((self.sim.getTime() - self.t_0 - t_lift, 1.))
send_moving_base_xform_PID(self.sim.controller(0),
w_T_h_des_se3[0],
vectorops.interpolate(t_i, t_f, u))
if (self.sim.getTime() - self.t_0
) >= t_lift: # wait for a lift before checking if object fell
d_hand = w_T_h_curr_se3[1][2] - w_T_h_des_se3[1][2]
d_com = object_com_z - self.object_com_z_0
if d_hand - d_com > 0.1:
self.object_fell = True
print "!!!!!!!!!!!!!!!!!!"
print "Grasp Unsuccessful"
print "!!!!!!!!!!!!!!!!!!"
self.sim.simulate(0.01)
self.sim.updateWorld()
if not vis.shown() or (self.sim.getTime() -
self.t_0) >= 2.5 or self.object_fell:
if vis.shown(): # simulation stopped because it was successful
print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
print "Saving grasp, status:", "failure" if self.object_fell else "success"
print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\n"
w_T_h_curr = np.array(se3.homogeneous(w_T_h_curr_se3))
w_T_o_curr = np.array(
se3.homogeneous(self.obj.getTransform()))
h_T_o = np.linalg.inv(w_T_h_curr).dot(w_T_o_curr)
if self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs:
q_grasp = [
float('nan')
] * self.db.n_dofs if self.object_fell else self.hand.getConfiguration(
)
elif self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs + self.hand.m_dofs:
q_grasp = [
float('nan')
] * self.db.n_dofs if self.object_fell else self.hand.getFullConfiguration(
)
else:
raise Exception(
'Error: unexcpeted number of joints for hand')
c_p, c_f = getObjectPhalanxMeanContactPoint(
self.sim, self.obj, self.robot, self.links_to_check)
try:
self.db.save_simulation(self.box_dims, self.curr_pose,
h_T_o, q_grasp, c_p, c_f)
except:
print "\nXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
print "X Error while calling save_simulation X"
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
traceback.print_exc()
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n"
self.is_simulating = False
self.sim = None
self.robot.setConfig(self.q_0)
def launch_balls(robotname,num_balls=10):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
maxlayer = 16
numlayers = int(math.ceil(float(num_balls)/float(maxlayer)))
for layer in xrange(numlayers):
if layer + 1 == numlayers:
ballsperlayer = num_balls%maxlayer
else:
ballsperlayer = maxlayer
w = int(math.ceil(math.sqrt(ballsperlayer)))
h = int(math.ceil(float(ballsperlayer)/float(w)))
for i in xrange(ballsperlayer):
bid = world.loadElement("data/objects/sphere_10cm.obj")
if bid < 0:
raise RuntimeError("data/objects/sphere_10cm.obj couldn't be loaded")
ball = world.rigidObject(world.numRigidObjects()-1)
R = so3.identity()
x = i % w
y = i / w
x = (x - (w-1)*0.5)*box_dims[0]*0.7/(w-1)
y = (y - (h-1)*0.5)*box_dims[1]*0.7/(h-1)
t = [x,y,0.08 + layer*0.11]
t[0] += random.uniform(-0.005,0.005)
t[1] += random.uniform(-0.005,0.005)
ball.setTransform(R,t)
robot = make_moving_base_robot(robotname,world)
box = make_box(world,*box_dims)
box2 = make_box(world,*box_dims)
box2.geometry().translate((0.7,0,0))
xform = resource.get("balls/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=True)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#A StateMachineController instance is now attached to control the robot
import balls_controller
sim.setController(robot,balls_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
"""
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
def launch_shelf(robotname,objects):
"""Launches the task 2 program that asks the robot to retrieve some set of objects
packed within a shelf.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname,world)
box = make_box(world,*box_dims)
shelf = make_shelf(world,*shelf_dims)
shelf.geometry().translate((0,shelf_offset,shelf_height))
rigid_objects = []
for objectset,objectname in objects:
object = make_object(objectset,objectname,world)
#TODO: pack in the shelf using x-y translations and z rotations
object.setTransform(*se3.mul((so3.identity(),[0,shelf_offset,shelf_height + 0.01]),object.getTransform()))
rigid_objects.append(object)
xy_jiggle(world,rigid_objects,[shelf],[-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100)
doedit = True
xform = resource.get("shelf/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world)
if not xform:
print "User quit the program"
return
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#controlfunc is now attached to control the robot
import shelf_controller
sim.setController(robot,shelf_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
program.reshape(800,600)
vis.show()
while vis.shown():
time.sleep(0.1)
return
#this code manually updates the vis
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
Tb = [euler_angle_to_rotation(eb),[1,0,1]]
def update_interpolation(u):
#linear interpolation with euler angles
e = interpolate_euler_angles(ea,eb,u)
t = interpolate_linear(Ta[1],Tb[1],u)
return (euler_angle_to_rotation(e),t)
if __name__ == "__main__":
#draw the world reference frame, the start and the goal, and the interpolated frame
vis.add("world",se3.identity())
vis.add("start",Ta)
vis.add("end",Tb)
vis.add("interpolated",Ta)
vis.setAttribute("world","length",0.25)
vis.setAttribute("interpolated","fancy",True)
vis.setAttribute("interpolated","width",0.03)
vis.setAttribute("interpolated","length",1.0)
period = 3.0
t0 = time.time()
vis.show()
while vis.shown():
#repeat the interpolation every 3 seconds
u = ((time.time()-t0)%period)/period
T = update_interpolation(u)
vis.setItemConfig("interpolated",T)
time.sleep(0.01)
vis.kill()
def launch_simple(robotname,object_set,objectname,use_box=False):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
If use_box is True, then the test object is placed inside a box.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname,world)
object = make_object(object_set,objectname,world)
if use_box:
box = make_box(world,*box_dims)
object.setTransform(*se3.mul((so3.identity(),[0,0,0.01]),object.getTransform()))
doedit = True
xform = resource.get("%s/default_initial_%s.xform"%(object_set,robotname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world)
set_moving_base_xform(robot,xform[0],xform[1])
xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=False)
if xform:
set_moving_base_xform(robot,xform[0],xform[1])
xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=doedit)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#the result of simple_controller.make() is now attached to control the robot
import simple_controller
sim.setController(robot,simple_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
"""
program.simulate = True
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
