def __init__(self, endeffectors, constraints, traj):
vis.GLPluginInterface.__init__(self)
self.endeffectors = endeffectors
self.constraints = constraints
self.robot = self.constraints[0].robot
self.traj = traj
self.refConfig = self.robot.getConfig()
vis.add("ghost1", self.refConfig)
vis.setColor("ghost1", 0, 1, 0, 0.5)
#vis.hide("ghost1",False)
def bumpTrajectory():
relative_xforms = []
robot.setConfig(self.refConfig)
for e in self.endeffectors:
xform = vis.getItemConfig("ee_" + robot.link(e).getName())
T = (xform[:9], xform[9:])
T0 = robot.link(e).getTransform()
Trel = se3.mul(se3.inv(T0), T)
print "Relative transform of", e, "is", Trel
relative_xforms.append(Trel)
bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,
self.traj,
self.constraints,
relative_xforms,
ee_relative=True,
closest=True)
assert bumpTraj != None
vis.animate(("world", world.robot(0).getName()), bumpTraj)
self.refresh()
self.add_action(bumpTrajectory, "Bump trajectory", 'b')
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 multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
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 callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
def run_cartesian(world, paths):
sim_world = world
sim_robot = world.robot(0)
vis.add("world", world)
planning_world = world.copy()
planning_robot = planning_world.robot(0)
sim = Simulator(sim_world)
robot_controller = RobotInterfaceCompleter(
KinematicSimControlInterface(sim_robot))
#TODO: Uncomment this if you'd like to test in the physics simulation
#robot_controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim))
if not robot_controller.initialize():
raise RuntimeError("Can't connect to robot controller")
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15, 0, 0)
ee_local_axis = (1, 0, 0)
lifth = 0.05
drawing_controller = DrawingController(robot_controller, planning_robot,
planning_robot.getConfig(), paths,
ee_link, ee_local_pos,
ee_local_axis, (0, 0, 1), lifth)
controller_vis = RobotInterfacetoVis(robot_controller)
#note: this "storage" argument is only necessary for jupyter to keep these around and not destroy them once main() returns
def callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
vis.loop(callback=callback)
def sampling_9(serial_num):
world_name = "flatworld"
world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml"
robot_file = "../../data/robot_model_files/robots/irb120_icp.rob"
world = get_world(world_file, robot_file, visualize_robot=True)
robot = world.robot(0)
est_config_list = cal_est_config_list(robot, serial_num)
path = collision_checking(world, est_config_list, 0.001)
if path is False:
sys.exit("The calibration path exist self-collision.")
else:
print("The calibration path is collision free.")
vis.clear()
vis.add("world", world)
for i in range(len(est_config_list)):
vis.add("estimated configure" + str(i), est_config_list[i])
vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5)
vis.spin(float('inf'))
df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi))
df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv",
header=False, index=False)
time.sleep(1.)
est_config_list = est_config_list[0:]
controller = SampleController(est_config_list, robot, '192.168.1.178')
controller.start()
def initialize(self):
vis.add("instructions1",
"Right-click to get the list of intersecting items")
vis.add("instructions2",
"Press q, Ctrl+q, or select Quit from the menu to quit")
vis.GLPluginInterface.initialize(self)
return True
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 main():
world, robot, link_index, geom_index, obs_index = create_world()
q0 = [0, -2, 0]
qf = [3, 1, 3.14]
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
q0=q0,
qf=qf,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create guess and solve
n_grid = 10
guess = np.linspace(q0, qf, n_grid)
rst = trajopt.optimize(guess)
print(rst)
traj = rst['sol']
# show the results
vis.add('world', world)
robot.setConfig(traj[-1])
for i, qi in enumerate(traj[:-1]):
name = 'ghost%d' % i
vis.add(name, qi)
vis.setAttribute(name, 'type', 'Config')
vis.setColor(name, 0, 1, 0, 0.4)
vis.spin(100)
def sphere_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = sphere_staggered_grid(N)
else:
G = sphere_grid(N)
for n in G.nodes():
x = G.node[n]['params']
vis.add(str(n), x)
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
xi = G.node[i]['params']
xj = G.node[j]['params']
vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj]))
vis.hideLabel(str(i) + '-' + str(j))
dist = vectorops.distance(xi, xj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def convert(value_new,cmap_new,feature_new=None,lighting_new=None):
global value,cmap,feature,lighting,w
if value_new is None or value_new == value:
if cmap_new is None or cmap == cmap_new:
if feature_new is None or feature == feature_new:
if lighting_new is None or feature == lighting_new:
return
if cmap_new is not None:
cmap = cmap_new
if value_new is not None:
value = value_new
if feature_new is not None:
feature = feature_new
if lighting_new is not None:
lighting = lighting_new
if lighting_new == 'none':
lighting = None
if value is None:
value = 'z'
if cmap is None:
cmap = 'viridis'
a_app = colorize.colorize(w.rigidObject(0),value,cmap,feature,lighting=lighting)
#a_app.drawGL(a)
#if not value.startswith('n'):
colorize.colorize(a_pc,value,cmap,lighting=lighting)
vis.remove("A")
vis.add("A",a_app)
vis.remove("B")
vis.add("B",a_pc)
vis.dirty("B")
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 qt_template(world):
"""Runs a custom Qt frame around a visualization window"""
if not glinit.available('PyQt5'):
print(
"PyQt5 is not available on your system, try sudo apt-get install python-qt5"
)
return
#Qt objects must be explicitly deleted for some reason in PyQt5...
g_mainwindow = None
#All Qt functions must be called in the vis thread.
#To hook into that thread, you will need to pass a window creation function into vis.customUI.
def makefunc(gl_backend):
global g_mainwindow
g_mainwindow = MyQtMainWindow(gl_backend)
return g_mainwindow
vis.customUI(makefunc)
vis.add("world", world)
vis.setWindowTitle("Klamp't Qt test")
vis.spin(float('inf'))
vis.kill()
#Safe cleanup of all Qt objects created in makefunc.
#If you don't have this, PyQt5 complains about object destructors being called from the wrong thread
del g_mainwindow
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 RobotCOMPlot(SimRobot, vis):
COMPos_start = SimRobot.getCom()
COMPos_end = COMPos_start[:]
COMPos_end[2] = COMPos_end[2] - 7.50
vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end]))
vis.hideLabel("COM", True)
vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("COM", 'width', 5.0)
def set_remaining_milestones(self, remaining, G_list):
self.remaining = remaining
self.remaining_milestones = self.milestones[list(remaining)]
self.fraction = len(remaining) / self.milestones.shape[0]
self.create_internal_subgraphs(G_list)
# if(self.fraction < 0.35):
vis.add('remaining_milestones',
[i[:self.robot.numLinks()] for i in self.remaining_milestones],
color=[1, 0, 0, 0.5])
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 COM2IntersectionPlot(i, vis, COM_Pos, Intersection):
# This function is used to plot PIP from COM to intersections
Edge_Index = str(i)
vis.add("PIPEdgefromCOM:" + Edge_Index,
Trajectory([0, 1], [COM_Pos, Intersection]))
vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def add_hm_to_klampt_vis(self, pcd_fname):
pc_xyzs, non_floor_points_xyz = self.parse_hm(pcd_fname)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
vis.add("pcloud", pcloud)
r, g, b, a = PCloudParser.PCLOUD_COLOR
vis.setColor("pcloud", r, g, b, a)
def shift_object(amt):
global cur_object,cur_grasp,shown_grasps,db
vis.remove(db.objects[cur_object])
cur_object += amt
if cur_object >= len(db.objects):
cur_object = 0
elif cur_object < 0:
cur_object = len(db.objects)-1
vis.add(db.objects[cur_object],w.rigidObject(cur_object))
shift_grasp(None)
def convert(geom,type,label):
global a,b,atypes,btype
if label=='A':
vis.add(label,atypes[type])
vis.setColor(label,1,0,0,0.5)
a = atypes[type]
else:
vis.add(label,btypes[type])
vis.setColor(label,0,1,0,0.5)
b = btypes[type]
def get_paths_svg(fn,add_to_vis=True):
trajs,attrs = svgimport.svg_import_trajectories(fn,center=True,want_attributes=True)
if add_to_vis:
for i,(traj,attr) in enumerate(zip(trajs,attrs)):
name = attr.get('name',"path %d"%i)
vis.add(name,traj)
for a,v in attr.items():
if a != 'name':
vis.setAttribute(name,a,v)
return trajs
def runPlanner(runfunc, name):
global lastPlan
res = runfunc()
if res:
if lastPlan:
vis.remove(lastPlan)
vis.add(name, res)
vis.setColor(name, 1, 0, 0)
vis.animate(("world", robot.getName()), res)
lastPlan = name
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 goToWidget():
#first, set all constraints so they are fit at the robot's last solved configuration, and get the
#current workspace coordinates
vis.setItemConfig("ghost1", self.goalConfig)
self.robot.setConfig(self.goalConfig)
for e, d in zip(self.endeffectors, self.constraints):
d.matchDestination(*self.robot.link(e).getTransform())
wcur = config.getConfig(self.constraints)
wdest = []
for e in self.endeffectors:
xform = vis.getItemConfig("ee_" + robot.link(e).getName())
wdest += xform
print "Current workspace coords", wcur
print "Dest workspace coords", wdest
#Now interpolate
self.robot.setConfig(self.goalConfig)
traj1 = cartesian_trajectory.cartesian_interpolate_linear(
robot,
wcur,
wdest,
self.constraints,
delta=1e-2,
maximize=False)
self.robot.setConfig(self.goalConfig)
traj2 = cartesian_trajectory.cartesian_interpolate_bisect(
robot, wcur, wdest, self.constraints, delta=1e-2)
self.robot.setConfig(self.goalConfig)
#traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False)
traj3 = None
print "Method1 Method2 Method3:"
print " ", (traj1 != None), (traj2 != None), (traj3 != None)
if traj1: traj = traj1
elif traj2: traj = traj2
elif traj3: traj = traj3
else:
traj = cartesian_trajectory.cartesian_interpolate_linear(
robot,
wcur,
wdest,
self.constraints,
delta=1e-2,
maximize=True)
if traj:
print "Result has", len(traj.milestones), "milestones"
self.goalConfig = traj.milestones[-1]
vis.setItemConfig(("world", world.robot(0).getName()),
self.goalConfig)
vis.animate(("world", world.robot(0).getName()),
traj,
speed=0.2,
endBehavior='loop')
vis.setItemConfig("ghost2", traj.milestones[-1])
vis.add("ee_trajectory", traj)
self.refresh()
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 ConvexEdgesPlot(SimRobot, convex_edges_list, vis):
Convex_Edges_Number = len(convex_edges_list) / 2
COMPos = SimRobot.getCom()
for i in range(0, Convex_Edges_Number):
EdgeA = convex_edges_list[2 * i]
EdgeB = convex_edges_list[2 * i + 1]
# Three edges to be added: A->B, A -> COM, B-> COM
Edge_Index = str(i)
vis.add("Edge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB]))
vis.hideLabel("Edge:" + Edge_Index, True)
vis.setAttribute("Edge:" + Edge_Index, 'width', 5.0)
def visualize_line(p1, p2, name="default_line", add_z=0, color=None, hm=None):
if len(p1) < 3:
p1 = [p1[0], p1[1], 0]
if len(p2) < 3:
p2 = [p2[0], p2[1], 0]
p1 = [p1[0], p1[1], p1[2] + add_z]
p2 = [p2[0], p2[1], p2[2] + add_z]
traj = trajectory.Trajectory(milestones=[p1[0:3], p2[0:3]])
vis.add(name, traj)
if color:
VisUtils.set_color(name, color)
def convert(geom, type, label):
global a, b, atypes, btypes, Ta, Tb
if label == 'A':
vis.add(label, atypes[type])
vis.setColor(label, 1, 0, 0, 0.5)
a = atypes[type]
a.setCurrentTransform(*Ta)
else:
vis.add(label, btypes[type])
vis.setColor(label, 0, 1, 0, 0.5)
b = btypes[type]
b.setCurrentTransform(*Tb)
def remesh(geom, label):
global a, b, Ta, Tb
if label == 'A':
a = a.convert(a.type(), 0.06)
vis.add(label, a)
vis.setColor(label, 1, 0, 0, 0.5)
a.setCurrentTransform(*Ta)
elif label == 'B':
b = b.convert(b.type(), 0.06)
vis.add(label, b)
vis.setColor(label, 0, 1, 0, 0.5)
b.setCurrentTransform(*Tb)
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)
print "CSpace stats:"
spacestats = space.getStats()
for k in sorted(spacestats.keys()):
print " ",k,":",spacestats[k]
print "Planner stats:"
planstats = planner.getStats()
for k in sorted(planstats.keys()):
print " ",k,":",planstats[k]
if path:
#save planned milestone path to disk
print "Saving to my_plan.configs"
resource.set("my_plan.configs",path,"Configs")
#visualize path as a Trajectory resource
from klampt.model.trajectory import RobotTrajectory
traj = RobotTrajectory(robot,range(len(path)),path)
#resource.edit("Planned trajectory",traj,world=world)
#visualize path in the vis module
from klampt import vis
vis.add("world",world)
vis.animate(("world",robot.getName()),path)
vis.add("trajectory",traj)
vis.spin(float('inf'))
#play nice with garbage collection
planner.space.close()
planner.close()
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
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
print 2,":",traj.eval(2)
print 3,":",traj.eval(3)
print 4,":",traj.eval(4)
print 5,":",traj.eval(5)
print 6,":",traj.eval(6)
#print some interpolated points
print 0.5,":",traj.eval(0.5)
print 2.5,":",traj.eval(2.5)
#print some stuff after the end of trajectory
print 7,":",traj.eval(6)
print 100.3,":",traj.eval(100.3)
print -2,":",traj.eval(-2)
from klampt import vis
vis.add("point",[0,0,0])
vis.animate("point",traj)
vis.add("traj",traj)
#vis.spin(float('inf')) #show the window until you close it
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
vis.animate("point",traj2)
vis.hide("traj")
vis.add("traj2",traj2.configTrajectory().discretize(0.1))
#vis.spin(float('inf'))
traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4)
#next, try this line instead
#traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4)
