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 init_gamepad(self):
"""Initializes the gamepad"""
self.lastGamepadState = {}
self.originalConfig = None
self.tuckConfig = resource.get("tuck.config",directory="UI/Resources",doedit=False)
self.neutralConfig = resource.get("neutral.config",directory="UI/Resources",doedit=False)
# Connect to controller
try:
self.j = logitech.Joystick(0)
except:
print "Joystick not found"
print "Note: Pygame reports there are " + str(logitech.numJoys()) + " joysticks"
return False
return True
def make_thumbnails(folder,outputfolder):
for fn in glob.glob(folder+"/*"):
filename = os.path.basename(fn)
mkdir_p(outputfolder)
print(os.path.splitext(filename)[1])
if os.path.splitext(filename)[1] in ['.xml','.rob','.obj','.env']:
#world file
print(fn)
world = WorldModel()
world.readFile(fn)
im = resource.thumbnail(world,(128,96))
if im != None:
im.save(os.path.join(outputfolder,filename+".thumb.png"))
else:
print("Could not save thumbnail.")
exit(0)
vis.lock()
del world
vis.unlock()
elif os.path.splitext(filename)[1] in resource.knownTypes():
res = resource.get(filename,doedit=False,directory=folder)
im = resource.thumbnail(res,(128,96))
if im != None:
im.save(os.path.join(outputfolder,filename+".thumb.png"))
else:
print("Could not save thumbnail.")
exit(0)
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None):
"""Launches a very simple program that spawns a box with dimensions specified in boxes_db.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform",
default=se3.identity(), world=world, doedit=False)
for box_dims, poses in box_db.db.items():
if world.numRigidObjects() > 0:
world.remove(world.rigidObject(0))
obj = make_box(world,
box_dims[0],
box_dims[1],
box_dims[2])
poses_filtered = []
R,t = obj.getTransform()
obj.setTransform(R, [0, 0, box_dims[2]/2.])
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p_T_h = np.array(se3.homogeneous(xform))
p_T_h[2][3] += 0.02
for pose in poses:
w_T_p = w_T_o.dot(pose)
w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h))
if CollisionTestPose(world, robot, obj, w_T_h_des_se3):
pose_pp = se3.from_homogeneous(pose)
t_pp = pose_pp[1]
q_pp = so3.quaternion(pose_pp[0])
q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]]
print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims
elif w_T_p[2][3] <= 0.:
print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table"
else:
poses_filtered.append(pose)
print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses"
# embed()
# create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
# emulator takes the robot index (0), start link index (6), and start driver index (6)
program = FilteredMVBBTesterVisualizer(box_dims,
poses_filtered,
world,
p_T_h,
module,
box_db,
links_to_check)
vis.setPlugin(None)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.1)
return
def launch_xform_viewer(robotname):
"""Launches a very simple program that spawns an object from one of the
databases.
It launches a visualization of the mvbb decomposition of the object, and corresponding generated poses.
It then spawns a hand and tries all different poses to check for collision
"""
world = WorldModel()
robot = make_moving_base_robot(robotname, world)
set_moving_base_xform(robot, se3.identity()[0], se3.identity()[1])
xform = resource.get("default_initial_%s.xform" % robotname,
description="Initial hand transform",
default=se3.identity(),
world=world,
doedit=False)
print "Transform:", xform
program = XFormVisualizer(world, xform)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.01)
vis.kill()
return
def gen_grasp_worlds(N=10):
if len(base_boxes) == 0:
#edit base boxes
for basefn in base_files:
world = WorldModel()
world.readFile(basefn)
base_name = os.path.splitext(os.path.basename(basefn))[0]
box = GeometricPrimitive()
box.setAABB([0, 0, 0], [1, 1, 1])
base_boxes.append(
resource.get(base_name + '.geom',
'GeometricPrimitive',
default=box,
world=world,
doedit='auto'))
output_file_pattern = "generated_worlds/world_%04d.xml"
#TODO: play around with me, problem 1a
num_objects = [3, 3, 4, 5, 6, 6]
for i in range(N):
nobj = random.choice(num_objects)
world = WorldModel()
base_world = random.choice(range(len(base_files)))
world.readFile(base_files[base_world])
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn, world)
obj_fns.append(fn)
bbox = base_boxes[base_world]
bmin, bmax = [v for v in bbox.properties[0:3]
], [v for v in bbox.properties[3:6]]
#TODO: Problem 1: arrange objects within box bmin,bmax
arrange_objects(world, obj_fns, bmin, bmax)
save_world(world, output_file_pattern % (i, ))
def vis_interaction_test(world):
"""Run some tests of visualization module interacting with the resource module"""
print("Showing robot in modal dialog box")
vis.add("robot", world.robot(0))
vis.add("ee", world.robot(0).link(11).getTransform())
vis.dialog()
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
import time
if MULTITHREADED:
print(
"Showing threaded visualization (this will fail on GLUT or Mac OS)"
)
vis.show()
for i in range(3):
vis.lock()
q = world.robot(0).getConfig()
q[9] = 3.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
vis.lock()
q = world.robot(0).getConfig()
q[9] = -1.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
print("Hiding visualization window")
vis.show(False)
else:
print("Showing single-threaded visualization for 5s")
vis.spin(5.0)
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
def config_edit_template(world):
"""Shows how to edit Config, Configs, and Trajectory resources"""
config1 = resource.get("resourcetest1.config",
description="First config, always edited",
doedit=True,
editor='visual',
world=world)
print("Config 1:", config1)
config2 = resource.get("resourcetest1.config",
description="Trying this again...",
editor='visual',
world=world)
print("Config 2:", config2)
config3 = resource.get("resourcetest2.config",
description="Another configuration",
editor='visual',
world=world)
print("Config 3:", config3)
if config3 != None:
config3hi = config3[:]
config3hi[3] += 1.0
resource.set("resourcetest3_high.config", config3hi)
world.robot(0).setConfig(config3hi)
configs = []
if config1 != None: configs.append(config1)
if config2 != None: configs.append(config2)
if config3 != None: configs.append(config3)
print("Configs resource:", configs)
configs = resource.get("resourcetest.configs",
default=configs,
description="Editing config sequence",
doedit=True,
editor='visual',
world=world)
if configs is None:
print("To edit the trajectory, press OK next time")
else:
traj = RobotTrajectory(world.robot(0), list(range(len(configs))),
configs)
traj = resource.edit(name="Timed trajectory",
value=traj,
description="Editing trajectory",
world=world)
def load_hand_configs():
global hand_types, hand_subrobots, hand_configs
for link, hand in hand_types.iteritems():
if hand_configs[link] is None:
continue
resource.setDirectory(hands[link].resource_directory)
qhand = resource.get(hand_configs[link] + ".config",
type="Config",
doedit=False)
#take out the 6 floating DOFs
hand_subrobots[link].setConfig(qhand[6:])
def thumbnail_template(world):
"""Will save thumbnails of all previously created resources"""
import os, glob
robotname = world.robot(0).getName()
for fn in glob.glob('resources/' + robotname + '/*'):
filename = os.path.basename(fn)
print(os.path.splitext(filename)[1])
if os.path.splitext(filename)[1] in resource.knownTypes():
print(fn)
res = resource.get(filename)
im = resource.thumbnail(res, (128, 96), world=world)
if im != None:
im.save('resources/' + robotname + "/" + filename +
".thumb.png")
def init(self, world):
assert self.j == None, "Init may only be called once"
self.world = world
self.originalConfig = None
self.tuckConfig = resource.get("tuck.config",
directory="UI/Resources",
doedit=False)
self.neutralConfig = resource.get("neutral.config",
directory="UI/Resources",
doedit=False)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
robot = world.robot(0)
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
# Connect to controller
try:
self.j = logitech.Joystick(0)
return True
except:
print "Joystick not found"
print "Note: Pygame reports there are " + str(
logitech.numJoys()) + " joysticks"
return False
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 load_config(self,name):
object = self.world.rigidObject(0)
self.config_name = name
print "Loading from",name+".config,",name+".xform","and",name+".array"
qrob = resource.get(name+".config",type="Config",doedit=False)
Tobj = resource.get(name+".xform",type="RigidTransform",doedit=False)
self.contact_units = resource.get(name+"_units.array",type="IntArray",default=[],doedit=False)
if len(self.contact_units) == 0:
print "COULDNT LOAD "+name+"_units.array, trying "+name+".array"
contact_links = resource.get(name+".array",type="IntArray",default=[],doedit=False)
if len(contact_links) > 0:
robot = self.world.robot(0)
contact_links = [robot.link(l).getName() for l in contact_links]
self.contact_units = []
for i,(link,unit) in enumerate(self.hand.all_units):
if link in contact_links:
self.contact_units.append(i)
print "UNITS",self.contact_units
object.setTransform(*Tobj)
qobj = vis.getItemConfig("rigidObject")
vis.setItemConfig("robot",qrob)
vis.setItemConfig("rigidObject",qobj)
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
coordinates.updateFromWorld()
def get_env_model(env_id, renderer, volume=True, resolution=1):
"""
Load the scene object with given [env_id] as Klampt VolumeGrid, mesh and add it to renderer.
"""
path = os.path.join(YCBV_PATH, f"models_eval/obj_{env_id:06d}.ply")
env_model = resource.get(path)
env_model.transform(obj_roff[env_id - 1].T.reshape(9).tolist(), obj_toff[env_id - 1].reshape(3).tolist())
if volume:
env_model = env_model.convert('VolumeGrid', resolution)
mesh = trimesh.load(path)
T = np.eye(4)
T[:3, :3] = obj_roff[env_id - 1]
T[:3, 3] = obj_toff[env_id - 1]
mesh = mesh.apply_transform(T)
path = os.path.join(YCBV_PATH, f"models/obj_{env_id:06d}.ply")
renderer.add_object(-env_id, path, offset=[obj_roff[env_id - 1], obj_toff[env_id - 1]])
return env_model, mesh
def get_tgt_model(obj_id, renderer):
"""
Load the target object with given [obj_id] as Klampt PointCloud, numpy array, mesh and add it to renderer.
"""
path_ply = os.path.join(YCBV_PATH, f"models_eval/obj_{obj_id:06d}.ply")
cloud = resource.get(path_ply).convert('PointCloud')
cloud.transform(obj_roff[obj_id - 1].T.reshape(9).tolist(), obj_toff[obj_id - 1].reshape(3).tolist())
ply = inout.load_ply(path_ply)
ply['pts'] = (obj_roff[obj_id - 1] @ ply['pts'].T + obj_toff[obj_id - 1].reshape(3, 1)).T
mesh = trimesh.load(path_ply)
T = np.eye(4)
T[:3, :3] = obj_roff[obj_id - 1]
T[:3, 3] = obj_toff[obj_id - 1]
mesh = mesh.apply_transform(T)
renderer.add_object(obj_id, os.path.join(YCBV_PATH, f"models/obj_{obj_id:06d}.ply"),
offset=[obj_roff[obj_id - 1], obj_toff[obj_id - 1]])
return cloud, ply, mesh
def launch_shelf(robotname, objects):
"""Launches the task 2 program that asks the robot to retrieve some set of objects
packed within a shelf.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
box = make_box(world, *box_dims)
shelf = make_shelf(world, *shelf_dims)
shelf.geometry().translate((0, shelf_offset, shelf_height))
rigid_objects = []
for objectset, objectname in objects:
object = make_object(objectset, objectname, world)
#TODO: pack in the shelf using x-y translations and z rotations
object.setTransform(
*se3.mul((so3.identity(), [0, shelf_offset, shelf_height +
0.01]), object.getTransform()))
rigid_objects.append(object)
xy_jiggle(world, rigid_objects, [shelf],
[-0.5 * shelf_dims[0], -0.5 * shelf_dims[1] + shelf_offset],
[0.5 * shelf_dims[0], 0.5 * shelf_dims[1] + shelf_offset], 100)
doedit = True
xform = resource.get("shelf/default_initial_%s.xform" % (robotname, ),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world)
if not xform:
print "User quit the program"
return
set_moving_base_xform(robot, xform[0], xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
#controlfunc is now attached to control the robot
import shelf_controller
sim.setController(robot, shelf_controller.make(sim, hand, program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
while vis.shown():
time.sleep(0.1)
return
#this code manually updates the vis
vis.add("world", world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
return
def launch_balls(robotname, num_balls=10):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
maxlayer = 16
numlayers = int(math.ceil(float(num_balls) / float(maxlayer)))
for layer in xrange(numlayers):
if layer + 1 == numlayers:
ballsperlayer = num_balls % maxlayer
else:
ballsperlayer = maxlayer
w = int(math.ceil(math.sqrt(ballsperlayer)))
h = int(math.ceil(float(ballsperlayer) / float(w)))
for i in xrange(ballsperlayer):
bid = world.loadElement("data/objects/sphere_10cm.obj")
if bid < 0:
raise RuntimeError(
"data/objects/sphere_10cm.obj couldn't be loaded")
ball = world.rigidObject(world.numRigidObjects() - 1)
R = so3.identity()
x = i % w
y = i / w
x = (x - (w - 1) * 0.5) * box_dims[0] * 0.7 / (w - 1)
y = (y - (h - 1) * 0.5) * box_dims[1] * 0.7 / (h - 1)
t = [x, y, 0.08 + layer * 0.11]
t[0] += random.uniform(-0.005, 0.005)
t[1] += random.uniform(-0.005, 0.005)
ball.setTransform(R, t)
robot = make_moving_base_robot(robotname, world)
box = make_box(world, *box_dims)
box2 = make_box(world, *box_dims)
box2.geometry().translate((0.7, 0, 0))
xform = resource.get("balls/default_initial_%s.xform" % (robotname, ),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world,
doedit=True)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot, xform[0], xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
#A StateMachineController instance is now attached to control the robot
import balls_controller
sim.setController(robot, balls_controller.make(sim, hand, program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
"""
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world", world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
return
def launch_simple(robotname, object_set, objectname, use_box=False):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
If use_box is True, then the test object is placed inside a box.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
object = make_object(object_set, objectname, world)
if use_box:
box = make_box(world, *box_dims)
object.setTransform(
*se3.mul((so3.identity(), [0, 0, 0.01]), object.getTransform()))
doedit = True
xform = resource.get("%s/default_initial_%s.xform" %
(object_set, robotname),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world)
set_moving_base_xform(robot, xform[0], xform[1])
xform = resource.get("%s/initial_%s_%s.xform" %
(object_set, robotname, objectname),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world,
doedit=False)
if xform:
set_moving_base_xform(robot, xform[0], xform[1])
xform = resource.get("%s/initial_%s_%s.xform" %
(object_set, robotname, objectname),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world,
doedit=doedit)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot, xform[0], xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
#the result of simple_controller.make() is now attached to control the robot
import simple_controller
sim.setController(robot, simple_controller.make(sim, hand, program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
"""
program.simulate = True
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world", world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
return
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 launch_balls(robotname,num_balls=10):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
maxlayer = 16
numlayers = int(math.ceil(float(num_balls)/float(maxlayer)))
for layer in xrange(numlayers):
if layer + 1 == numlayers:
ballsperlayer = num_balls%maxlayer
else:
ballsperlayer = maxlayer
w = int(math.ceil(math.sqrt(ballsperlayer)))
h = int(math.ceil(float(ballsperlayer)/float(w)))
for i in xrange(ballsperlayer):
bid = world.loadElement("data/objects/sphere_10cm.obj")
if bid < 0:
raise RuntimeError("data/objects/sphere_10cm.obj couldn't be loaded")
ball = world.rigidObject(world.numRigidObjects()-1)
R = so3.identity()
x = i % w
y = i / w
x = (x - (w-1)*0.5)*box_dims[0]*0.7/(w-1)
y = (y - (h-1)*0.5)*box_dims[1]*0.7/(h-1)
t = [x,y,0.08 + layer*0.11]
t[0] += random.uniform(-0.005,0.005)
t[1] += random.uniform(-0.005,0.005)
ball.setTransform(R,t)
robot = make_moving_base_robot(robotname,world)
box = make_box(world,*box_dims)
box2 = make_box(world,*box_dims)
box2.geometry().translate((0.7,0,0))
xform = resource.get("balls/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=True)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#A StateMachineController instance is now attached to control the robot
import balls_controller
sim.setController(robot,balls_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
"""
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
from klampt import vis
from klampt.math import vectorops,so3,se3
from klampt.model.create import pile
from klampt.io import resource
from known_grippers import *
#load the world model
w = WorldModel()
if not w.readFile(robotiq_140_trina_left.klampt_model):
print("Couldn't load",robotiq_140_trina_left.klampt_model)
exit(1)
import os
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__),'../data'))
resource.setDirectory(os.path.join(data_dir,'resources/TRINA'))
qhome = resource.get('home.config')
w.robot(0).setConfig(qhome)
#create a box with objects in it
box = world_generator.make_box(w,0.5,0.5,0.1)
box.geometry().translate((1,0,0.8))
objects_in_box = []
for i in range(1):
g = world_generator.make_ycb_object('random',w)
objects_in_box.append(g)
grasp_edit.grasp_edit_ui(robotiq_140_trina_right,objects_in_box[0])
#visualize the right gripper
robotiq_140_trina_right.add_to_vis(w.robot(0))
#look at the disembodied right gripper
m2 = from_numpy(mnp, 'TriangleMesh')
print(len(m2.vertices), len(m2.indices))
print("POINT CLOUD CONVERT")
pcnp = to_numpy(pc)
print("Numpy size", pcnp.shape)
pc2 = from_numpy(pcnp, 'PointCloud')
print(pc2.numPoints(), pc2.numProperties())
print("GEOMETRY CONVERT")
gnp = to_numpy(geom)
print("Numpy geometry has transform", gnp[0])
print("TRAJECTORY CONVERT")
resource.setDirectory('.')
traj = resource.get("../../data/motions/athlete_flex_opt.path")
trajnp = to_numpy(traj)
rtraj = RobotTrajectory(w.robot(0), traj.times, traj.milestones)
rtrajnp = to_numpy(rtraj)
rtraj2 = from_numpy(rtrajnp, template=rtraj)
print("Return type", rtraj2.__class__.__name__, "should be RobotTrajectory")
geom2 = w.robot(0).link(11).geometry()
vg = geom2.convert('VolumeGrid', 0.01)
print("VOLUME GRID CONVERT")
vgnp = to_numpy(vg.getVolumeGrid())
print(vgnp[0], vgnp[1])
import matplotlib.pyplot as plt
import numpy as np
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)
print
print
#this tests the "bump" function stuff
print "***** BEGINNING BUMP FUNCTION TEST *****"
configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world)
if configs is None:
print "To test the bump function, you need to create a reference trajectory"
vis.kill()
exit(0)
print "Found trajectory with",len(configs),"configurations"
traj = trajectory.RobotTrajectory(robot,range(len(configs)),configs)
vis.setWindowTitle("Klamp't Trajectory bump test")
vis.pushPlugin(BumpKeyCapture(endeffectors,eeobjectives,traj))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
print "Ending vis."
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)
print
print
#this tests the "bump" function stuff
print "***** BEGINNING BUMP FUNCTION TEST *****"
configs = resource.get("cartesian_test" + world.robot(0).getName() +
".configs",
world=world)
print "Found trajectory with", len(configs), "configurations"
traj = trajectory.RobotTrajectory(robot, range(len(configs)), configs)
vis.setWindowTitle("Klamp't Trajectory bump test")
vis.pushPlugin(BumpKeyCapture(endeffectors, eeobjectives, traj))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
print "Ending vis."
vis.kill()
def launch_grasping(robot_name, object_set, object_name):
"""
Launches simple program that simulates a robot grasping and object.
It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with initial conditions, and launches a visualization.
The controller waits for and executes the references given from outside.
:param robot_name: the name of the robot to be spawned (in data/robot)
:param object_set: the name of the object set (in data/objects)
:param object_name: the name of the object (in data/objects/<object_set>)
:return: nothing
"""
# Creating world and importing terrain
world = WorldModel()
world.loadElement(terrain_file)
# Making the robot and the object (from make_elements.py)
robot = mk_el.make_robot(robot_name, world)
object = mk_el.make_object(object_set, object_name, world)
# NOT CLEAR WHAT THIS DOES... TODO: CHECK WHILE TESTING!!!
doedit = True
xform = resource.get("%s/default_initial_%s.xform" %
(object_set, robot_name),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world)
mv_el.set_moving_base_xform(robot, xform[0], xform[1])
xform = resource.get("%s/initial_%s_%s.xform" %
(object_set, robot_name, object_name),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world,
doedit=False)
if xform:
mv_el.set_moving_base_xform(robot, xform[0], xform[1])
xform = resource.get("%s/initial_%s_%s.xform" %
(object_set, robot_name, object_name),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world,
doedit=doedit)
if not xform:
print "User quit the program"
return
# this sets the initial condition for the simulation
mv_el.set_moving_base_xform(robot, xform[0], xform[1])
# Launch the simulation
program = GLSimulationPlugin(world)
sim = program.sim
# Setting up simulation parameters
visPreShrink = True # turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreShrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreShrink)
# Creating Hand emulator from the robot name
sys.path.append('../../IROS2016ManipulationChallenge')
module = importlib.import_module('plugins.' + robot_name)
# emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
# The result of adaptive_controller.make() is now attached to control the robot
import adaptive_controller
sim.setController(robot, adaptive_controller.make(sim, hand, program.dt))
# Latches the current configuration in the PID controller
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
# Updating simulation and visualization
return update_simulation(world, sim)
def launch_shelf(robotname,objects):
"""Launches the task 2 program that asks the robot to retrieve some set of objects
packed within a shelf.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname,world)
box = make_box(world,*box_dims)
shelf = make_shelf(world,*shelf_dims)
shelf.geometry().translate((0,shelf_offset,shelf_height))
rigid_objects = []
for objectset,objectname in objects:
object = make_object(objectset,objectname,world)
#TODO: pack in the shelf using x-y translations and z rotations
object.setTransform(*se3.mul((so3.identity(),[0,shelf_offset,shelf_height + 0.01]),object.getTransform()))
rigid_objects.append(object)
xy_jiggle(world,rigid_objects,[shelf],[-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100)
doedit = True
xform = resource.get("shelf/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world)
if not xform:
print "User quit the program"
return
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#controlfunc is now attached to control the robot
import shelf_controller
sim.setController(robot,shelf_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
program.reshape(800,600)
vis.show()
while vis.shown():
time.sleep(0.1)
return
#this code manually updates the vis
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
q0 = robot.getConfig()
kinematicscache = geometryopt.RobotKinematicsCache(robot, gridres, pcres)
trajcache = geometryopt.RobotTrajectoryCache(
kinematicscache,
NUM_TRAJECTORY_DIVISIONS * MORE_SUBDIVISION + MORE_SUBDIVISION - 1)
if DUMP_SDF:
for i in xrange(robot.numLinks()):
fn = 'output/' + robot.link(i).getName() + '.mat'
print("Saving SDF to", fn)
geometryopt.dump_grid_mat(trajcache.kinematics.geometry[i].grid, fn)
try:
trajinit = resource.get('robottrajopt_initial.path',
default=None,
doedit=False)
except Exception:
trajinit = None
if trajinit is None:
trajcache.qstart = q0[:]
trajcache.qstart[1] = 1.0
trajcache.qstart[2] = 1.8
trajcache.qend = q0[:]
trajcache.qend[1] = -1.0
trajcache.qend[2] = 1.8
trajcache.qstart = [
0.0, 0.2399999999999998, 0.03999999999999922, -0.8000000000000002,
-0.7400000000000002, -1.6000000000000005, 0.0
]
trajcache.qend = [
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
vis.add("IK goal", obj)
vis.dialog()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-3
space.setup()
#Generate some waypoint configurations using the resource editor
print("#########################################")
print("Editing the waypoints")
print("#########################################")
configs = resource.get("planningtest.configs",
"Configs",
default=[],
world=world,
doedit=False)
cindex = 0
while True:
if cindex < len(configs):
robot.setConfig(configs[cindex])
(save,
q) = resource.edit("plan config " + str(cindex + 1),
robot.getConfig(),
"Config",
world=world,
description="Press OK to add waypoint, cancel to stop")
if save:
if False and not space.feasible(q):
print("Configuration is infeasible. Failures:")
if qmax[i] - qmin[i] > math.pi * 2:
qmin[i] = -float('inf')
qmax[i] = float('inf')
robot.setJointLimits(qmin, qmax)
qmin, qmax = robot2.getJointLimits()
for i in range(len(qmin)):
if qmax[i] - qmin[i] > math.pi * 2:
qmin[i] = -float('inf')
qmax[i] = float('inf')
robot2.setJointLimits(qmin, qmax)
swab = world.rigidObject('swab')
plate = world.rigidObject('plate')
#add the world elements individually to the visualization
vis.add("world", world)
qstart = resource.get("start.config", world=world)
robot.setConfig(qstart)
robot2.setConfig(qstart)
#transform all the grasps to use the kinova arm gripper and object transform
orig_grasps = db.object_to_grasps["048_hammer"]
sorted_grasps = sorted(orig_grasps, key=lambda grasp: grasp.score)
grasp_swab = sorted_grasps[2].get_transformed(
swab.getTransform()).transfer(source_gripper, target_gripper)
grasp_plate = grasp.Grasp(ik_constraint=sorted_grasps[2].ik_constraint,
finger_links=sorted_grasps[2].finger_links,
finger_config=sorted_grasps[2].finger_config,
contacts=sorted_grasps[2].contacts)
grasp_plate = grasp_plate.get_transformed(plate.getTransform()).transfer(
source_gripper, target_gripper)
def launch_simple(robotname,object_set,objectname,use_box=False):
"""Launches a very simple program that simulates a robot grasping an object from one of the
databases. It first allows a user to position the robot's free-floating base in a GUI.
Then, it sets up a simulation with those initial conditions, and launches a visualization.
The controller closes the hand, and then lifts the hand upward. The output of the robot's
tactile sensors are printed to the console.
If use_box is True, then the test object is placed inside a box.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname,world)
object = make_object(object_set,objectname,world)
if use_box:
box = make_box(world,*box_dims)
object.setTransform(*se3.mul((so3.identity(),[0,0,0.01]),object.getTransform()))
doedit = True
xform = resource.get("%s/default_initial_%s.xform"%(object_set,robotname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world)
set_moving_base_xform(robot,xform[0],xform[1])
xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=False)
if xform:
set_moving_base_xform(robot,xform[0],xform[1])
xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=doedit)
if not xform:
print "User quit the program"
return
#this sets the initial condition for the simulation
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.'+robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim,0,6,6)
sim.addEmulator(0,hand)
#the result of simple_controller.make() is now attached to control the robot
import simple_controller
sim.setController(robot,simple_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
"""
program.simulate = True
vis.setPlugin(program)
vis.show()
while vis.shown():
time.sleep(0.1)
return
"""
#this code manually updates the visualization
vis.add("world",world)
vis.show()
t0 = time.time()
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.001))
t0 = t1
return
robot = world.robot(0)
#add the world elements individually to the vis
vis.add("robot", robot)
for i in range(1, world.numRobots()):
vis.add("robot" + str(i), world.robot(i))
for i in range(world.numRigidObjects()):
vis.add("rigidObject" + str(i), world.rigidObject(i))
for i in range(world.numTerrains()):
vis.add("terrain" + str(i), world.terrain(i))
#Generate some waypoint configurations using the resource editor
configs = resource.get("pathtest.configs",
"Configs",
default=[],
world=world,
doedit=False)
if len(configs) < 2:
print "Invalid # of milestones, need 2 or more"
configs = resource.get("pathtest.configs",
"Configs",
default=[],
world=world,
doedit=True)
if len(configs) < 2:
print "Didn't add 2 or more milestones, quitting"
exit(-1)
resource.set("pathtest.configs", configs)
traj0 = trajectory.RobotTrajectory(robot,
world = WorldModel()
res = world.readFile(fn)
if not res:
print("Unable to read file", fn)
exit(0)
robot = world.robot(0)
#need to fix the spin joints somewhat
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':
#need to fix the spin joints somewhat
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)
#these describe the box dimensions
goal_bounds = [(-0.08, -0.68, 0.4), (0.48, -0.32, 0.5)]
#you can play around with which object you select for problem 3b...
obj = world.rigidObject(0)
gripper_link = robot.link(9)
if PROBLEM == '3a':
qstart = resource.get("transfer_start.config", world=world)
Tobj = resource.get("transfer_obj_pose.xform",
world=world,
referenceObject=obj)
obj.setTransform(*Tobj)
qgoal = resource.get("transfer_goal.config", world=world)
robot.setConfig(qstart)
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()), Tobj)
vis.add("goal", qgoal, color=(1, 0, 0, 0.5))
robot.setConfig(qgoal)
Tobj_goal = se3.mul(robot.link(9).getTransform(), Tobject_grasp)
vis.add("Tobj_goal", Tobj_goal)
robot.setConfig(qstart)
elif PROBLEM == '3b':
#determine a grasp pose via picking
orig_grasps = db.object_to_grasps[obj.getName()]
