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 closeEvent(self,event):
if len(self.modified) > 0:
reply = QtWidgets.QMessageBox.question(self, "Unsaved changes", "Would you like to save changes to " + ', '.join(self.modified)+ "?",
QtWidgets.QMessageBox.Yes|QtWidgets.QMessageBox.No);
if reply == QtWidgets.QMessageBox.Yes:
self.onSaveClicked()
vis.show(False)
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 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 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 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 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 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 closeEvent(self,event):
if self.isVisible():
reply = QMessageBox.question(self, "Confirm quit", "Do you really want to quit?",
QMessageBox.Yes|QMessageBox.No);
if reply == QMessageBox.Yes:
vis.show(False)
QMainWindow.close(self)
else:
QMainWindow.closeEvent(self,event)
def loop_through_sensors(world=self.world,
sensor=self.sensor,
max_pics=max_pics):
for counter in tqdm(range(max_pics)):
if counter > 0:
self.chessEngine.randomizePieces(32)
# Arrange pieces and model world
self.chessEngine.arrangePieces()
self._randomlyRotateCamera(20, 40, 0.6)
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
rgb, depth = sensing.camera_to_images(self.sensor)
# Project RGB and depth images to rectify them
local_corner_coords = np.float32([
sensing.camera_project(self.sensor, self.robot, pt)[:2]
for pt in self._boardWorldCorners
])
H = cv2.getPerspectiveTransform(local_corner_coords,
self._rectifiedPictureCorners)
color_rectified = cv2.warpPerspective(
rgb, H, (DataUtils.IMAGE_SIZE, DataUtils.IMAGE_SIZE))
depth_rectified = cv2.warpPerspective(
depth, H, (DataUtils.IMAGE_SIZE, DataUtils.IMAGE_SIZE))
depth_rectified = np.uint8(
(depth_rectified - depth_rectified.min()) /
(depth_rectified.max() - depth_rectified.min()))
pieces_arrangement = self.chessEngine.getPieceArrangement()
images, classes = DataUtils.split_image_by_classes(
color_rectified, depth_rectified, data_distribution,
pieces_arrangement)
rectified_fname = self._rectifiedFNFormat % (counter)
Image.fromarray(color_rectified).save(rectified_fname)
for idx in range(sum(data_distribution)):
color_fname = self._colorFNFormat % (counter, idx)
depth_fname = self._depthFNFormat % (counter, idx)
Image.fromarray(images[idx, :, :, :3]).save(color_fname)
Image.fromarray(images[idx, :, :, 3]).save(depth_fname)
metadata_f.write(
f'{color_fname},{depth_fname},{classes[idx]}\n')
vis.show(False)
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 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 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 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 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_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 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 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 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 visualiser_init(self, no_display=False):
"""Delayed *initialisation* method for environment visualiser.
:param no_display: Controls whether turn on the
visualisation or not, defaults to False. This option is deprecated,
and instead, the environment should derived directly from the base
visualisation to turn off visualisation.
"""
if not no_display:
from klampt import vis
vis.add("world", self.args.cc.world)
vis.show()
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 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 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 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 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 loop_through_sensors(world=world,sensor=sensor,
world_camera_viewpoints=world_camera_viewpoints,
index=index,counters=counters):
viewno = counters[0]
variation = counters[1]
total_count = counters[2]
print("Generating data for sensor view",viewno,"variation",variation)
sensor_xform = world_camera_viewpoints[viewno]
sensing.set_sensor_xform(sensor,sensor_xform)
rgb_filename = "image_dataset/color_%04d_var%04d.png"%(total_count,variation)
depth_filename = "image_dataset/depth_%04d_var%04d.png"%(total_count,variation)
grasp_filename = "image_dataset/grasp_%04d.png"%(total_count,)
if PROBLEM=='1b':
sensor.kinematicReset()
sensor.kinematicSimulate(world,0.01)
print("Done with kinematic simulate")
rgb,depth = sensing.camera_to_images(sensor)
print("Saving to",rgb_filename,depth_filename)
Image.fromarray(rgb).save(rgb_filename)
depth_scale = 8000
depth_quantized = (depth * depth_scale).astype(np.uint32)
Image.fromarray(depth_quantized).save(depth_filename)
with open("image_dataset/metadata.csv",'a') as f:
line = "{},{},{},{},{},{},{}\n".format(index,viewno,loader.write(sensor_xform,'RigidTransform'),os.path.basename(rgb_filename),os.path.basename(depth_filename),os.path.basename(grasp_filename),variation)
f.write(line)
if PROBLEM=='1c' and variation==0:
#calculate grasp map and save it
grasp_map = make_grasp_map(world)
grasp_map_quantized = (grasp_map*255).astype(np.uint8)
channels = ['score','opening','axis_heading','axis_elevation']
for i,c in enumerate(channels):
base,ext=os.path.splitext(grasp_filename)
fn = base+'_'+c+ext
Image.fromarray(grasp_map_quantized[:,:,i]).save(fn)
#loop through variations and views
counters[1] += 1
if counters[1] >= max_variations:
counters[1] = 0
counters[0] += 1
if counters[0] >= len(world_camera_viewpoints):
vis.show(False)
counters[2] += 1
def robot_move(mode,world,robot,link,point_ee,point_world,maxDev,IKErrorTolerence,
EEZLimit,collider,robotControlApi=None,ServoTime=9999.0,dt=1.0,
use_const = True,vis=vis,use_collision_detect = False,use_ik_detect = False):
robotCurrentConfig=klampt_2_controller(robot.getConfig())
goal=ik.objective(link,local=point_ee,world=point_world)
res=ik.solve_nearby(goal,maxDeviation=maxDev,tol=0.00001)
#res=ik.solve_global(goal,tol=0.00001)
if res:
# collision detect
if check_collision_linear(robot,collider,controller_2_klampt(robot,robotCurrentConfig),robot.getConfig(),10):
print "[!]Warning: collision detected!"
if use_collision_detect == True:
vis.show()
if input('continue?') != 1:
exit()
else:
pass
# cal difference
diff=np.max(np.absolute((np.array(vectorops.sub(robotCurrentConfig[0:5],klampt_2_controller(robot.getConfig())[0:5])))))
EEZPos=link.getTransform()[1]
if diff<IKErrorTolerence and EEZPos>EEZLimit: #126 degrees
if mode == 'debugging':
pass
elif mode == 'physical':
if use_const:
constantVServo(robotControlApi,ServoTime,klampt_2_controller(robot.getConfig()),dt)
else:
robotControlApi.setConfig(klampt_2_controller(robot.getConfig()))
else:
print "[!]IK too far away"
if use_ik_detect == True:
if input('continue?') != 1:
exit()
else:
diff = 9999.0
print "[!]IK failture"
if use_ik_detect == True:
vis.show()
if input('continue?') != 1:
exit()
return robot, diff
def visRoadMap(self):
vis.show()
edgeList = self.G.edges()
cnt = 0
for e in edgeList:
n1 = e[0]
n2 = e[1]
tr = trajectory.Trajectory()
tr.milestones.append([n1.x, n1.y, n1.z])
tr.milestones.append([n2.x, n2.y, n2.z])
fName = "a" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setAttribute(fName, "width", 0.5)
vis.setColor(fName, 0.4940, 0.1840, 0.5560)
cnt = cnt + 1
self.rmCnt = cnt
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 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 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
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