コード例 #1
0
ファイル: run.py プロジェクト: hoffiee/OnlineStyrning
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
# Allt som behövs importeras
from Robots import Robot
from Planscene import plan_scene
from ROS import ros
import moveit_msgs.msg
from moveit_commander import RobotCommander
import rospy


# Debugging
# from optparse import OptionParser
# import inspect



robot=RobotCommander()
pp=ros(Robot("manipulator"),plan_scene())



print pp.plan([1.0,1.0,0,0,0,0],[0.0,0.0,0.0,0.0,0.0,0.0])
print "done 1"
rospy.sleep(1)
print pp.plan([1.0,1.0,0,0,0,0],[0.0,0.0,0.0,0.0,0.0,0.0])

print "Done 2"



コード例 #3
0
ファイル: __init__.py プロジェクト: hoffiee/OnlineStyrning
#Node is used for remapping joint_states to /robot/joint_states. Solution for problem with ROS...

#Node which the different messages are published from!
#Skapar object av klassen


#Enarmad Robot
#robot1 = Robot("manipulator")


#Tvåarmad Robot
robot1 = Robot("right_arm", 1)
robot2 = Robot("left_arm", 2)

#Scene objektet
scene = plan_scene()
# Starting a prompt, One Arm
#promt(scene, robot1)
# Starting a prompt, TWO ARMS
promt(scene, robot1, robot2)









コード例 #4
0
ファイル: run.py プロジェクト: hoffiee/OnlineStyrning
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"
コード例 #5
0
                self.test_scene.remove_object(id)

            self.test_scene.r1_publish()




            rospy.sleep(random.uniform(1,4))
    except moveit_commander.exception.MoveItCommanderException as er:
        print str(er)

    except KeyboardInterrupt as er:
        print str(er)

rospy.init_node("Plan_scene_publisher")
test_scene = plan_scene()
move_object_demo()



def add_move_remove(self):
    try:
        while True:

            print "Iteration " , str(i)
            i = i +1
            id_list = self.test_scene.get_id_list()
            if len(id_list) == 0:
                r_action = 0
            else:
                r_id= randint(0,len(id_list)-1)