Пример #1
0
def start():
	if cfg.Demonstration():


		JointsBuffer1 = convert_joints()
		JointsBuffer2 = convert_joints()
		
		try: 
			# Init threads with jointsbuffer
			t1=RobotThread("R1",JointsBuffer1)
			t2=RobotThread("R2",JointsBuffer2)


			# If program crashes, shut down threads
			t1.setDaemon(True)
			t2.setDaemon(True)

			# start thread
			t1.start()
			t2.start()


			while True:
				pass # Just keeps run state going until user cancels

		except KeyboardInterrupt:
			t1.Set_Run_State(False)
			t2.Set_Run_State(False)
			print "Execution stopped"


		return "USERINPUT"
	else:


		#================================
		#========= INITIERING ===========
		#================================


		# initiate tasks
		t=RobotTasks(["R1","R2"])



		#===============================================
		#============ Robot 1, RIGHT, from observer ====
		#===============================================
		JHome=[0,0,0,0,30,0]
		JGet=[30,60,20,0,0,0]
		JPlace1=[-30,60,-70,-130,60,150]
		JPlace2=[-40,70,-36,42,-34,-173]
		JPlace3=[-41,65,-90,8,32,150]

	        middle_point1 = [-24.18, 69.51, -12.3, 52.69, -69.14, -24.89]#xyz, 455.64, 36.75, 169.07 mm
                outer_point1 = [31.14, 53.06, -4.54, -25.1, 24.75, 68.41] #xyz = 427, 543, 205.3 mmm
                
                tas1 = [middle_point1, outer_point1, middle_point1 , outer_point1 , middle_point1, outer_point1]

		#tas1=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
		#tas1=[JPlace1]

                #==============================================
                #============ Robot 2, LEFT from observer =====
		#==============================================
		JHome=[0,0,0,0,30,0]
		JGet=[-30,60,20,0,0,0]
		JPlace1=[40,67,-73,0,52,5]
		JPlace2=[37,42,-12,0,-50,5]
		JPlace3=[13,42,-61,99,68,5]
                    
                middle_point2 = [22.28, 68.45, -10.05, 98.56, 69.60, 5.14]
		outer_point2 = [-29.46, 55, -12.5, 45.7, -9.5] #xyz, 428,-544, 220 mm	
		
                tas2 =[outer_point2, middle_point2, outer_point2, middle_point2, outer_point2, middle_point2]
                #tas2=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
		#tas2=[JPlace1]


		# test add both tasks and single task into both dicts.
		t.add_task("R1",tas1)
		t.add_task("R2",tas2)

		t1=RobotThread("R1",[[]])
		t2=RobotThread("R2",[[]])
		t1.setDaemon(True)
		t2.setDaemon(True)
		t1.start()
		t2.start()

		robot=RobotCommander() 
                # UNsure which arm is which..................
		pp=ros(Robot("left_arm"),Robot("right_arm"),plan_scene()) 

		##UUgly again
		class listener_class(threading.Thread):
			def __init__(self, t1,t2):
				self.t1 = t1
				self.t2 = t2
				threading.Thread.__init__(self)
				self.listener()

			def callback(self):
				t1.Set_Run_State(False)
				t2.Set_Run_State(False)
			def listener(self):
				rospy.Subscriber("/planning_scene", moveit_msgs.msg.PlanningScene, self.callback())
        		rospy.spin

		listener_thread = listener_class(t1,t2)
		listener_thread.setDaemon(True)
		listener_thread.start()


		#================================
		# DRIFT AV ROBOTAR OCH UPPGIFTER.
		#================================

		try:
			ir1 = 0
                        ir2 = 0
			while t.count_tasks("R1") > 0 or t.count_tasks("R2") > 0:
				# t.print_tasks()
                                
				if not t1.is_alive() and t.count_tasks("R1") > 0:
					print "Planning for Robot 1"
					# Update scene (ej klar än)
					#print "Setting R1",
					#print [i*3.141592/180.0 for i in robotComm.GetJoints("R1")],
					#print [i*3.141592/180 for i in t.current_task("R1")]
				        start = time.time()	
					Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R1")],[i*3.141592/180 for i in t.current_task("R1")],"R1")
					# For some strange reason, [0,0,0,0,0,0,0] is added to Joints_buffer in the end.
					# Joints_buffer.pop() removes the last index and in an uggly way solves it
                                        if not Joints_buffer:
                                            print "----------No MotionPlan found for R1"
                                        else:
                                            Joints_buffer.pop() 
					#print "Starting Robot 1"
					#print Joints_buffer
					t1=RobotThread("R1",Joints_buffer)
					t1.setDaemon(True)
					#t1.Set_Buffer(t.current_task("R1"))
					#print "R1: Task set:",
					#print t.current_task("R1")
					t1.Set_Run_State(True)
					#t1.Execute_Buffer()
					t1.start()
                                        end = time.time()
                                        ir1 = ir1 + 1
                                        print "------------Robot1 seq: ", str(ir1)
                                        print "------------Robot1 Total plantime: ", str(start-end)
                                        
					t.task_complete("R1")
				elif not t2.is_alive() and t.count_tasks("R2") > 0:
					print "Planning for Robot 2"
					#print "Setting R2",
					#print [i*3.141592/180.0 for i in robotComm.GetJoints("R2")],
					#print [i*3.141592/180 for i in t.current_task("R2")]
                                        start2 = time.time()
					Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R2")],[i*3.141592/180 for i in t.current_task("R2")],"R2")
					
                                        if not Joints_buffer:
                                            print "-------------No MotionPlan found for R2"
                                        else:
                                            Joints_buffer.pop()
					#print "Starting Robot 2"
					#print Joints_buffer
					t2=RobotThread("R2",Joints_buffer)
					t2.setDaemon(True)
					#t2.Set_Buffer(t.current_task("R2"))
					#print "R2: Task set:",
					#print t.current_task("R2")
					t2.Set_Run_State(True)
					t2.start()
					#t2.Execute_Buffer()
                                        end2 =time.time()
                                        ir2 = ir2 + 1
                                        print "-------------Robot2 seq: ", str(ir2)
                                        print "-------------Robot2 Total plantime: ", str(start2-end2)
					t.task_complete("R2")



			while t1.is_alive() or t2.is_alive():
				pass
			print "All tasks are completed"
			time.sleep(1) # Let the threads settle before entering USERINPUT

		except KeyboardInterrupt:
			t1.Set_Run_State(False)
			t2.Set_Run_State(False)
			print "Execution stopped"

		
	return "USERINPUT"
