try: answer = self.probs[0].SendCommand('RunCBiRRT jointgoals %d %s %s'%(len(goaljoints),Serialize1DMatrix(matrix(goaljoints)),TSRChainString_A1)) print "runcbirrt successful" print answer if (answer == '0'): return [] except openrave_exception, e: print "Cannot send command runcbirrt" print e # Rename the file so that we can keep the data w/o overwriting it with a new trajectory try: os.rename("cmovetraj.txt","movetraj0.txt") print "Executing trajectory 0" try: answer = pr2_rave2ros_traj_play_from_file('movetraj0.txt') print "traj call successful" print answer except openrave_exception, e: print e except OSError, e: print e ############################################# ## CREATE TRAJ 1: APPROACH 1 -->APPROACH 2 ## ############################################# print "Press enter to generate approach-1 --> approach-2 (trajectory1)" sys.stdin.readline() goaljoints = str2num(self.approachik_2) TSRString_A2_R = SerializeTSR(5,'NULL',self.T0_RH1_APPROACH_2,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
def handle_play_trajectories(self,req): print "Playing trajectories" ####################################################### ## WE'RE DONE GENERATING TRAJECTORIES, NOW PLAY THEM ## ####################################################### pr2_rave2ros_traj_play_from_file('movetraj0.txt') # initconfig --> approach 1 pr2_rave2ros_traj_play_from_file('movetraj1.txt') # approach 1 --> approach 2 pr2_rave2ros_traj_play_from_file('movetraj2.txt') # approach 2 --> startik numreps = 4 for r in range(numreps): pr2MoveGrippers('close') # Close grippers in ROS so that we can interact with the wheel in Gazebo pr2_rave2ros_traj_play_from_file('movetraj3.txt') # startik --> goalik pr2MoveGrippers('open') # Open grippers in ROS so that we can interact with the wheel in Gazebo pr2_rave2ros_traj_play_from_file('movetraj4.txt') # goalik --> startik pr2_rave2ros_traj_play_from_file('movetraj5.txt') # startik --> approach 2 pr2_rave2ros_traj_play_from_file('movetraj6.txt') # approach 2 --> approach 1 pr2_rave2ros_traj_play_from_file('movetraj7.txt') # approach 1 --> initconfig ########################################## ## DONE PLAYING THE TRAJECTORIES - EXIT ## ########################################## print "press enter to exit" sys.stdin.readline() emptyResponse = [] return emptyResponse