def PlayTrajWithPhysics(robot,traj,autoinit=False,waitdone=True,resetafter=False):
#TODO eliminate adding controller here, potential issues switching back
#Lock the environment, halt simulation in preparation
env=robot.GetEnv()
with env:
if robot.GetController().GetXMLId()!='trajectorycontroller':
ctrl=_rave.RaveCreateController(env,'trajectorycontroller')
robot.SetController(ctrl)
else:
ctrl=robot.GetController()
ctrl.SetPath(traj)
#Resets initial pose, so now we need to set any DOF not in trajectory
if autoinit:
setInitialPose(robot)
ctrl.SetDesired(robot.GetDOFValues())
#Eventuall make this variable? might not matter if .0005 is good
_time.sleep(1)
ctrl.SendCommand('start')
if waitdone:
t0=_time.time()
while not(ctrl.IsDone()):
print "Real time {}, sim time {}".format(_time.time()-t0,ctrl.GetTime())
#Only approximate time here
_time.sleep(.1)
handle=plot_projected_com(robot)
if resetafter:
with env:
ctrl.Reset()
return ctrl
def PlayTrajWithPhysics(robot,
traj,
autoinit=False,
waitdone=True,
resetafter=False):
#TODO eliminate adding controller here, potential issues switching back
#Lock the environment, halt simulation in preparation
env = robot.GetEnv()
with env:
if robot.GetController().GetXMLId() != 'trajectorycontroller':
ctrl = _rave.RaveCreateController(env, 'trajectorycontroller')
robot.SetController(ctrl)
else:
ctrl = robot.GetController()
ctrl.SetPath(traj)
#Resets initial pose, so now we need to set any DOF not in trajectory
if autoinit:
setInitialPose(robot)
ctrl.SetDesired(robot.GetDOFValues())
#Eventuall make this variable? might not matter if .0005 is good
_time.sleep(1)
ctrl.SendCommand('start')
if waitdone:
t0 = _time.time()
while not (ctrl.IsDone()):
print "Real time {}, sim time {}".format(_time.time() - t0,
ctrl.GetTime())
#Only approximate time here
_time.sleep(.1)
handle = plot_projected_com(robot)
if resetafter:
with env:
ctrl.Reset()
return ctrl
def plotProjectedCOG(robot):
return _oh.plot_projected_com(robot)
def plotProjectedCOG(robot):
return _oh.plot_projected_com(robot)
