from __future__ import with_statement # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'
from openhubo import trajectory
import openhubo
import openravepy as rave
import time
if __name__ == '__main__':
(env, options) = openhubo.setup('qtcoin')
[robot, ind, __, __, __] = openhubo.load_scene(env, options)
#Read in trajectory from matlab source
traj = trajectory.read_text_traj('trajectories/ingress_test.traj.txt',
robot, 0.01, 1.0)
config = traj.GetConfigurationSpecification()
Getvals = trajectory.makeJointValueExtractor(robot, traj, config)
Gettrans = trajectory.makeTransformExtractor(robot, traj, config)
probs_cbirrt = rave.RaveCreateProblem(env, 'CBiRRT')
env.LoadProblem(probs_cbirrt, 'rlhuboplus')
i = 0
t0 = time.time()
for i in range(traj.GetNumWaypoints()):
robot.SetTransformWithDOFValues(Gettrans(i), Getvals(i))
#print openhubo.find_com(robot)
datastring = probs_cbirrt.SendCommand(
'CheckSupport supportlinks 2 rightFoot leftFoot exact 1 ')
datalist = datastring.split(' ')
[traj,config]=tr.create_trajectory(robot)
#Note the new waypoint-building syntax
traj.Insert(0,pose0.to_waypt(dt=0.0))
traj.Insert(1,pose1.to_waypt(dt=1.0))
traj.Insert(2,pose0.to_waypt(dt=1.0))
#Need to do this to add timing information for interpolating
planningutils.RetimeActiveDOFTrajectory(traj,robot,True)
print 'Dump all DOFs to youngbum format'
tr.write_youngbum_traj(traj,robot,0.005,'traj_example_youngbum.traj')
print 'Only use a selection of DOF\'s instead of everything'
tr.write_youngbum_traj(traj,robot,0.005,'traj_example_youngbum2.traj',dofs=range(28))
print 'Write to hubo-read-trajectory compatible format'
tr.write_hubo_traj(traj,robot,0.025,'traj_example_hubo.traj')
print 'Test reading of trajectories'
traj_in_yb=tr.read_youngbum_traj('traj_example_youngbum2.traj',robot,0.005)
traj_in_text=tr.read_text_traj('traj_example_youngbum2.traj',robot,0.005)
print 'Test IU trajectory import'
iutraj = tr.IUTrajectory(robot)
iutraj.load_from_file('70_0.20.iutraj','iumapping.txt',openhubo.get_root_dir())
traj2=iutraj.to_openrave()
tr.write_hubo_traj(traj2,robot,0.025,'traj_example_iu.traj')
from __future__ import with_statement # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'
from openhubo import trajectory
import openhubo
import openravepy as rave
import time
if __name__=='__main__':
(env,options)=openhubo.setup('qtcoin')
[robot,ind,__,__,__]=openhubo.load_scene(env,options)
#Read in trajectory from matlab source
traj=trajectory.read_text_traj('trajectories/ingress_test.traj.txt',robot,0.01,1.0)
config=traj.GetConfigurationSpecification()
Getvals=trajectory.makeJointValueExtractor(robot,traj,config)
Gettrans=trajectory.makeTransformExtractor(robot,traj,config)
probs_cbirrt = rave.RaveCreateProblem(env,'CBiRRT')
env.LoadProblem(probs_cbirrt,'rlhuboplus')
i=0
t0=time.time()
for i in range(traj.GetNumWaypoints()):
robot.SetTransformWithDOFValues(Gettrans(i),Getvals(i))
#print openhubo.find_com(robot)
datastring=probs_cbirrt.SendCommand('CheckSupport supportlinks 2 rightFoot leftFoot exact 1 ')
datalist=datastring.split(' ')
check=float(datalist[0])