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()
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()
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
plate.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[robot2.link(9).index])
vis.addAction(planTriggered, "Plan grasp", 'p')
executing_plan = False
execute_start_time = None
# execute the planned path
def executePlan():
global solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time
if solved_trajectory is None:
return
executing_plan = True
if PHYSICS_SIMULATION:
execute_start_time = 0
solved_trajectory = path_to_trajectory(solved_trajectory,
timing='robot',
smoothing=None)
print("Tests threading after dialog")
world = WorldModel()
world.readFile('../../data/objects/block.obj')
resource.edit("object transform",
world.rigidObject(0).getTransform(),
world=world)
def launchdialog():
resource.edit("object transform launched from window",
world.rigidObject(0).getTransform(),
world=world)
def launchwindow():
origwindow = vis.getWindow()
vis.createWindow("Pop up window")
vis.add("world2", world)
vis.show()
vis.setWindow(origwindow)
print("Now running show() (only works on multithreaded systems, not mac)")
vis.add("world", world)
vis.addAction(launchdialog, "Launch a dialog", "d")
vis.addAction(launchwindow, "Launch a window", "w")
vis.show()
while vis.shown():
time.sleep(0.1)
vis.kill()
dR0 = [0.0] * 9
#TODO: for some reason the interpolation doesn't work very well...
#vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1))
#for m in circle_smooth_traj.milestones:
# T = m[:12]
# vT = m[12:]
# print("Orientation velocity",vT[:9])
#print("SMOOTH")
#for R in circle_smooth_traj.discretize_se3(0.1).milestones:
# print(so3.apply(R,[0,1,0]))
vis.add("xform", se3.identity())
vis.animate("xform", circle_smooth_traj)
#vis.add("Camera traj",circle_traj.discretize(0.25))
vis.addAction(lambda: vis.followCamera(None), "stop folllowing")
vis.addAction(lambda: vis.followCamera(cam), "robot camera")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), True, False, True),
"link, translate")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), False, True, True),
"link, rotate")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), True, True, True),
"link, full pose")
vis.addAction(
lambda: vis.followCamera(
if cmap is None:
cmap = 'viridis'
a_app = colorize.colorize(w.rigidObject(0),value,cmap,feature,lighting=lighting)
#a_app.drawGL(a)
#if not value.startswith('n'):
colorize.colorize(a_pc,value,cmap,lighting=lighting)
vis.remove("A")
vis.add("A",a_app)
vis.remove("B")
vis.add("B",a_pc)
vis.dirty("B")
#print("PC First color %08x"%(int(a_pc.getPointCloud().getProperty(0,'rgba')),))
convert('z',None,'faces')
vis.addAction(lambda:convert(None,'viridis'),"viridis colormap")
vis.addAction(lambda:convert(None,'plasma'),"plasma colormap")
vis.addAction(lambda:convert(None,'random'),"random colormap")
vis.addAction(lambda:convert('position',None),"position value")
vis.addAction(lambda:convert('x',None),"x value")
vis.addAction(lambda:convert('y',None),"y value")
vis.addAction(lambda:convert('z',None),"z value")
vis.addAction(lambda:convert('normal',None),"normal value")
vis.addAction(lambda:convert('nx',None),"nx value")
vis.addAction(lambda:convert('ny',None),"ny value")
vis.addAction(lambda:convert('nz',None),"nz value")
vis.addAction(lambda:convert(None,None,'faces'),"colorize faces")
vis.addAction(lambda:convert(None,None,'vertices'),"colorize vertices")
vis.addAction(lambda:convert(None,None,None,[0,0,-1]),"lighting on")
vis.addAction(lambda:convert(None,None,None,'none'),"lighting off")
print(
"Tests multiple simultaneous windows. This requires multithreaded visualization,"
)
print("which doesn't work on Mac")
world1 = WorldModel()
world1.readFile('../../data/objects/block.obj')
id1 = vis.createWindow('First')
def firsthello():
print("hello from First")
vis.addAction(firsthello, "First's action", "p")
vis.add('world1', world1)
vis.show()
world2 = WorldModel()
world2.readFile('../../data/robots/athlete.rob')
id2 = vis.createWindow('Second')
def secondhello():
print("hello from Second")
vis.addAction(secondhello, "Second's action", "q")
vis.add('world2', world2)
def convert(geom, type, label):
global a, b, atypes, btypes, Ta, Tb
if label == 'A':
vis.add(label, atypes[type])
vis.setColor(label, 1, 0, 0, 0.5)
a = atypes[type]
a.setCurrentTransform(*Ta)
else:
vis.add(label, btypes[type])
vis.setColor(label, 0, 1, 0, 0.5)
b = btypes[type]
b.setCurrentTransform(*Tb)
vis.addAction(lambda: convert(a, 'GeometricPrimitive', 'A'),
"A to GeometricPrimitive")
vis.addAction(lambda: convert(a, 'TriangleMesh', 'A'), "A to TriangleMesh")
vis.addAction(lambda: convert(a, 'PointCloud', 'A'), "A to PointCloud")
vis.addAction(lambda: convert(a, 'VolumeGrid', 'A'), "A to VolumeGrid")
vis.addAction(lambda: convert(a, 'ConvexHull', 'A'), "A to ConvexHull")
vis.addAction(lambda: convert(b, 'GeometricPrimitive', 'B'),
"B to GeometricPrimitive")
vis.addAction(lambda: convert(b, 'TriangleMesh', 'B'), "B to TriangleMesh")
vis.addAction(lambda: convert(b, 'PointCloud', 'B'), "B to PointCloud")
vis.addAction(lambda: convert(b, 'VolumeGrid', 'B'), "B to VolumeGrid")
vis.addAction(lambda: convert(b, 'ConvexHull', 'B'), "B to ConvexHull")
global mode
global drawExtra
mode = 'collision'
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
grasps)
else:
res = pick.plan_pick_multistep(world, robot, obj, target_gripper,
grasps)
if res is None:
print("Unable to plan pick")
else:
(transit, approach, lift) = res
traj = transit
traj = traj.concat(approach, relative=True, jumpPolicy='jump')
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[9])
vis.animate(vis.getItemName(robot), traj)
robot.setConfig(qstart)
vis.addAction(planTriggered, "Plan grasp", 'p')
if PROBLEM == '2a':
def shiftGrasp(amt):
global grasp, grasps, grasp_index
grasp_index += amt
if grasp_index >= len(grasps):
grasp_index = 0
elif grasp_index < 0:
grasp_index = len(grasps) - 1
print("Grasp", grasp_index)
grasp = grasps[grasp_index]
Tgripper = grasp.ik_constraint.closestMatch(*se3.identity())
source_gripper_model.setConfig(
orig_grasps[grasp_index].set_finger_config(
cur_grasp += amt
if cur_grasp >= len(all_grasps):
cur_grasp = -1
elif cur_grasp < -1:
cur_grasp = len(all_grasps)-1
if cur_grasp==-1:
for i,grasp in enumerate(all_grasps):
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(i))
shown_grasps.append((i,grasp))
print("Showing",len(shown_grasps),"grasps")
else:
grasp = all_grasps[cur_grasp]
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(cur_grasp))
Tbase = grasp.ik_constraint.closestMatch(*se3.identity())
g.add_to_vis(robot,animate=False,base_xform=Tbase)
robot.setConfig(grasp.set_finger_config(robot.getConfig()))
shown_grasps.append((cur_grasp,grasp))
if grasp.score is not None:
vis.addText("score","Score %.3f"%(grasp.score,),position=(10,10))
else:
vis.addText("score","",position=(10,10))
vis.addAction(lambda: shift_object(1),"Next object",'.')
vis.addAction(lambda: shift_object(-1),"Prev object",',')
vis.addAction(lambda: shift_grasp(1),"Next grasp",'=')
vis.addAction(lambda: shift_grasp(-1),"Prev grasp",'-')
vis.addAction(lambda: shift_grasp(None),"All grasps",'0')
vis.add("gripper",w.robot(0))
vis.run()
lastPlan = None
def runPlanner(runfunc, name):
global lastPlan
res = runfunc()
if res:
if lastPlan:
vis.remove(lastPlan)
vis.add(name, res)
vis.setColor(name, 1, 0, 0)
vis.animate(("world", robot.getName()), res)
lastPlan = name
vis.addAction(lambda: runPlanner(doRRTStar, "RRT*"), "Run RRT*")
vis.addAction(lambda: runPlanner(doHybridRRTStar, "Hybrid RRT*"),
"Run Hybrid RRT*")
vis.addAction(lambda: runPlanner(doLazyRRGStar, "Lazy-RRG*"), "Run Lazy-RRG*")
vis.addAction(lambda: runPlanner(doHybridLazyRRGStar, "Hybrid LazyRRG*"),
"Run Hybrid LazyRRG*")
vis.addAction(lambda: runPlanner(doRestartSBL, "Restart SBL"),
"Run Restart SBL")
vis.addAction(lambda: runPlanner(doHybridSBL, "Hybrid SBL"), "Run Hybrid SBL")
vis.addAction(lambda: runPlanner(doRestartShortcutSBL, "Restart-Shortcut SBL"),
"Run Restart-Shortcut SBL")
vis.addAction(lambda: runPlanner(doHybridShortcutSBL, "Hybrid-Shortcut SBL"),
"Run Hybrid-Shortcut SBL*")
vis.addAction(doAllTests, "Run all tests")
print("Beginning visualization.")