Пример #1
0
 def test_hubo_read_trajectory(self):
     robot = self.robot
     pose = oh.Pose(robot)
     pose.update(mapping.create_random_bounded_pose(robot))
     traj, config = trajectory.create_trajectory(robot, pose.to_waypt())
     print traj
     trajectory.write_hubo_traj(traj, robot, .04)
Пример #2
0
[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')