def load_scene(env, scenename=None, stop=False, physics=True):
""" Load a robot model into the given environment, configuring a
trajectorycontroller and a reference robot to show desired movements vs. actual
pose. The returned tuple contains:
robot: handle to the created robot
controller: either trajectorycontroller or idealcontroller depending on physics
name-to-joint-index converter
ref_robot: handle to visiualization "ghost" robot
recorder: video recorder python class for quick video dumps
"""
# Set the robot controller and start the simulation
recorder = viewerrecorder(env)
#Default to "sim-timed video" i.e. plays back much faster
recorder.videoparams[0:2] = [1024, 768]
recorder.realtime = False
with env:
if stop:
env.StopSimulation()
if type(scenename) is list:
for n in scenename:
loaded = env.Load(n)
elif type(scenename) is str:
loaded = env.Load(scenename)
time.sleep(1)
#Explicitly disable physics if option is selected
with env:
if not physics:
env.SetPhysicsEngine(
rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine'))
robot = env.GetRobots()[0]
pose = ones(robot.GetDOF()) * .6
robot.SetDOFValues(pose)
collisionChecker = rave.RaveCreateCollisionChecker(env, 'pqp')
if collisionChecker == None:
collisionChecker = rave.RaveCreateCollisionChecker(env, 'ode')
print 'Note: Using ODE collision checker since PQP is not available'
env.SetCollisionChecker(collisionChecker)
if env.GetPhysicsEngine().GetXMLId(
) != 'GenericPhysicsEngine' and physics:
controller = rave.RaveCreateController(env, 'servocontroller')
robot.SetController(controller)
controller.SendCommand('set gains 100 0 0')
controller.SetDesired(pose / pose * pi / 4 * 1.1)
time.sleep(.5)
ind = makeNameToIndexConverter(robot)
return (robot, controller, ind, recorder)
def load_scene(env,scenename=None,stop=False,physics=True):
""" Load a robot model into the given environment, configuring a
trajectorycontroller and a reference robot to show desired movements vs. actual
pose. The returned tuple contains:
robot: handle to the created robot
controller: either trajectorycontroller or idealcontroller depending on physics
name-to-joint-index converter
ref_robot: handle to visiualization "ghost" robot
recorder: video recorder python class for quick video dumps
"""
# Set the robot controller and start the simulation
recorder=viewerrecorder(env)
#Default to "sim-timed video" i.e. plays back much faster
recorder.videoparams[0:2]=[1024,768]
recorder.realtime=False
with env:
if stop:
env.StopSimulation()
if type(scenename) is list:
for n in scenename:
loaded=env.Load(n)
elif type(scenename) is str:
loaded=env.Load(scenename)
time.sleep(1)
#Explicitly disable physics if option is selected
with env:
if not physics:
env.SetPhysicsEngine(rave.RaveCreatePhysicsEngine(env,'GenericPhysicsEngine'))
robot = env.GetRobots()[0]
pose=ones(robot.GetDOF())*.6
robot.SetDOFValues(pose)
collisionChecker = rave.RaveCreateCollisionChecker(env,'pqp')
if collisionChecker==None:
collisionChecker = rave.RaveCreateCollisionChecker(env,'ode')
print 'Note: Using ODE collision checker since PQP is not available'
env.SetCollisionChecker(collisionChecker)
if env.GetPhysicsEngine().GetXMLId()!='GenericPhysicsEngine' and physics:
controller=rave.RaveCreateController(env,'servocontroller')
robot.SetController(controller)
controller.SendCommand('set gains 100 0 0')
controller.SetDesired(pose/pose*pi/4*1.1)
time.sleep(.5)
ind=makeNameToIndexConverter(robot)
return (robot,controller,ind,recorder)
waypt0 = list(pose0)
waypt1 = list(pose1)
waypt0.extend(zeros(7))
waypt1.extend(zeros(7))
waypt0.append(t0)
waypt1.append(t1)
waypt2 = copy(waypt0)
waypt2[-1] = t1
traj.Insert(0, waypt0)
traj.Insert(1, waypt1)
traj.Insert(2, waypt2)
planningutils.RetimeActiveDOFTrajectory(traj, robot, True)
# Prove that the retiming actually works
for k in range(40):
data = traj.Sample(float(k) / 10)
print data[ind("LKP")]
vidrec = viewerrecorder(env)
controller.SetPath(traj)
vidrec.start()
controller.SendCommand("start")
while not (controller.IsDone()):
time.sleep(0.1)
vidrec.stop()
waypt0 = list(pose0)
waypt1 = list(pose1)
waypt0.extend(zeros(7))
waypt1.extend(zeros(7))
waypt0.append(t0)
waypt1.append(t1)
waypt2 = copy(waypt0)
waypt2[-1] = t1
traj.Insert(0, waypt0)
traj.Insert(1, waypt1)
traj.Insert(2, waypt2)
planningutils.RetimeActiveDOFTrajectory(traj, robot, True)
#Prove that the retiming actually works
for k in range(40):
data = traj.Sample(float(k) / 10)
print data[ind('LKP')]
vidrec = viewerrecorder(env)
controller.SetPath(traj)
vidrec.start()
controller.SendCommand('run')
while not (controller.IsDone()):
time.sleep(.1)
vidrec.stop()
