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
