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 play_with_trajectory(traj, configs=[3]):
vis.add("trajectory", traj)
names = []
for i, x in enumerate(traj.milestones):
if i in configs:
print("Editing", i)
names.append("milestone " + str(i))
vis.add(names[-1], x[:])
vis.edit(names[-1])
#vis.addPlot("distance")
vis.show()
while vis.shown():
vis.lock()
t0 = time.time()
updated = False
for name in names:
index = int(name.split()[1])
qi = vis.getItemConfig(name)
if qi != traj.milestones[index]:
traj.milestones[index] = qi
updated = True
if updated:
vis.add("trajectory", traj)
xnew = trajcache.trajectoryToState(traj)
ctest2.setx(xnew)
res = ctest2.minvalue(xtraj)
print(res)
ctest2.clearx()
vis.unlock()
t1 = time.time()
#vis.logPlot("timing","opt",t1-t0)
time.sleep(max(0.001, 0.025 - (t1 - t0)))
def edit_camera_xform(world_fn, xform=None, title=None):
"""Visual editor of the camera position
"""
world = WorldModel()
world.readFile(world_fn)
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
if xform is not None:
sensing.set_sensor_xform(sensor, xform)
vis.createWindow()
if title is not None:
vis.setWindowTitle(title)
vis.resizeWindow(1024, 768)
vis.add("world", world)
vis.add("sensor", sensor)
vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot))
vis.edit("sensor_xform")
def update_sensor_xform():
sensor_xform = vis.getItemConfig("sensor_xform")
sensor_xform = sensor_xform[:9], sensor_xform[9:]
sensing.set_sensor_xform(sensor, sensor_xform)
vis.loop(callback=update_sensor_xform)
sensor_xform = vis.getItemConfig("sensor_xform")
return sensor_xform[:9], sensor_xform[9:]
def 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 __init__(self,h):
GLPluginInterface.__init__(self)
self.hand = h
h.load_frames()
self.world = WorldModel()
#load robot
robot = moving_base.make_moving_base_robot(h.robot_file,self.world,floating=True)
#load object
id = self.world.loadElement(object_file)
if id < 0:
raise IOError("Unable to load "+object_file)
assert self.world.numRigidObjects() > 0
object = self.world.rigidObject(0)
object.setTransform(so3.rotation([1,0,0],math.pi/2),[0,0,0.2])
#set up unit frames
coordinates.setWorldModel(self.world)
coordinates.listItems()
hand_coordinates = coordinates.manager().subgroups[robot.getName()]
hand_coordinates.listItems()
self.unit_frames = []
for i,(key,unit) in enumerate(h.all_units):
self.unit_frames.append(hand_coordinates.addFrame("unit"+str(i),parent=hand_coordinates.frame(key),relativeCoordinates=unit.localTransform))
#set up visualization
vis.add("robot",robot)
vis.edit("robot")
vis.add("rigidObject",object)
vis.edit("rigidObject")
print "COM",object.getMass().getCom()
vis.add("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
vis.add("origin",se3.identity())
#vis.add("coordinates",coordinates.manager())
self.config_name = None
#set up friction solver
folder="FrictionCones/"
self.friction_factories = {}
for key,unit in h.all_units:
if unit.type not in self.friction_factories:
self.friction_factories[unit.type] = stability.GeneralizedFrictionConeFactory()
self.friction_factories[unit.type].load(folder + unit.type)
self.contact_units = []
self.wrenches = None
self.torques = None
self.feasible = None
self.angle = 0
self.wrenchSpace = None
self.wrenchSpace6D = None
self.frame_gl_objects = []
self.wrench_space_gl_object = None
def edit_config():
global world, robot
resource.setDirectory("resources/")
qhands = [r.getConfig() for r in hand_subrobots.values()]
q = resource.get("robosimian_body_stability.config",
default=robot.getConfig(),
doedit=False)
robot.setConfig(q)
for r, qhand in zip(hand_subrobots.values(), qhands):
r.setConfig(qhand)
vis.add("world", world)
vis.edit(("world", robot.getName()), True)
vis.dialog()
vis.edit(("world", robot.getName()), False)
resource.set("robosimian_body_stability.config", robot.getConfig())
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 modification_template(world):
"""Tests a variety of miscellaneous vis functions"""
vis.add("world",world)
robot = world.robot(0)
vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque
import random
for i in range(10):
#set some links to random colors
randlink = random.randint(0,robot.numLinks()-1)
color = (random.random(),random.random(),random.random())
vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color)
#test the on-screen text display
vis.addText("text2","Here's some red text")
vis.setColor("text2",1,0,0)
vis.addText("text3","Here's bigger text")
vis.setAttribute("text3","size",24)
vis.addText("text4","Transform status")
vis.addText("textbottom","Text anchored to bottom of screen",(20,-30))
#test a point
pt = [2,5,1]
vis.add("some point",pt)
#test a rigid transform
vis.add("some blinking transform",[so3.identity(),[1,3,0.5]])
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks()-1)
#point constraint
obj = ik.objective(link,local=[[0,0,0]],world=[pt])
#hinge constraint
#obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
vis.add("ik objective",obj)
#enable plotting
vis.addPlot('plot')
vis.addPlotItem('plot','some point')
vis.setPlotDuration('plot',10.0)
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Manual animation visualization test")
class MyCallback:
def __init__(self):
self.iteration = 0
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
callback = MyCallback()
if not MULTITHREADED:
vis.loop(callback=callback,setup=vis.show)
else:
vis.show()
while vis.shown():
callback()
time.sleep(0.01)
#use this to remove a plot
vis.remove("plot")
vis.kill()
def keyboardfunc(self,c,x,y):
if c == 'h':
print "Help: "
print "- s: save grasp configuration to disk"
print "- l: load grasp configuration from disk"
print "- <: rotate left"
print "- >: rotate right"
print "- [space]: solve for stability"
print "- f: solve for the feasible force volume"
print "- w: solve for the feasible wrench space"
print "- f: solve for the robust force volume"
print "- m: save the current volume to a mesh"
print "- M: load the current volume from a mesh"
print "- 6: calculates the 6D wrench space"
print "- 7: loads the 6D wrench space"
print "- 0: zeros the robot's base transform"
print "- 9: hides the widgets"
if c == 's':
name = raw_input("Enter a name for this configuration > ")
if name.strip() == '':
print "Not saving..."
else:
self.save_config(name)
self.config_name = name
elif c == 'l':
vis.animate("rigidObject",None)
name = raw_input("Enter a name for the configuration to load > ")
if name.strip() == '':
print "Not loading..."
else:
self.load_config(name)
elif c == ',' or c == '<':
vis.animate("rigidObject",None)
self.rotate(math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == '.' or c == '>':
vis.animate("rigidObject",None)
self.rotate(-math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == ' ':
self.solve(self.contact_units)
elif c == 'f':
if self.wrenchSpace6D != None:
cm = se3.apply(self.world.rigidObject(0).getTransform(),self.world.rigidObject(0).getMass().getCom())
self.wrenchSpace = self.wrenchSpace6D.getSlice(cm).convex_decomposition[0]
self.wrench_space_gl_object = None
else:
self.wrenchSpace = self.calculate_force_volume((0,0,0))
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'w':
self.wrenchSpace = self.calculate_3D_wrench_space()
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'r':
dropout = raw_input("How much to drop out? (default 0) > ")
if dropout.strip() == '':
dropout = 0
else:
dropout = float(dropout.strip())
perturb = raw_input("How much to perturb? (default 0.1) > ")
if perturb.strip() == '':
perturb = 0.1
else:
perturb = float(perturb.strip())
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace = self.calculate_robust_force_volume((0,0,0),dropout=dropout,configurationPerturbation=perturb,maxVolumes=N)
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'm':
assert self.wrenchSpace != None
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getSaveFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = polytope.hull_to_klampt_geom(self.wrenchSpace)
g.saveFile(str(name))
elif c == 'M':
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getOpenFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = Geometry3D()
g.loadFile(str(name))
self.wrenchSpace = polytope.klampt_geom_to_hull(g)
self.wrench_space_gl_object = None
#decomp = stability.approximate_convex_decomposition(g)
#print "Convex decomposition into",len(decomp),"pieces"
print len(polytope.pockets(g)),"pocket faces"
elif c == '6':
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace6D = self.calculate_6D_wrench_space(N)
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '7':
self.wrenchSpace6D = self.load_6D_wrench_space()
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '0':
robot = self.world.robot(0)
q = robot.getConfig()
q[0:6] = [0]*6
robot.setConfig(q)
vis.setItemConfig("robot",robot.getConfig())
coordinates.updateFromWorld()
elif c == '9':
vis.edit("robot",False)
vis.edit("rigidObject",False)
#vis.hide("COM")
#object = self.world.rigidObject(0)
#vis.edit("COM")
else:
return False
return True
robot = world.robot(0)
link = robot.link(7)
#coordinates of the constrained IK point on the robot, in the link's local frame
localpos = [0.17,0,0]
#coordinates of the IK goal in world coordinates
position = [0,0,0]
t0 = time.time()
#draw the world, local point in yellow, and target point in white
vis.add("world",world)
vis.add("local_point",position)
vis.setColor("local_point",1,1,0)
vis.add("target",position)
vis.setColor("target",1,1,1)
vis.edit("target")
vis.show()
while vis.shown():
vis.lock()
#move the target and solve
t = time.time()-t0
r = 0.4
position = vis.getItemConfig("target")
position[1] = r*math.cos(t)
position[2] = 0.7+r*math.sin(t)
q = solve_ik(link,localpos,position)
robot.setConfig(q)
vis.setItemConfig("local_point",link.getWorldPosition(localpos))
vis.setItemConfig("target",position)
vis.unlock()
time.sleep(0.01)
#if res:
# print "Return value",val
# endeffectors = val
vis.add("world",world)
vis.listItems()
vis.setColor(("world",world.robot(0).getName()),1,1,0,1)
coordinates.setRobotModel(robot)
eenames = [robot.link(e).getName() for e in endeffectors]
eeobjectives = []
for e in endeffectors:
f = coordinates.frame(robot.link(e).getName())
fw = coordinates.addFrame(robot.link(e).getName()+"_tgt",robot.link(e).getTransform())
assert f != None
vis.add("ee_"+robot.link(e).getName(),fw)
vis.edit("ee_"+robot.link(e).getName())
obj = coordinates.ik_fixed_objective(f)
eeobjectives.append(obj)
#this tests the cartesian interpolation stuff
print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****"
vis.setWindowTitle("Klamp't Cartesian interpolation test")
vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
vis.hide("ghost1")
vis.hide("ghost2")
vis.animate(("world",world.robot(0).getName()),None)
raise RuntimeError("Couldn't read the world file")
shelf = make_shelf(w,*shelf_dims)
shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height))
obj = w.makeRigidObject("point_cloud_object") #Making Box
obj.geometry().loadFile(KLAMPT_Demo+"/data/objects/apc/genuine_joe_stir_sticks.pcd")
#set up a "reasonable" inertial parameter estimate for a 200g object
m = obj.getMass()
m.estimate(obj.geometry(),0.200)
obj.setMass(m)
#we'll move the box slightly forward so the robot can reach it
obj.setTransform(so3.identity(),[shelf_offset_x-0.05,shelf_offset_y-0.3,shelf_height+0.01])
vis.add("world",w)
vis.add("ghost",w.robot(0).getConfig(),color=(0,1,0,0.5))
vis.edit("ghost")
from klampt import Simulator
sim = Simulator(w)
def setup():
vis.show()
def callback():
sim.controller(0).setPIDCommand(vis.getItemConfig("ghost"),[0]*w.robot(0).numLinks())
sim.simulate(0.01)
sim.updateWorld()
vis.loop(setup=setup,callback=callback)
topic = sys.argv[1]
#rospy.init_node("Klampt_point_cloud_visualizer")
world = klampt.WorldModel()
world.makeRigidObject("point_cloud")
g = world.rigidObject(0).geometry()
g.loadFile("ros:/" + topic)
if len(sys.argv) > 2 and sys.argv[2] == 'save':
dosave = True
else:
dosave = False
point_cloud_count = 0
vis.add("world", world)
vis.edit(("world", "point_cloud"))
def updatePointCloud():
#in klampt / robotio.h -- this needs to be done to update ROS
processed = io.ProcessStreams()
if processed:
#don't strictly need the prior if statement, this is just a slight optimization
#if the sender is slower than the visualization
if g.empty():
print("empty")
else:
pc = g.getPointCloud()
print(pc.numPoints(), "points and", pc.numProperties(),
"properties")
def __init__(self, fn):
## Creates a world and loads all the items on the command line
self.world = WorldModel()
self.robotSystemName = 'O'
for f in fn:
print(f)
res = self.world.readFile(f)
if not res:
raise RuntimeError("Unable to load model " + fn)
self.showVis = False
coordinates.setWorldModel(self.world)
vis.lock()
bW.getDoubleRoomWindow(self.world, 8, 8, 1)
vis.unlock()
## Add the world to the visualizer
vis.add("world", self.world)
vp = vis.getViewport()
vp.w, vp.h = 1800, 800
vis.setViewport(vp)
self.robots = []
self.n = self.world.numRobots()
for i in range(self.n):
self.robots.append(
sphero6DoF(self.world.robot(i),
self.world.robot(i).getName()))
self.eps = 0.000001
self.sj = [[0, 0, 0], [0.2, 0, 0]]
self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0],
[0.070534, -0.097082, 0], [0.057063, -0.018541, 0],
[0.11413, 0.037082, 0], [0.035267, 0.048541, 0],
[0, 0.12, 0], [-0.035267, 0.048541, 0],
[-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]]
self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0],
[0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0],
[0.6, 0, 0], [0.8, 0, 0]]
self.sj = self.sjL
self.xB = [-4, 4]
self.yB = [-4, 4]
self.zB = [0.02, 1]
self.rad = 0.04
self.currConfig = [0, 0, 1, 1, 0]
self.scMin = 1
self.scXMin = 1
self.scYMin = 2
self.sumDist = 0
if self.n > 1:
minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1]))
minSijX = math.fabs(self.sj[0][0] - self.sj[1][0])
minSijY = math.fabs(self.sj[0][1] - self.sj[1][1])
for i in range(self.n):
for j in range(self.n):
if i != j:
dist = vectorops.norm(
vectorops.sub(self.sj[i], self.sj[j]))
if dist < minSij:
minSij = dist
dist = math.fabs(self.sj[i][0] - self.sj[j][0])
if dist < minSijX:
minSijX = dist
dist = math.fabs(self.sj[i][1] - self.sj[j][1])
if dist < minSijY:
minSijY = dist
for i in range(self.n):
self.sumDist += vectorops.norm(self.sj[i])
if minSij > self.eps:
self.scMin = 2 * math.sqrt(2) * self.rad / minSij
if minSijX > self.eps:
self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX
if minSijY > self.eps:
self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY
self.scMax = max(2, self.scMin)
self.scB = [self.scMin, 2 * self.scMin]
print('Minimum scale')
print(self.scMin)
vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]])
vis.setAttribute(self.robotSystemName, "size", 12)
vis.edit(self.robotSystemName)
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 32)
vis.edit("WCS")
self.collisionChecker = collide.WorldCollider(self.world)
if self.showVis:
## Display the world coordinate system
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom",
"WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
vis.show()
simTime = 60
startTime = time.time()
oldTime = startTime
self.setConfig(0, 0, 1, 1, 0)
q = self.robots[0].getConfig()
if self.showVis:
q2f = ['{0:.2f}'.format(elem) for elem in q]
strng = "Robot configuration: " + str(q2f)
vis.addText("textConfig", strng)
colFlag = self.checkCollision()
print(colFlag)
if self.showVis:
time.sleep(10)
def grasp_edit_ui(gripper, object, grasp=None):
assert gripper.klampt_model is not None
world = WorldModel()
res = world.readFile(gripper.klampt_model)
if not res:
raise ValueError("Unable to load klampt model")
robot = world.robot(0)
base_link = robot.link(gripper.base_link)
base_xform = base_link.getTransform()
base_xform0 = base_link.getTransform()
parent_xform = se3.identity()
if base_link.getParent() >= 0:
parent_xform = robot.link(base_link.getParent()).getTransform()
if grasp is not None:
base_xform = grasp.ik_constraint.closestMatch(*base_xform)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
robot.setConfig(
gripper.set_finger_config(robot.getConfig(), grasp.finger_config))
q0 = robot.getConfig()
grob = gripper.get_subrobot(robot)
grob._links = [l for l in grob._links if l != gripper.base_link]
#set up visualizer
oldWindow = vis.getWindow()
if oldWindow is None:
oldWindow = vis.createWindow()
vis.createWindow()
vis.add("gripper", grob)
vis.edit("gripper")
vis.add("object", object)
vis.add("base_xform", base_xform)
vis.edit("base_xform")
def make_grasp():
return Grasp(ik.objective(base_link, R=base_xform[0], t=base_xform[1]),
gripper.finger_links,
gripper.get_finger_config(robot.getConfig()))
#add hooks
robot_appearances = [
robot.link(i).appearance().clone() for i in range(robot.numLinks())
]
robot_shown = [True]
def toggle_robot(arg=0, data=robot_shown):
vis.lock()
if data[0]:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().setDraw(False)
data[0] = False
else:
for i in range(robot.numLinks()):
if i not in grob._links and i != gripper.base_link:
robot.link(i).appearance().set(robot_appearances[i])
data[0] = True
vis.unlock()
def randomize():
print("TODO")
def reset():
vis.lock()
robot.setConfig(q0)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform0))
vis.unlock()
vis.add("base_xform", base_xform0)
vis.edit("base_xform")
vis.setItemConfig("gripper", grob.getConfig())
def save():
fmt = gripper.name + "_" + object.getName() + '_grasp_%d.json'
ind = 0
while os.path.exists(fmt % (ind, )):
ind += 1
fn = fmt % (ind, )
g = make_grasp()
print("Saving grasp to", fn)
with open(fn, 'w') as f:
json.dump(g.toJson(), f)
vis.addAction(toggle_robot, 'Toggle show robot', 'v')
vis.addAction(randomize, 'Randomize', 'r')
vis.addAction(reset, 'Reset', '0')
vis.addAction(save, 'Save to disk', 's')
def loop_setup():
vis.show()
def loop_callback():
global base_xform
xform = vis.getItemConfig("base_xform")
base_xform = (xform[:9], xform[9:])
vis.lock()
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
vis.unlock()
def loop_cleanup():
vis.show(False)
vis.loop(setup=loop_setup, callback=loop_callback, cleanup=loop_cleanup)
# this works with Linux/Windows, but not Mac
# loop_setup()
# while vis.shown():
# loop_callback()
# time.sleep(0.02)
# loop_cleanup()
g = make_grasp()
#restore RobotModel
base_link.setParentTransform(*se3.mul(se3.inv(parent_xform), base_xform0))
vis.setWindow(oldWindow)
return g
#test a point
pt = [2, 5, 1]
vis.add("some point", pt)
#test a rigid transform
vis.add("some blinking transform", [so3.identity(), [1, 3, 0.5]])
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks() - 1)
#point constraint
obj = ik.objective(link, local=[[0, 0, 0]], world=[pt])
#hinge constraint
#obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
vis.add("ik objective", obj)
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test the on-screen text display
vis.addText("text2", "Here's some red text")
vis.setColor("text2", 1, 0, 0)
vis.addText("text3", "Here's bigger text")
vis.setAttribute("text3", "size", 24)
vis.addText("text4", "Transform status")
vis.addText("textbottom", "Text anchored to bottom of screen", (20, -30))
vis.addPlot('plot')
vis.addPlotItem('plot', 'some point')
vis.setPlotDuration('plot', 10.0)
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print("Angle between")
print(so3.matrix(Ra))
print("and")
print(so3.matrix(Rb))
print("is", so3.distance(Ra, Rb))
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
vis.edit("end")
vis.setAttribute("world", "length", 0.25)
vis.setAttribute("interpolated", "fancy", True)
vis.setAttribute("interpolated", "width", 0.03)
vis.setAttribute("interpolated", "length", 1.0)
vis.addText("angle_display", "")
vis.show()
t0 = time.time()
while vis.shown():
#interpolate with a period of 3 seconds
period = 3.0
u = ((time.time() - t0) % period) / period
T = interpolate_transform(Ta, Tb, u)
#uncomment for question 3
print "Input object has type", obj.type(), "with", obj.numElements(
), "elements"
geom1 = PenetrationDepthGeometry(obj, gridres, pcres)
geom2 = PenetrationDepthGeometry(obj2, gridres, pcres)
geom2.setTransform((so3.identity(), [1.2, 0, 0]))
vis.add("obj1", geom1.grid)
vis.add("obj2", geom2.pc)
vis.setColor("obj1", 1, 1, 0, 0.5)
vis.setColor("obj2", 0, 1, 1, 0.5)
vis.setAttribute("obj2", "size", 3)
vis.addPlot("timing")
vis.add("transform", se3.identity())
vis.edit("transform")
vis.show()
oldcps = []
while vis.shown():
vis.lock()
t0 = time.time()
q = vis.getItemConfig("transform")
T = (q[:9], q[9:])
#debug optimization
geom1.setTransform(T)
Tcollfree, trace, tracetimes, cps = optimizeCollFree(geom1,
geom2,
T,
verbose=0,
want_trace=True,
for i in xrange(1,world.numRobots()):
for j in xrange(world.robot(i).numLinks()):
obstacles.append(world.robot(i).link(j))
for i in xrange(world.numRigidObjects()):
obstacles.append(world.rigidObject(i))
#for i in xrange(world.numTerrains()):
# obstacles.append(world.terrain(i))
print("%d robots, %d rigid objects, %d terrains"%(world.numRobots(),world.numRigidObjects(),world.numTerrains()))
assert len(obstacles) > 0
constraints,pairs = geometryopt.makeCollisionConstraints(robot,obstacles,gridres,pcres)
print("Created",len(constraints),"constraints")
vis.add("world",world)
movableObjects = []
for i in xrange(world.numRigidObjects()):
vis.edit(("world",world.rigidObject(i).getName()))
movableObjects.append(("world",world.rigidObject(i).getName()))
#extract geometries from constraints
linkgeoms = [None]*robot.numLinks()
obstaclegeoms = [None]*len(obstacles)
for c,(link,obj) in zip(constraints,pairs):
for i,obs in enumerate(obstacles):
if obj is obs:
obstaclegeoms[i] = c.env
linkgeoms[link.index] = c.robot.geometry[link.index]
#assert all(o is not None for o in linkgeoms),"Hm... couldn't find link geometries?"
assert all(o is not None for o in obstaclegeoms),"Hm... couldn't find obstacle geometries?"
#this recreates the geometries too much
#obstaclegeoms = [PenetrationDepthGeometry(obs.geometry(),gridres,pcres) for obs in obstacles]
from klampt import *
from klampt import vis
import sys
if len(sys.argv) <= 1:
fn = "../../data/baxter_apc.xml"
else:
fn = sys.argv[1]
w = WorldModel()
w.readFile(fn)
w.makeRobot("reduced")
dofmap = w.robot(1).reduce(w.robot(0))
vis.add("robot", w.robot(0))
vis.add("reduced", w.robot(1), color=(1, 0, 0))
vis.edit("reduced")
vis.loop()
qmin, qmax = robot.getJointLimits()
for i in range(len(qmin)):
if qmax[i] - qmin[i] > math.pi * 2:
qmin[i] = -float('inf')
qmax[i] = float('inf')
robot.setJointLimits(qmin, qmax)
qstart = resource.get("start.config", world=world)
#add the world elements individually to the visualization
vis.add("world", world)
vis.add("start", qstart, color=(0, 1, 0, 0.5))
# qgoal = resource.get("goal.config",world=world)
qgoal = resource.get("goal_easy.config", world=world)
robot.setConfig(qgoal)
vis.edit(vis.getItemName(robot))
def planTriggered():
global world, robot
qtgt = vis.getItemConfig(vis.getItemName(robot))
qstart = vis.getItemConfig("start")
robot.setConfig(qstart)
if PROBLEM == '1a':
path = planning.feasible_plan(world, robot, qtgt)
else:
path = planning.optimizing_plan(world, robot, qtgt)
if path is not None:
ptraj = trajectory.RobotTrajectory(robot, milestones=path)
ptraj.times = [t / len(ptraj.times) * 5.0 for t in ptraj.times]
#this function should be used for creating a C1 path to send to a robot controller
m.estimate(g, 1.0, 0.0)
H = m.getInertia()
print(" COM volume-only %.3f %.3f %.3f" % tuple(m.getCom()))
print(" Inertia volume-only %.3f %.3f %.3f" % (H[0], H[4], H[8]))
a = atypes['GeometricPrimitive']
b = btypes['GeometricPrimitive']
vis.add("A", a)
vis.add("B", b)
vis.setColor("A", 1, 0, 0, 0.5)
vis.setColor("B", 0, 1, 0, 0.5)
Ta = se3.identity()
Tb = [so3.identity(), [1, 0, 0]]
vis.add("Ta", Ta)
vis.add("Tb", Tb)
vis.edit("Ta")
vis.edit("Tb")
ray = ([-3, 0, 0], [1, 0, 0])
vis.add("ray",
Trajectory([0, 1], [ray[0], vectorops.madd(ray[0], ray[1], 20)]),
color=[1, 0.5, 0, 1])
vis.add("hitpt", [0, 0, 0], color=[1, 0, 1, 1])
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]
