Program Examples:Visualization with RoboDK

From SoftMC-Wiki
Revision as of 06:46, 18 April 2018 by Itay (talk | contribs)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search
Language: English  • 中文(简体)‎

Here is an example of the programs that can be used in order to visualize the PUMA Soft-MC group in RoboDK software.
These programs can be used to visualize one or more PUMA robots.These programs can be modified to allow the visualization of other types of groups/robots.
This pdf file:[1] is a presentation explaining these programs more extensively.
This python file: [2]is the python library used in the example.
Draft hands out working with softMC simulator and RoboDK [3]



The first program is the server side, hosted by the MC.
This program opens a TCP/IP server, waits for the connection of the client (RoboDK) and sends the robot's joints values to the client in an infinite loop.

'------------------------------------------------------------------------------
' 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---------------'





The second program is the client side, running in RoboDK (in python language).
This program uses the class MCenv which inherits from the robolink class of RoboDK.
With this class one can set the puma robots to use, to read the data from the MC and to move the robots in the scene.

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')