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 main():
print("============================================================")
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print("USAGE: simtest.py [world_file] [controller files (.py)]")
print("============================================================")
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
sys.path.append(os.path.abspath("../control"))
import trajectory_controller
#it's a path file, try to load it
controller = trajectory_controller.make(world.robot(i), c)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)
def grasp_plan_main():
world = WorldModel()
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
world.readFile("table.xml")
xform = resource.get("table_camera_00.xform",type='RigidTransform')
sensing.set_sensor_xform(sensor,xform)
box = GeometricPrimitive()
box.setAABB([0,0,0],[1,1,1])
box = resource.get('table.geom','GeometricPrimitive',default=box,world=world,doedit='auto')
bmin,bmax = [v for v in box.properties[0:3]],[v for v in box.properties[3:6]]
nobj = 5
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn,world)
#TODO: you might want to mess with colors here too
obj.appearance().setSilhouette(0)
obj_fns.append(fn)
for i in range(world.numTerrains()):
#TODO: you might want to mess with colors here too
world.terrain(i).appearance().setSilhouette(0)
problem1.arrange_objects(world,obj_fns,bmin,bmax)
intrinsics = dict()
w = int(sensor.getSetting('xres'))
h = int(sensor.getSetting('yres'))
xfov = float(sensor.getSetting('xfov'))
yfov = float(sensor.getSetting('yfov'))
intrinsics['cx'] = w/2
intrinsics['cy'] = h/2
intrinsics['fx'] = math.tan(xfov*0.5)*h*2
intrinsics['fy'] = math.tan(xfov*0.5)*h*2
print("Intrinsics",intrinsics)
planner = ImageBasedGraspPredictor()
def do_grasp_plan(event=None,world=world,sensor=sensor,planner=planner,camera_xform=xform,camera_intrinsics=intrinsics):
sensor.kinematicReset()
sensor.kinematicSimulate(world,0.01)
rgb,depth = sensing.camera_to_images(sensor)
grasps,scores = planner.generate(camera_xform,camera_intrinsics,rgb,depth)
for i,(g,s) in enumerate(zip(grasps,scores)):
color = (1-s,s,0,1)
g.add_to_vis("grasp_"+str(i),color=color)
def resample_objects(event=None,world=world,obj_fns=obj_fns,bmin=bmin,bmax=bmax):
problem1.arrange_objects(world,obj_fns,bmin,bmax)
vis.add("world",world)
vis.add("sensor",sensor)
vis.addAction(do_grasp_plan,'Run grasp planner','p')
vis.addAction(resample_objects,'Sample new arrangement','s')
vis.run()
def so3_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = so3_staggered_grid(N)
else:
G = so3_grid(N)
vispt = [1, 0, 0]
for n in G.nodes():
R = G.node[n]['params']
trans = so3.apply(R, vispt)
#trans = so3.axis_angle(R)[0]
#trans = vectorops.unit(so3.quaternion(R)[:3])
vis.add(str(n), [R, trans])
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
Ri = G.node[i]['params']
Rj = G.node[j]['params']
tmax = 9
times = range(tmax + 1)
milestones = []
for t in times:
u = float(t) / tmax
trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
milestones.append(trans)
vis.add(
str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
vis.hideLabel(str(i) + '-' + str(j))
dist = so3.distance(Ri, Rj)
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()
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
traj = trajectory.path_to_trajectory(ptraj,
timing='robot',
smoothing=None)
#show the path in the visualizer, repeating for 60 seconds
vis.animate("start", traj)
vis.add("traj", traj, endeffectors=[9])
vis.addAction(planTriggered, "Plan to target", 'p')
vis.run()
def print_config(self):
config = self.robotWidget.get()
print("Config:",config)
self.world.robot(0).setConfig(config)
def save_world(self):
fn = "widgets_test_world.xml"
print("Saving file to",fn)
self.world.saveFile(fn)
if __name__ == "__main__":
print("==============================================================================")
print("gl_vis_widgets.py: This example demonstrates how to manually add visualization")
print("widgets to pose a robot using the GLWidgetPlugin interface")
if len(sys.argv)<=1:
print()
print("USAGE: gl_vis_widgets.py [world_file]")
print("==============================================================================")
if len(sys.argv)<=1:
exit()
world = klampt.WorldModel()
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model "+fn)
viewer = MyGLViewer(world)
vis.run(viewer)
def main():
# This funciton is used for the multi-contact humanoid push recovery
# The default robot to be loaded is the HRP2 robot in this same folder
print "This funciton is used for the multi-contact humanoid push recovery"
if len(sys.argv) <= 1:
print "USAGE: The default robot to be loaded is the HRP2 robot in this same folder"
exit()
world = WorldModel() # WorldModel is a pre-defined class
input_files = sys.argv[1:]
# sys.argv will automatically capture the input files' names
for fn in input_files:
result = world.readFile(
fn
) # Here result is a boolean variable indicating the result of this loading operation
if not result:
raise RuntimeError("Unable to load model " + fn)
global Terr_No
Terr_No = world.numTerrains()
# This system call will rewrite the robot_angle_init.config into the robot_angle_init.txt
# However, the initiali angular velocities can be customerized by the user in robot_angvel_init.txt
# # Now world has already read the world file
# The first step is to validate the feasibility of the given initial condition
Sigma_Init = [
1, 1, 0, 0
] # This is the initial contact status: 1__------> contact constraint is active,
# 0--------> contact constraint is inactive
# [left foot, right foot, left hand, right hand]
DOF_val, Config_Init = Configuration_Loader(
) # This is the initial condition: joint angle and joint angular velocities
# The initial joint angular velocties will be of the same length and value as the config_init
global DOF
DOF = DOF_val
Velocity_Init = np.zeros(len(Config_Init))
for i in range(0, len(Config_Init)):
Velocity_Init[i] = Config_Init[i]
State_Init = np.append(Config_Init, Velocity_Init)
# Now it is the validation of the feasibility of the given initial condition
Init_Config, Init_Velocity = Robot_Init_Opt_fn(world, Sigma_Init,
State_Init)
#
# Init_Config_File = open('Init_Config.txt', 'w')
# for Config in Init_Config:
# print>>Init_Config_File, Config
# Init_Config_File.close()
#
# Init_Velocity_File = open('Init_Velocity.txt', 'w')
# for Velocity in Init_Velocity:
# print>>Init_Velocity_File, Velocity
# Init_Velocity_File.close()
# However, the easy method is to directly read in the initial configuration and the initial velocity
# with open('./Init_Config.txt') as Init_Config_File:
# Init_Config = Init_Config_File.read().splitlines()
# Init_Config = [float(i) for i in Init_Config]
#
# with open('./Init_Velocity.txt') as Init_Velocity_File:
# Init_Velocity = Init_Velocity_File.read().splitlines()
# Init_Velocity = [float(i) for i in Init_Velocity]
# ipdb.set_trace()
# Here the Initial robot state has been optimized
# since the optimization of the initial configuration takes longer time, we save the optimized result and directly use it
# ipdb.set_trace()
sim_robot = world.robot(0)
viewer = MyGLViewer(world, Init_Config, Init_Velocity, Sigma_Init)
# This part of the code is used to find the extremities in the local coordinate
# Local_Extremeties = [0.1, 0, -0.1, -0.15 , 0, -0.1, 0.1, 0, -0.1, -0.15 , 0, -0.1, 0, 0, -0.22, 0, 0, -0.205] # This 6 * 3 vector describes the local coordinate of the contact extremeties in their local coordinate
# The left foot: 4 points!
# vis.pushPlugin(viewer)
#add the world to the visualizer
# vis.add("world", world)
# sim_robot.setConfig(Init_Config)
#
# vis.show()
# vis.lock()
# # left_foot_offset1 = [0.13, 0.055, -0.105]
# left_foot_offset1 = [-0.015, 0.0, -0.205]
# # left_foot_offset2 = [0.1, 0, -0.1]
# # left_foot_offset3 = [0.1, 0, -0.1]
# # left_foot_offset4 = [0.1, 0, -0.1]
#
# low_point = left_foot_offset1
# up_point = low_point[:]
# up_point[2] = up_point[2] - 5
#
# current_link = sim_robot.link(End_Link_Ind[3])
# current_link_appear = current_link.appearance()
#
# # print current_link.getAxis()
#
# link_color = random_colors()
#
# # print current_link.getWorldDirection([ 0, 1, 0])
#
# current_link_appear.setColor(link_color[0], link_color[1], link_color[2])
#
# low_point_coor = current_link.getWorldPosition(low_point)
# up_point_coor = current_link.getWorldPosition(up_point)
# print low_point_coor
#
# 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("foot force", Trajectory([0, 1], [low_point_coor, up_point_coor]))
# 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]))
# vis.unlock()
# ipdb.set_trace()
vis.run(viewer)
def main():
print("""
===============================================================================
A program to quickly browse Klamp't objects.
USAGE: %s [item1 item2 ...]
where the given items are world, robot, terrain, object, or geometry files. Run
it without arguments
%s
for an empty reference world. You may add items to the reference world using
the `Add to World` button. If you know what items to use in the reference
world, run it with
%s world.xml
or
%s item1 item2 ...
where the items are world, robot, terrain, object, or geometry files.
===============================================================================
""" % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0]))
#must be explicitly deleted for some reason in PyQt5...
g_browser = None
def makefunc(gl_backend):
global g_browser
browser = ResourceBrowser(gl_backend)
g_browser = browser
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
return browser
vis.customUI(makefunc)
vis.setWindowTitle("Klamp't Resource Browser")
vis.run()
del g_browser
return
#this code below is incorrect...
app = QtWidgets.QApplication(sys.argv)
browser = ResourceBrowser()
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
#browser.splitter.setWindowState(QtCore.Qt.WindowMaximized)
browser.setWindowTitle("Klamp't Resource Browser")
browser.show()
# Start the main loop.
res = app.exec_()
return res
from klampt import *
from klampt import vis
from klampt.vis.glrobotprogram import GLWorldPlugin
class MyPlugin(GLWorldPlugin):
def __init__(self,world):
GLWorldPlugin.__init__(self,world)
def mousefunc(self,button,state,x,y):
#Put your mouse handler here
#the current example prints out the list of objects clicked whenever
#you right click
print "mouse",button,state,x,y
if button==2:
if state==0:
print [o.getName() for o in self.click_world(x,y)]
return
GLWorldPlugin.mousefunc(self,button,state,x,y)
def motionfunc(self,x,y,dx,dy):
return GLWorldPlugin.motionfunc(self,x,y,dx,dy)
world = WorldModel()
if not world.readFile("../../../Klampt-examples/data/athlete_plane.xml"):
raise RuntimeError("Couldn't load world")
vis.run(MyPlugin(world))
def simulation_template(world):
"""Runs a custom simulation plugin"""
viewer = MyGLSimulationViewer(world)
vis.run(viewer)
vis.kill()
glVertex3f(V[j][0], V[j][1], V[j][2])
glEnd()
glDisable(GL_BLEND)
glEnable(GL_LIGHTING)
return True
if __name__ == '__main__':
if problem == "3":
from klampt import WorldModel
from klampt import vis
world = WorldModel()
world.readFile("../../data/tx90cuptable.xml")
plugin = RigidObjectCSpacePlugin(world, world.rigidObject(0))
vis.setWindowTitle("Rigid object planning")
vis.run(plugin)
exit()
space = None
start = None
goal = None
if problem == "1":
space = CircleObstacleCSpace()
space.addObstacle(Circle(0.5, 0.5, 0.36))
start = (0.06, 0.5)
goal = (0.94, 0.5)
elif problem == "2":
space = RigidBarCSpace()
space.addObstacle(Circle(0.5, 0.5, 0.4))
start = (0.1, 0.1, 0.0)
goal = (0.9, 0.9, 6.20)
program = CSpaceObstacleProgram(space, start, goal)
GLPluginInterface.__init__(self)
self.world = world
self.q = world.robot(0).getConfig()
def display(self):
self.world.drawGL()
def motionfunc(self, x, y, dx, dy):
if 'shift' in self.modifiers():
self.q[2] = float(y) / 400
self.q[3] = float(x) / 400
self.world.robot(0).setConfig(self.q)
return True
return False
def idle(self):
return True
if __name__ == "__main__":
print """mousetest.py: A simple program where the mouse motion, when
shift-clicking, gets translated into joint values for an animated robot."""
world = WorldModel()
res = world.readFile("../../data/tx90blocks.xml")
if not res:
raise RuntimeError("Unable to load world")
#set a custom initial configuration of the world
vis.setWindowTitle("mousetest.py")
vis.run(GLTest(world))
from klampt import *
from klampt import vis
from klampt.vis.glrobotprogram import GLWorldPlugin
class MyPlugin(GLWorldPlugin):
def __init__(self, world):
GLWorldPlugin.__init__(self, world)
def mousefunc(self, button, state, x, y):
#Put your mouse handler here
#the current example prints out the list of objects clicked whenever
#you right click
print "mouse", button, state, x, y
if button == 2:
if state == 0:
print[o.getName() for o in self.click_world(x, y)]
return
GLWorldPlugin.mousefunc(self, button, state, x, y)
def motionfunc(self, x, y, dx, dy):
return GLWorldPlugin.motionfunc(self, x, y, dx, dy)
world = WorldModel()
if not world.readFile("../../../Klampt-examples/data/athlete_plane.xml"):
raise RuntimeError("Couldn't load world")
vis.run(MyPlugin(world))
def main():
print(
"================================================================================"
)
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print(
"USAGE: klampt_sim [world_file] [trajectory (.traj/.path) or controller (.py)]"
)
print()
print(
" Try: klampt_sim athlete_plane.xml motions/athlete_flex_opt.path "
)
print(" [run from Klampt-examples/data/]")
print(
"================================================================================"
)
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
from klampt.control.blocks import trajectory_tracking
from klampt.model.trajectory import RobotTrajectory
from klampt.io import loader
traj = loader.load('Trajectory', c)
rtraj = RobotTrajectory(world.robot(i), traj.times,
traj.milestones)
#it's a path file, try to load it
controller = trajectory_tracking.TrajectoryPositionController(
rtraj)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)
self.sim = sim
def display(self):
self.sim.updateWorld()
self.world.drawGL()
def idle(self):
rfs = sim.controller(0).sensor("RF_ForceSensor")
print("Sensor values:", rfs.getMeasurements())
sim.simulate(self.dt)
return
if __name__ == "__main__":
print("================================================================")
print(
"gl_vis.py: This example demonstrates how to use the GL visualization interface"
)
print(" to tie directly into the GUI.")
print()
print(" The demo simulates a world and reads a force sensor")
print("================================================================")
world = klampt.WorldModel()
res = world.readFile("../../data/hubo_plane.xml")
if not res:
raise RuntimeError("Unable to load world")
sim = klampt.Simulator(world)
print("STARTING vis.run()")
vis.run(GLTest(world, sim))
print("END OF vis.run()")
def stability_vis(): program = GLStabilityPlugin() vis.run(program)
