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_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