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 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 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 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 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 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 show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5))
else:
print("Nothing reachable?")
Tee = robot.link(ee_link).getTransform()
gpen.setCurrentTransform(*Tee)
box = GeometricPrimitive()
box.setAABB(lower_corner,upper_corner)
gbox = Geometry3D(box)
#show this if you want to debug the size of the grid domain
#vis.add("box",gbox,color=(1,1,1,0.2))
vis.add("pen tip",se3.apply(Tee,ee_local_pos))
vis.loop()
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()
# pick up object
Tobject_grasp = se3.mul(
se3.inv(cur_robot.link(9).getTransform()),
cur_obj.getTransform())
was_grasping = True
if during_transfer:
cur_obj.setTransform(
*se3.mul(cur_robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration() - 0.8:
swab_init_height = swab.getTransform()[1][2]
swab_height = copy.deepcopy(swab_init_height)
if t > solved_trajectory.duration() + 1:
executing_plan = False
solved_trajectory = None
vis.add('gripper end', cur_robot.link(9).getTransform())
next_robot_to_move += 1
# let swab drop into trash can
if solved_trajectory is not None and next_robot_to_move == 0:
if solved_trajectory.duration(
) - 0.8 < t < solved_trajectory.duration() + 1:
if swab_height > 0:
swab_height = swab_init_height - 0.5 * 1 * swab_time**2
swab_time = swab_time + sim_dt
Rs, ts = swab.getTransform()
ts[2] = swab_height
swab.setTransform(Rs, ts)
vis.loop(callback=loop_callback)
def generateImages(self, max_pics=100, data_distribution=None):
assert max_pics > 0
if data_distribution is None:
data_distribution = DataUtils.LIMITED_DISTRIBUTION
assert len(data_distribution) == 13
self._createDatasetDirectory()
for i in range(self.world.numRigidObjects()):
self.world.rigidObject(i).appearance().setSilhouette(0)
metadata_f = open(self._metaDataFN, mode='w+')
metadata_f.write('color,depth,piece\n')
DEPTH_SCALE = 8000
def loop_through_sensors(world=self.world,
sensor=self.sensor,
max_pics=max_pics):
for counter in tqdm(range(max_pics)):
if counter > 0:
self.chessEngine.randomizePieces(32)
# Arrange pieces and model world
self.chessEngine.arrangePieces()
self._randomlyRotateCamera(20, 40, 0.6)
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
rgb, depth = sensing.camera_to_images(self.sensor)
# Project RGB and depth images to rectify them
local_corner_coords = np.float32([
sensing.camera_project(self.sensor, self.robot, pt)[:2]
for pt in self._boardWorldCorners
])
H = cv2.getPerspectiveTransform(local_corner_coords,
self._rectifiedPictureCorners)
color_rectified = cv2.warpPerspective(
rgb, H, (DataUtils.IMAGE_SIZE, DataUtils.IMAGE_SIZE))
depth_rectified = cv2.warpPerspective(
depth, H, (DataUtils.IMAGE_SIZE, DataUtils.IMAGE_SIZE))
depth_rectified = np.uint8(
(depth_rectified - depth_rectified.min()) /
(depth_rectified.max() - depth_rectified.min()))
pieces_arrangement = self.chessEngine.getPieceArrangement()
images, classes = DataUtils.split_image_by_classes(
color_rectified, depth_rectified, data_distribution,
pieces_arrangement)
rectified_fname = self._rectifiedFNFormat % (counter)
Image.fromarray(color_rectified).save(rectified_fname)
for idx in range(sum(data_distribution)):
color_fname = self._colorFNFormat % (counter, idx)
depth_fname = self._depthFNFormat % (counter, idx)
Image.fromarray(images[idx, :, :, :3]).save(color_fname)
Image.fromarray(images[idx, :, :, 3]).save(depth_fname)
metadata_f.write(
f'{color_fname},{depth_fname},{classes[idx]}\n')
vis.show(False)
vis.loop(callback=loop_through_sensors)
metadata_f.close()
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
robotiq_140.maximum_span = 0.140 - 0.01
robotiq_140.minimum_span = 0
robotiq_140.open_config = [0]*6
robotiq_140.closed_config = [0.7,0.7,0.7,0.7,0.7,0.7]
robotiq_85_kinova_gen3 = GripperInfo.mounted(robotiq_85,os.path.join(data_dir,"robots/kinova_with_robotiq_85.urdf"),"gripper:Link_0","robotiq_85-kinova_gen3")
robotiq_140_trina_left = GripperInfo.mounted(robotiq_140,os.path.join(data_dir,"robots/TRINA.urdf"),"left_gripper:base_link","robotiq_140-trina-left")
robotiq_140_trina_right = GripperInfo.mounted(robotiq_140,os.path.join(data_dir,"robots/TRINA.urdf"),"right_gripper:base_link","robotiq_140-trina-right")
if __name__ == '__main__':
from klampt import vis
import sys
if len(sys.argv) == 1:
grippers = [i for i in GripperInfo.all_grippers]
print("ALL GRIPPERS",grippers)
else:
grippers = sys.argv[1:]
for i in grippers:
g = GripperInfo.get(i)
print("SHOWING GRIPPER",i)
g.add_to_vis()
vis.setWindowTitle(i)
def setup():
vis.show()
def cleanup():
vis.show(False)
vis.clear()
vis.loop(setup=setup,cleanup=cleanup)
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()
(point_cloud_count, ))
g.saveFile("temp%04d.pcd" % (point_cloud_count, ))
#this needs to be done to refresh the appearance
a = world.rigidObject(0).appearance()
a.refresh()
if MULTITHREADED:
vis.show()
while vis.shown():
vis.lock()
updatePointCloud()
vis.unlock()
#TODO: do anything else?
#runs at most 10Hz
time.sleep(0.1)
else:
data = {'next_update_time': time.time()}
def callback():
if time.time() >= data['next_update_time']:
#run at approximately 10Hz
data['next_update_time'] += 0.1
updatePointCloud()
vis.loop(setup=vis.show, callback=callback)
vis.kill()
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)
vis.setWindowTitle("Simulating path using trajectory.execute_trajectory")
if vis.multithreaded():
#for some tricky Qt reason, need to sleep before showing a window again
#Perhaps the event loop must complete some extra cycles?
time.sleep(0.01)
vis.show()
t0 = time.time()
while vis.shown():
#print "Time",sim.getTime()
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.0))
t0 = t1
else:
data = {'next_sim_time':time.time()}
def callback():
if time.time() >= data['next_sim_time']:
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
data['next_sim_time'] += 0.01
vis.loop(callback=callback,setup=vis.show)
print("Ending vis.")
vis.kill()
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()
def run_simulation(world):
value = resource.get('start.config',default=world.robot(0).getConfig(),world=world)
world.robot(0).setConfig(value)
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 when you are ready for testing 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 = CircleController(robot_controller,planning_robot,ee_link,
ee_local_pos,ee_local_axis,
radius=0.05,period=5.0)
controller_vis = RobotInterfacetoVis(robot_controller)
trace = Trajectory()
#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,trace=trace,
storage=[sim_world,planning_world,sim,controller_vis]):
start_clock = time.time()
dt = 1.0/robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
drawing_controller.advance(dt)
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee=sim_robot.link(ee_link).getTransform()
wp = se3.apply(Tee,ee_local_pos)
if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01:
trace.milestones.append(wp)
trace.times.append(0)
if len(trace.milestones)==2:
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
if len(trace.milestones) > 200:
trace.milestones = trace.milestones[-100:]
trace.times = trace.times[-100:]
if len(trace.milestones)>2:
if _backend=='IPython':
vis.remove("trace")
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
else:
vis.dirty("trace")
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0,dt-duration))
vis.loop(callback=callback)
def gen_grasp_images(max_variations=10):
"""Generates grasp training images for Problem 1b"""
global camera_viewpoints
if len(camera_viewpoints) == 0:
#This code pops up the viewpoint editor
edited = False
for base in base_files:
camera_viewpoints[base] = []
camera_fn_template = os.path.join(
"resources",
os.path.splitext(base)[0] + '_camera_%02d.xform')
index = 0
while True:
camera_fn = camera_fn_template % (index, )
if not os.path.exists(camera_fn):
break
xform = loader.load('RigidTransform', camera_fn)
camera_viewpoints[base].append(xform)
index += 1
if len(camera_viewpoints[base]) > 0:
#TODO: if you want to edit the camera transforms, comment this line out and replace it with "pass"
continue
#pass
edited = True
for i, xform in enumerate(camera_viewpoints[base]):
print("Camera transform", base, i)
sensor_xform = edit_camera_xform(
base,
xform,
title="Camera transform {} {}".format(base, i))
camera_viewpoints[base][i] = sensor_xform
camera_fn = camera_fn_template % (i, )
loader.save(sensor_xform, 'RigidTransform', camera_fn)
while True:
if len(camera_viewpoints[base]) > 0:
print("Do you want to add another? (y/n)")
choice = input()
else:
choice = 'y'
if choice.strip() == 'y':
sensor_xform = edit_camera_xform(
base, title="New camera transform {}".format(base))
camera_viewpoints[base].append(sensor_xform)
camera_fn = camera_fn_template % (
len(camera_viewpoints[base]) - 1, )
loader.save(sensor_xform, 'RigidTransform', camera_fn)
else:
break
if edited:
print("Quitting; run me again to render images")
return
#Here's where the main loop begins
try:
os.mkdir('image_dataset')
except Exception:
pass
try:
os.remove("image_dataset/metadata.csv")
except Exception:
pass
with open("image_dataset/metadata.csv", 'w') as f:
f.write(
"world,view,view_transform,color_fn,depth_fn,grasp_fn,variation\n")
total_image_count = 0
for fn in glob.glob("generated_worlds/world_*.xml"):
ofs = fn.find('.xml')
index = fn[ofs - 4:ofs]
world = WorldModel()
world.readFile(fn)
wtype = autodetect_world_type(world)
if wtype == 'unknown':
print("WARNING: DONT KNOW WORLD TYPE OF", fn)
world_camera_viewpoints = camera_viewpoints[wtype + '.xml']
assert len(world_camera_viewpoints) > 0
#TODO: change appearances
for i in range(world.numRigidObjects()):
world.rigidObject(i).appearance().setSilhouette(0)
for i in range(world.numTerrains()):
world.terrain(i).appearance().setSilhouette(0)
world.readFile('camera.rob')
robot = world.robot(0)
sensor = robot.sensor(0)
vis.add("world", world)
counters = [0, 0, total_image_count]
def loop_through_sensors(
world=world,
sensor=sensor,
world_camera_viewpoints=world_camera_viewpoints,
index=index,
counters=counters):
viewno = counters[0]
variation = counters[1]
total_count = counters[2]
print("Generating data for sensor view", viewno, "variation",
variation)
sensor_xform = world_camera_viewpoints[viewno]
#TODO: problem 1.B: perturb camera and change colors
# Generate random axis, random angle, rotate about angle for orientation perturbation
# Generate random axis, random dist, translate distance over axis for position perturbation
rand_axis = np.random.rand(3)
rand_axis = vectorops.unit(rand_axis)
multiplier = np.random.choice([True, False], 1)
rand_angle = (np.random.rand(1)[0] * 10 +
10) * (-1 if multiplier else 1)
# print(rand_angle)
R = so3.from_axis_angle((rand_axis, np.radians(rand_angle)))
rand_axis = vectorops.unit(np.random.random(3))
rand_dist = np.random.random(1)[0] * .10 + .10
# print(rand_dist)
t = vectorops.mul(rand_axis, rand_dist)
sensor_xform = se3.mul((R, t), sensor_xform)
sensing.set_sensor_xform(sensor, sensor_xform)
rgb_filename = "image_dataset/color_%04d_var%04d.png" % (
total_count, variation)
depth_filename = "image_dataset/depth_%04d_var%04d.png" % (
total_count, variation)
grasp_filename = "image_dataset/grasp_%04d_var%04d.png" % (
total_count, variation)
#Generate sensor images
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
print("Done with kinematic simulate")
rgb, depth = sensing.camera_to_images(sensor)
print("Saving to", rgb_filename, depth_filename)
Image.fromarray(rgb).save(rgb_filename)
depth_scale = 8000
depth_quantized = (depth * depth_scale).astype(np.uint32)
Image.fromarray(depth_quantized).save(depth_filename)
with open("image_dataset/metadata.csv", 'a') as f:
line = "{},{},{},{},{},{},{}\n".format(
index, viewno, loader.write(sensor_xform,
'RigidTransform'),
os.path.basename(rgb_filename),
os.path.basename(depth_filename),
os.path.basename(grasp_filename), variation)
f.write(line)
#calculate grasp map and save it
grasp_map = make_grasp_map(world, total_count)
grasp_map_quantized = (grasp_map * 255).astype(np.uint8)
channels = ['score', 'opening', 'axis_heading', 'axis_elevation']
for i, c in enumerate(channels):
base, ext = os.path.splitext(grasp_filename)
fn = base + '_' + c + ext
Image.fromarray(grasp_map_quantized[:, :, i]).save(fn)
#loop through variations and views
counters[1] += 1
if counters[1] >= max_variations:
counters[1] = 0
counters[0] += 1
if counters[0] >= len(world_camera_viewpoints):
vis.show(False)
counters[2] += 1
print("Running render loop")
vis.loop(callback=loop_through_sensors)
total_image_count = counters[2]