Пример #2
0
def start():
	if cfg.Demonstration():


		JointsBuffer1 = convert_joints()
		JointsBuffer2 = convert_joints()
		
		try: 
			# Init threads with jointsbuffer
			t1=RobotThread("R1",JointsBuffer1)
			t2=RobotThread("R2",JointsBuffer2)


			# If program crashes, shut down threads
			t1.setDaemon(True)
			t2.setDaemon(True)

			# start thread
			t1.start()
			t2.start()


			while True:
				pass # Just keeps run state going until user cancels

		except KeyboardInterrupt:
			t1.Set_Run_State(False)
			t2.Set_Run_State(False)
			print "Execution stopped"


		return "USERINPUT"
	else:
 	
		# initiate tasks
		t=RobotTasks(["R1","R2"])

		# Robot 1
		JHome=[0,0,0,0,30,0]
		JGet=[30,60,20,0,0,0]
		JPlace1=[-30,60,-70,-130,60,150] # POSE MÅL: 450,33, -15,15, 592,10, [-1 -2 1 0]
		JPlace2=[-40,70,-36,42,-34,-173]
		JPlace3=[-41,65,-90,8,32,150]

		#tas1=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
		tas1=[JPlace1]


		# Robot 2
		JHome=[0,0,0,0,30,0]
		JGet=[-30,60,20,0,0,0]
		JPlace1=[40,67,-73,0,52,5]  # POSE MÅL: 453,18, 80,26, 444,89, [0 0 0 0]
		JPlace2=[37,42,-12,0,-50,5]
		JPlace3=[13,42,-61,99,68,5]

			
		#tas2=[JHome,JGet,JPlace1,JGet,JPlace2,JGet,JPlace3,JHome]
		tas2=[JPlace1]


		# test add both tasks and single task into both dicts.
		#t.add_task("R1",tas1)
		#t.add_task("R2",tas2)

		t1=RobotThread("R1",[[]])
		t2=RobotThread("R2",[[]])
		t1.setDaemon(True)
		t2.setDaemon(True)
		t1.start()
		t2.start()

		robot=RobotCommander() 
		pp=ros(Robot("left_arm"),Robot("right_arm"),plan_scene())

		# ##UUgly again
		# class listener_class(threading.Thread):
		# 	def __init__(self, t1,t2):
		# 		self.t1 = t1
		# 		self.t2 = t2
		# 		threading.Thread.__init__(self)
		# 		self.listener()

		# 	def callback(self):
		# 		t1.Set_Run_State(False)
		# 		t2.Set_Run_State(False)
		# 	def listener(self):
		# 		rospy.Subscriber("/planning_scene", moveit_msgs.msg.PlanningScene, self.callback())
  #       		rospy.spin

		# listener_thread = listener_class(t1,t2)
		# listener_thread.setDaemon(True)
		# listener_thread.start()

		xyz1=[]
		xyz2=[]
		times1=[]
		times2=[]

		
		#pose test
		# Robot 1
		mittpunkt=[455.64/1000.0, 36.75/1000.0, 169.07/1000.0]
		yttrepunkt=[427/1000.0, 543/1000.0, 205.3/1000.0]

		tas1=[yttrepunkt, mittpunkt, yttrepunkt, mittpunkt]

		# Robot 2
		mittpunkt=[455.64/1000.0, -36.75/1000.0, 169.07/1000.0]
		yttrepunkt=[428/1000.0, -544/1000.0, 202/1000.0]

		tas2=[yttrepunkt, mittpunkt, yttrepunkt, mittpunkt]

		#================================
		# DRIFT AV ROBOTAR OCH UPPGIFTER.
		#================================
		# Kör 10 testfall
		for i in range(5):

			robotComm.SetJoints("R1")
			robotComm.SetJoints("R2")
			time.sleep(2)

			t.add_task("R1",tas1)
			t.add_task("R2",tas2)


			try:

				while t.count_tasks("R1") > 0 or t.count_tasks("R2") > 0:
					# t.print_tasks()
					s=time.time()
					if not t1.is_alive() and t.count_tasks("R1") > 0:
						print "Planning for Robot 1"
						
						
						Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R1")],[i for i in t.current_task("R1")],"R1")
						# For some strange reason, [0,0,0,0,0,0,0] is added to Joints_buffer in the end.
						# Joints_buffer.pop() removes the last index and in an uggly way solves it
                                                print "R1: Task set:",
						print t.current_task("R1")

						if not Joints_buffer:
							print "----------No MotionPlan found for R1"
						else:
							Joints_buffer.pop() 
							#t.task_complete("R1")
                                               
						#print "Starting Robot 1"
						#print Joints_buffer
						t1=RobotThread("R1",Joints_buffer)
						t1.setDaemon(True)
						#t1.Set_Buffer(t.current_task("R1"))
						
						t1.Set_Run_State(True)
						#t1.Execute_Buffer()
						t1.start()
						times1.append(time.time()-s)
						
					elif not t2.is_alive() and t.count_tasks("R2") > 0:
						print "Planning for Robot 2"
						
						Joints_buffer = pp.plan([i*3.141592/180 for i in robotComm.GetJoints("R2")],[i*3.141592/180 for i in t.current_task("R2")],"R2")
						print "R2: Task set:",
						print t.current_task("R2")
						if not Joints_buffer:
							print "----------No MotionPlan found for R2"
						else:
							Joints_buffer.pop()
							#t.task_complete("R2")

						#print "Starting Robot 2"
						#print Joints_buffer
						t2=RobotThread("R2",Joints_buffer)
						t2.setDaemon(True)
						#t2.Set_Buffer(t.current_task("R2"))
						
						t2.Set_Run_State(True)
						t2.start()
						times2.append(time.time()-s)
						#t2.Execute_Buffer()
						
						



				while t1.is_alive() or t2.is_alive():
					pass
				print "All tasks are completed"
				#time.sleep(2) # Let the threads settle before entering USERINPUT

			except KeyboardInterrupt:
				t1.Set_Run_State(False)
				t2.Set_Run_State(False)
				print "Execution stopped"

			# Let threads settle before getting data
			time.sleep(2)

			xyz1.append(robotComm.get_cartesian("R1"))
			xyz2.append(robotComm.get_cartesian("R2"))


			#raw_input()

		print "============================"
		print "= RESULTAT AV ROBOT 1, xyz ="
		print "============================"
		for i in xyz1:
			print i

		print "============================"
		print "= RESULTAT AV ROBOT 1, tid ="
		print "============================"
		for i in times1:
			print i

		print "============================"
		print "= RESULTAT AV ROBOT 1, xyz ="
		print "============================"
		for i in xyz2:
			print i

		print "============================"
		print "= RESULTAT AV ROBOT 2, tid ="
		print "============================"
		for i in times2:
			print i


		f = open("2arm_noobj.csv", 'w')
		f.write(f, "==================\n Resultat robot 1, xyz \n ============")
		f.write(f, xyz1)
		f.write(f, "\n==================\n Resultat robot 1, tid \n ============")
		f.write(f, tid1)
		f.write(f, "\n==================\n Resultat robot 2, xyz \n ============")
		f.write(f, xyz2)
		f.write(f, "\n==================\n Resultat robot 2, tid \n ============")
		f.write(f, tid2)
		
	return "USERINPUT"