-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathsimpleMovement.py
154 lines (115 loc) · 4.95 KB
/
simpleMovement.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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
# Make sure to have the add-on "ZMQ remote API" running in
# CoppeliaSim. Do not launch simulation, but run this script
import threading
import math
from coppeliasim_zmqremoteapi_client import RemoteAPIClient
def blueRobot():
robotColor = 'blue'
client = RemoteAPIClient()
sim = client.require('sim')
sims[robotColor] = sim
sim.setStepping(True)
jointHandles = []
for i in range(7):
jointHandles.append(sim.getObject('/' + robotColor + 'Robot/joint', {'index': i}))
vel = 110 * math.pi / 180
accel = 40 * math.pi / 180
jerk = 80 * math.pi / 180
maxVel = [vel, vel, vel, vel, vel, vel, vel]
maxAccel = [accel, accel, accel, accel, accel, accel, accel]
maxJerk = [jerk, jerk, jerk, jerk, jerk, jerk, jerk]
for i in range(1):
targetPos1 = [90 * math.pi / 180, 90 * math.pi / 180, 170 * math.pi / 180, -90 * math.pi / 180, 90 * math.pi / 180, 90 * math.pi / 180, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos1)
targetPos2 = [-90 * math.pi / 180, 90 * math.pi / 180, 180 * math.pi / 180, -90 * math.pi / 180, 90 * math.pi / 180, 90 * math.pi / 180, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos2)
targetPos3 = [0, 0, 0, 0, 0, 0, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos3)
sim.setStepping(False)
def redRobot():
robotColor = 'red'
client = RemoteAPIClient()
sim = client.require('sim')
sims[robotColor] = sim
sim.setStepping(True)
jointHandles = []
for i in range(7):
jointHandles.append(sim.getObject('/' + robotColor + 'Robot/joint', {'index': i}))
vel = 110 * math.pi / 180
accel = 100 * math.pi / 180
jerk = 200 * math.pi / 180
maxVel = [vel, vel, vel, vel, vel, vel, vel]
maxAccel = [accel, accel, accel, accel, accel, accel, accel]
maxJerk = [jerk, jerk, jerk, jerk, jerk, jerk, jerk]
for i in range(1):
targetPos1 = [90 * math.pi / 180, 90 * math.pi / 180, 170 * math.pi / 180, -90 * math.pi / 180, 90 * math.pi / 180, 90 * math.pi / 180, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos1)
targetPos2 = [-90 * math.pi / 180, 90 * math.pi / 180, 180 * math.pi / 180, -90 * math.pi / 180, 90 * math.pi / 180, 90 * math.pi / 180, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos2)
targetPos3 = [0, 0, 0, 0, 0, 0, 0]
moveToConfig(robotColor, jointHandles, maxVel, maxAccel, maxJerk, targetPos3)
sim.setStepping(False)
def greenRobot():
robotColor = 'green'
client = RemoteAPIClient()
sim = client.require('sim')
sims[robotColor] = sim
sim.setStepping(True)
targetHandle = sim.getObject('/' + robotColor + 'Robot/target')
vel = 0.5
accel = 0.1
jerk = 3
maxVel = [vel, vel, vel, vel]
maxAccel = [accel, accel, accel, accel * 10]
maxJerk = [jerk, jerk, jerk, jerk]
initTr = sim.getObjectPose(targetHandle)
for i in range(1):
goalTr = initTr.copy()
goalTr[2] = goalTr[2] + 0.2
params = {}
params['object'] = targetHandle
params['targetPose'] = goalTr
params['maxVel'] = maxVel
params['maxAccel'] = maxAccel
params['maxJerk'] = maxJerk
sim.moveToPose(params) # one could also use sim.moveToPose_init, sim.moveToPose_step and sim.moveToPose_cleanup
goalTr[2] = goalTr[2] - 0.2
params['targetPose'] = goalTr
sim.moveToPose(params)
startTr = sim.getObjectPose(targetHandle)
goalTr = sim.rotateAroundAxis(goalTr, [1, 0, 0], [startTr[0], startTr[1], startTr[2]], 90 * math.pi / 180)
params['targetPose'] = goalTr
sim.moveToPose(params)
params['targetPose'] = initTr
sim.moveToPose(params)
sim.setStepping(False)
def moveToConfig(robotColor, handles, maxVel, maxAccel, maxJerk, targetConf):
sim = sims[robotColor]
currentConf = []
for i in range(len(handles)):
currentConf.append(sim.getJointPosition(handles[i]))
params = {}
params['joints'] = handles
params['targetPos'] = targetConf
params['maxVel'] = maxVel
params['maxAccel'] = maxAccel
params['maxJerk'] = maxJerk
sim.moveToConfig(params) # one could also use sim.moveToConfig_init, sim.moveToConfig_step and sim.moveToConfig_cleanup
print('Program started')
client = RemoteAPIClient()
sim = client.require('sim')
sim.loadScene(sim.getStringParam(sim.stringparam_scenedefaultdir) + '/messaging/movementViaRemoteApi.ttt')
global sims
sims = {}
blueRobotThread = threading.Thread(target=blueRobot)
redRobotThread = threading.Thread(target=redRobot)
greenRobotThread = threading.Thread(target=greenRobot)
blueRobotThread.start()
redRobotThread.start()
greenRobotThread.start()
sim.startSimulation()
blueRobotThread.join()
redRobotThread.join()
greenRobotThread.join()
sim.stopSimulation()
print('Program ended')