-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsendSimultan2MovementSequences-mov.py
133 lines (110 loc) · 7.01 KB
/
sendSimultan2MovementSequences-mov.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
# Make sure to have CoppeliaSim running, with followig scene loaded:
#
# scenes/messaging/movementViaRemoteApi.ttt
#
# Do not launch simulation, then run this script
#
# The client side (i.e. this script) depends on:
#
# sim.py, simConst.py, and the remote API library available
# in programming/remoteApiBindings/lib/lib
# Additionally you will need the python math and msgpack modules
try:
import sim
except:
print ('--------------------------------------------------------------')
print ('"sim.py" could not be imported. This means very probably that')
print ('either "sim.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "sim.py"')
print ('--------------------------------------------------------------')
print ('')
import math
import msgpack
class Client:
def __enter__(self):
self.executedMovId1='notReady'
self.executedMovId2='notReady'
sim.simxFinish(-1) # just in case, close all opened connections
self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim
return self
def __exit__(self,*err):
sim.simxFinish(-1)
print ('Program ended')
with Client() as client:
print("running")
if client.id!=-1:
print ('Connected to remote API server')
targetArm1='/blueArm'
targetArm2='/redArm'
client.stringSignalName1=targetArm1+'_executedMovId'
client.stringSignalName2=targetArm2+'_executedMovId'
def waitForMovementExecuted1(id):
while client.executedMovId1!=id:
retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_buffer)
if retCode==sim.simx_return_ok:
if type(s)==bytearray:
s=s.decode('ascii') # python2/python3 differences
client.executedMovId1=s
def waitForMovementExecuted2(id):
while client.executedMovId2!=id:
retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_buffer)
if retCode==sim.simx_return_ok:
if type(s)==bytearray:
s=s.decode('ascii') # python2/python3 differences
client.executedMovId2=s
# Start streaming client.stringSignalName1 and client.stringSignalName2 string signals:
sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_streaming)
sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_streaming)
# Set-up some movement variables:
mVel=100*math.pi/180
mAccel=150*math.pi/180
maxVel=[mVel,mVel,mVel,mVel,mVel,mVel]
maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel]
targetVel=[0,0,0,0,0,0]
# Start simulation:
sim.simxStartSimulation(client.id,sim.simx_opmode_blocking)
# Wait until ready:
waitForMovementExecuted1('ready')
waitForMovementExecuted1('ready')
# Send first movement sequence:
targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180]
movementData={"id":"movSeq1","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
# Execute first movement sequence:
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot)
# Wait until above movement sequence finished executing:
waitForMovementExecuted1('movSeq1')
waitForMovementExecuted1('movSeq1')
# Send second and third movement sequence, where third one should execute immediately after the second one:
targetConfig=[-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180]
targetVel=[-60*math.pi/180,-20*math.pi/180,0,0,0,0]
movementData={"id":"movSeq2","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
targetConfig=[0,0,0,0,0,0]
targetVel=[0,0,0,0,0,0]
movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_movementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
# Execute second and third movement sequence:
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRemoteApi_executeMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot)
# Wait until above 2 movement sequences finished executing:
waitForMovementExecuted1('movSeq3')
waitForMovementExecuted1('movSeq3')
sim.simxStopSimulation(client.id,sim.simx_opmode_blocking)
sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_discontinue)
sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_discontinue)
sim.simxGetPingTime(client.id)
# Now close the connection to CoppeliaSim:
sim.simxFinish(client.id)
else:
print ('Failed connecting to remote API server')