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 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 __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
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 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 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 make_thumbnails(folder,outputfolder):
for fn in glob.glob(folder+"/*"):
filename = os.path.basename(fn)
mkdir_p(outputfolder)
print(os.path.splitext(filename)[1])
if os.path.splitext(filename)[1] in ['.xml','.rob','.obj','.env']:
#world file
print(fn)
world = WorldModel()
world.readFile(fn)
im = resource.thumbnail(world,(128,96))
if im != None:
im.save(os.path.join(outputfolder,filename+".thumb.png"))
else:
print("Could not save thumbnail.")
exit(0)
vis.lock()
del world
vis.unlock()
elif os.path.splitext(filename)[1] in resource.knownTypes():
res = resource.get(filename,doedit=False,directory=folder)
im = resource.thumbnail(res,(128,96))
if im != None:
im.save(os.path.join(outputfolder,filename+".thumb.png"))
else:
print("Could not save thumbnail.")
exit(0)
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 loop_callback():
global base_xform
xform = vis.getItemConfig("base_xform")
base_xform = (xform[:9], xform[9:])
vis.lock()
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
vis.unlock()
def setMode(m):
global mode, drawExtra
mode = m
vis.lock()
for s in drawExtra:
vis.remove(s)
drawExtra = set()
vis.unlock()
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 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 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 toggle_robot(arg=0, data=robot_shown):
vis.lock()
if data[0]:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().setDraw(False)
data[0] = False
else:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().set(robot_appearances[i])
data[0] = True
vis.unlock()
def 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 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 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 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 setConfig(self, x, y, z, sc, tht):
self.currConfig = [x, y, z, sc, tht]
cosTht = math.cos(tht)
sinTht = math.sin(tht)
vis.lock()
pt = [x, y, z]
rotMat = mathUtils.euler_zyx_mat([tht, 0, 0])
vis.add(self.robotSystemName, [rotMat, pt])
for iR in range(self.n):
q = self.robots[iR].getConfig()
scSj = vectorops.mul(self.sj[iR], sc)
q[0] = self.currConfig[0] + cosTht * scSj[0] - sinTht * scSj[1]
q[1] = self.currConfig[1] + sinTht * scSj[0] + cosTht * scSj[1]
q[2] = self.currConfig[2] + scSj[2]
self.robots[iR].setConfig(q)
vis.unlock()
self.checkCollision()
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 checkCollision(self):
vis.lock()
## Checking collision
collisionFlag = False
for iR in range(self.n):
collRT0 = self.collisionChecker.robotTerrainCollisions(
self.world.robot(iR), self.world.terrain(0))
for i, j in collRT0:
collisionFlag = True
strng = "Robot collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
for iR in range(self.n):
collRT2 = self.collisionChecker.robotObjectCollisions(
self.world.robot(iR))
for i, j in collRT2:
collisionFlag = True
strng = self.world.robot(
iR).getName() + " collides with " + j.getName()
if self.showVis:
print(strng)
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
collRT3 = self.collisionChecker.robotSelfCollisions()
for i, j in collRT3:
collisionFlag = True
strng = i.getName() + " collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
if not collisionFlag:
if self.showVis:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
vis.unlock()
return collisionFlag
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 localPlanner(self, n1, n2, sleepFlag=False):
dist = self.env.n * vectorops.norm(
vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z]))
vel = 0.04
tm = dist / vel
for i in range(int(tm)):
delta = i / float(tm)
x = n1.x + (n2.x - n1.x) * delta
y = n1.y + (n2.y - n1.y) * delta
z = n1.z + (n2.z - n1.z) * delta
sc = n1.sc + (n2.sc - n1.sc) * delta
tht = n1.tht + (n2.tht - n1.tht) * delta
vis.lock()
self.env.setConfig(x, y, z, sc, tht)
vis.unlock()
colFlag = False
colFlag = self.env.checkCollision()
if (colFlag):
return False
if sleepFlag:
time.sleep(vel / 2)
self.env.setConfig(n2.x, n2.y, n2.z, n2.sc, n2.tht)
return True
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)
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
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 determine_reachable_points_robot(self,
sampling_places,
world,
robot,
lamp,
collider,
show_vis=False,
neighborhood=0.4,
float_height=0.08,
base_linknum=2):
self.set_robot_link_collision_margins(robot, 0.15, collider, [])
show_vis = True
if (show_vis):
vis.add('world', world)
# eliminating draw distance
vis.lock()
# time.sleep(0.5)
vp = vis.getViewport()
# vp.h = 640
# vp.w = 640
vp.clippingplanes = [0.1, 10000]
tform = pickle.load(open('tform.p', 'rb'))
vp.setTransform(tform)
scale = 1
vp.w = 1853 // scale
vp.h = 1123 // scale
# vis.setViewport(vp)
vis.scene().setViewport(vp)
vis.unlock()
vis.show()
reachable = []
configs = []
# world.terrain(0).geometry().setCollisionMargin(0.05)
for place in tqdm(sampling_places):
robot.setConfig(place)
reachable.append(
self.solve_ik_near_sample(robot,
lamp,
collider,
world,
place,
restarts=10,
tol=1e-2,
neighborhood=neighborhood,
float_height=float_height))
# print('reachable? = {}'.format(reachable[-1]))
cfig = robot.getConfig()
# print(cfig[2])
configs.append(cfig + place.tolist())
# after checking for those margins, we reset the robot to its original configs for general motion planning
# self.set_robot_link_collision_margins(robot,0,collider,self.angular_dofs)
self.set_robot_link_collision_margins(robot, 0, collider, [])
# we also reset the base and environment collision to simplify path planning:
world.terrain(0).geometry().setCollisionMargin(0)
return reachable, configs
def next_point(initialPos, randomPoint):
nn = initialPos
new_point = []
new_point_dist = []
times = []
collisionFlag = False
distanceFlag = False
for i in range(len(actions)):
s = 0
t = 0
theta = 0
old_distance = dist(nn, randomPoint)
initialPos = nn
while s <= EPSILON and t != 16:
vis.lock()
robot.setConfig(initialPos)
robot.velControlKin(actions[i][0] * u1, actions[i][1] * u2, dt)
vis.unlock()
if not checkCollision():
collisionFlag = False
t = t + 1
n_p = robot.getConfig()
else:
collisionFlag = True
break
if old_distance <= dist(n_p, randomPoint):
distanceFlag = True
break
else:
distanceFlag = False
old_distance = dist(n_p, randomPoint)
initialPos = n_p
if t != 0:
times.append(t)
new_point.append(n_p)
new_point_dist.append(dist(initialPos, randomPoint))
else:
break
if not new_point or not new_point_dist:
return None
else:
minInd = 0
print("new_point_dist", new_point_dist)
minVal = min(new_point_dist)
print("minVal", minVal)
for x in range(len(new_point_dist)):
if minVal == new_point_dist[x]:
minInd = x
np = new_point[minInd]
np.insert(3, times[minInd])
np.insert(4, actions[minInd][0])
np.insert(5, actions[minInd][1])
return np
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_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
