Program Examples:Visualization with RoboDK/zh-hans

From SoftMC-Wiki
Jump to: navigation, search
语言: [[::Program Examples:Visualization with RoboDK|English]]  • [[::Program Examples:Visualization with RoboDK/zh-hans|中文(简体)‎]]

以下是可以使用RoboDK软件中的PUMA Soft-MC组显示程序的示例。
这些程序可用于可视化一个或多个PUMA机器人。这些程序可以进行修改,以允许其他类型的组/机器人的可视化。
pdf文件:[1]是更详细地解释这些程序的演示文稿。
python文件: [2]是示例中使用的python库。
使用softMC模拟器和RoboDK的文档正在拟定[3]



第一个程序是服务器端,主机为MC。
该程序打开一个TCP / IP服务器,等待客户端(RoboDK)的连接,并将机器人的关节值发送到无限循环中的客户端。

'------------------------------------------------------------------------------
' File:        server.prg
' Purpose:     open a TCP/IP server, allow connection of client, and send data to client
' Version:     1.00
' Author:      Ron Danon
' Description:
' History:     2016-09-08  server.prg  V1.00
'              Created
'------------------------------------------------------------------------------
dim shared numOfRobots as long = 1

program

	dim index as long
	dim sockNum as long = 1
	dim cPort as long = 7132
	dim jointStr as string
	dim robotGroups[1] as generic group 'same as size of numOfRobots
  	dim robotNames[1] as string 'same as size of numOfRobots
  	
'---------------Define robot groups and names---------------'  	
robotGroups[1] = PUMA
robotNames[1] = "robot1"
'robotGroups[2] = PUMA2
'robotNames[2] = robot2
  	
'---------------Opening new socket---------------'
	call openNewSocket(sockNum,cPort)
  
'---------------Connecting to client---------------'  
	call connectClient(sockNum,cPort,0)

'---------------Main Loop---------------'  
	while True
		call createJsonStr(robotNames,robotGroups,jointStr)
		call checkIfReady(sockNum,cPort) 
		print #sockNum,jointStr     
	end while

'---------------Close sockets and exit---------------' 
' This section is not reached - might be used in future versions
	print #sockNum,"finish"
	sleep 1000
	close #sockNum
	Print "Server Closed Sockets. Server Exits"
	
end program ' server.prg


'---------------------------------------'
'---------------Functions---------------' 
'---------------------------------------'

sub openNewSocket(sockNum as long,cPort as Long)
	dim opened as long = 0
	while not opened 
		Try
			OpenSocket Options=1 as #sockNum
			opened = 1
		catch 5043 'socket is already open
			print "socket", sockNum, "is already open. trying next socket"
			sockNum = sockNum + 1
		end Try
	end while
	print "socket", sockNum, "is set"
end sub


sub connectClient(sockNum as long, cPort as Long, ByVal sockIsOpen as long)
	dim str1 as string
	if sockIsOpen then
		close #sockNum
		OpenSocket Options=1 as #sockNum
	end if
	Accept(#sockNum, cPort)
	sleep 500
	str1=input$(loc(sockNum),#sockNum) 'receive data to check if connected
	?"client is connected"
	sleep 200
end sub


sub checkIfReady(sockNum as long,cPort as Long)
	'Waits for a 'ready' input from roboDK before sending the next JSON string
	dim ready as long = 0
	while not ready
		Try
			while loc(sockNum) = 0   'while no data is available
				sleep 10
			end while    
		catch 5041 'client disconnected
			print "client disconnected, waiting for connection"
			call connectClient(sockNum,cPort,1)
		end Try
		ready = (input$(loc(sockNum),#sockNum) = "ready") 'check if the input is "ready"
	end while
end sub


sub createJsonStr(robotNames[*] as string, robotGroups[*] as generic group, jointStr as string)
'FORMAT --> '{"robotName": [double-a1, double-a2, double-a3, double-a4, double-a5, double-a6]}' <---
'double is in precision of 2 digits after the point, i.e.: '0.12', '179.12' 
	dim index1 as long
	dim index2 as long
	dim jointValues as generic joint
	
	jointStr = "{"
	for index1 = 1 to numOfRobots
		jointValues = robotGroups[index1].PFb
		jointStr = jointStr + chr$(34) + robotNames[index1] + chr$(34) + ": ["
		for index2 = 1 to 6
			jointStr = jointStr + STRD$(jointValues{index2}, "%.2f") + ", "	
		next
		jointStr = LEFT$(jointStr,LEN(jointStr)-2) + "], "
	next
	jointStr = LEFT$(jointStr,LEN(jointStr)-2) + "}"
end sub

'---------------End of functions---------------'





第二个程序是客户端,以RoboDK(以python语言)运行。
该程序使用继承自RoboDK的robolink类的MCenv类。
有了这个类可以设置PUMA机器人的使用,读取来自MC的数据,并在场景中移动机器人。

import mcEnv
    
host, port = "10.4.20.66", 7132
finsihedTransmit = False

MCE = mcEnv.MCenv()
MCE.setSimulationSpeed(1000)
MCE.connectClient(host,port)
MCE.setRobot("robot1", "hsr_jr612")

robotList = ["robot1"]

while not finsihedTransmit:
    MCE.writeStr("ready")
    jsonStr = MCE.readJsonStr()
    MCE.moveRobots(robotList, jsonStr)
    
# This section is not reached - might be used in future versions
raise Exception('Finished')