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)
pose0.send() env.StartSimulation(timestep=timestep) pose1=openhubo.Pose(robot,controller) pose1['LAP']=-pi/8 pose1['RAP']=-pi/8 pose1['LKP']=pi/4 pose1['RKP']=pi/4 pose1['LHP']=-pi/8 pose1['RHP']=-pi/8 [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))
