-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtutorial2arm.py
1562 lines (1372 loc) · 60.8 KB
/
tutorial2arm.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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env ipython --matplotlib=qt
# pylint: disable=trailing-whitespace,bad-whitespace,invalid-name
# pylint: disable=anomalous-backslash-in-string,bad-continuation
# pylint: disable=multiple-statements,redefined-outer-name,global-statement
# File: tutorial2arm.py
# Auth: Gary E. Deschaines
# Date: 5 July 2015
# Prog: Tutorial on two-link planar 2R robotic arm dynamics with ODE
# and Pygame.
# Desc: Models numerical solution for robot dynamics example presented
# in section 11.6 of reference [1] listed below.
#
# The purpose of this program is to simulate the motion of a two-link
# planar 2R robotic manipulator as articulated body solids modeled with
# Open Dynamics Engine (ODE) using torques calculated by iteratively
# solving a state space representation of the dynamic equations of motion
# for the modeled system as derived by Newton-Euler recursion.
#
# Original basis for this program was obtained at
#
# https://bitbucket.org/odedevs/ode/src/master/bindings/python/demos/tutorial2.py
#
# and modifications include these changes: double pendulum system to
# planar 2R robotic arm, joints to hinge and bodies to rods, utilized
# NumPy and SciPy linear algebra functions to analytically solve the
# differential equations of motions, added integration of differential
# equations of motion using Runge-Kutta 4th order method (RK4), and
# utilized Matplotlib to create plots of data collected from the ODE
# simulation and RK4 integration.
#
# References:
#
# [1] Dr. Robert L. William II, "Robot Mechanics: Notesbook Supplement
# for ME 4290/5290 Mechanics and Control of Robotic Manipulators",
# Ohio University, Mech. Engineering, Spring 2015; web available at
# https://people.ohio.edu/williams/html/PDF/Supplement4290.pdf
#
# [2] John J. Craig, "Introduction to Robotics: Mechanics and Control",
# 3rd ed., Pearson Prentice Hall, Pearson Education, Inc., Upper
# Saddle River, NJ, 2005
#
# [3] Kevin M. Lynch and Frank C. Park, "Modern Robotics: Mechanics, Planning
# and Control", preprint ver., Cambridge University Press, May 3, 2017;
# web available at http://hades.mech.northwestern.edu/images/7/7f/MR.pdf
#
# Disclaimer:
#
# See DISCLAIMER
import sys
from math import ceil, floor, cos, sin
from locale import format_string
try:
import ode
except ImportError:
print("* Error: PyODE package required.")
sys.exit()
try:
import pygame
from pygame.locals import QUIT, KEYDOWN
except ImportError:
print("* Error: PyGame package required.")
sys.exit()
try:
from vecMath import DPR, RPD
from vecMath import vecAdd, vecSub, vecCrossP, vecDivS, vecMulS
from vecMath import vecMag, vecRotZ
from vecMath import matDotV, vecToSkewMat, transposeM
except ImportError:
print("* Error: vecMath package required.")
sys.exit()
try:
import numpy as np
import scipy.linalg as la
import matplotlib as mpl
import matplotlib.pyplot as plt
from matplotlib.lines import Line2D
except ImportError:
print("* Error: NumPy, SciPy and Matplotlib packages required.")
print(" Suggest installing the SciPy stack.")
sys.exit()
try:
from RK4_Solver import RK4_Solver
except ImportError:
print("* Error: RK4_Solver class required.")
sys.exit()
import warnings
warnings.filterwarnings("ignore", category=DeprecationWarning)
# Processing and output control flags
T_STEP = 0.001 # Simulation and integration time step size (sec)
T_STOP = 1.000 # Simulation and integration stop time (sec)
MOTOR_AVEL = False # Use motor.setParam(ode.ParamVel,...) versus motor.addTorques(...)
PRINT_FBCK = False # Controls printing of joint feedback data
PRINT_EVAL = False # Controls printing of dynamics evaluation calculations
PRINT_DATA = False # Controls printing of collected data
PLOT_DATA = True # Controls plotting of collected data
SAVE_ANIM = False # Controls saving animation images
# Drawing constants
WINDOW_RESOLUTION = (640, 480)
DRAW_SCALE = WINDOW_RESOLUTION[0] / 4
"""Factor to multiply physical coordinates by to obtain screen size in pixels"""
DRAW_OFFSET = (WINDOW_RESOLUTION[0] / 2, 300)
"""Screen coordinates (in pixels) that map to the physical origin (0, 0, 0)"""
FILL_COLOR = (255, 255, 255) # background fill color
TEXT_COLOR = ( 10, 10, 10) # text drawing color
PATH_WIDTH = 1 # width of end-effector path line
PATH_COLOR = ( 0, 0, 0) # end-effector planned path color
BOX_WIDTH = 8 # width of the line (in pixels) representing a box
BOX_COLOR = ( 0, 50, 200) # for drawing box orientations from ODE
BDY_COLOR = ( 0, 50, 200) # for drawing body positions from ODE
JNT_COLOR = ( 50, 50, 50) # for drawing joint positions from ODE
ROD_WIDTH = 4 # width of the line (in pixels) representing a rod
ROD_COLOR = (255, 0, 255) # for drawing robotic arm orientations from RK4
COM_COLOR = (225, 0, 255) # for drawing robotic arm CoM positions from RK4
PIN_COLOR = (255, 0, 0) # for drawing robotic arm pin positions from RK4
Z_VEC = (0.0,0.0,0.0) # zero vector
I_MAT = ((1.0,0.0,0.0),(0.0,1.0,0.0),(0.0,0.0,1.0)) # identity matrix
#-----------------------------------------------------------------------------
# Utility functions
def coord(Wxyz, integer=False):
"""
Converts world (Wx,Wy,Wz) coordinates to screen (xs,ys) coordinates.
Setting 'integer' to True will return integer coordinates.
"""
xs = (DRAW_OFFSET[0] + DRAW_SCALE*Wxyz[0])
ys = (DRAW_OFFSET[1] - DRAW_SCALE*Wxyz[1])
if integer:
return int(round(xs)), int(round(ys))
return xs, ys
def distAngleToXYZ(dist, ang):
"""
Converts given distance and angle (in radians) to xyz coordinate.
"""
xyz = (dist*cos(ang), dist*sin(ang), 0.0)
return xyz
from tutorial2util import printVec, printFeedback
#=============================================================================
# Planar 2R Robotic Arm System - characterization and initial conditions
"""
Diagram of the planar 2R robotic arm system.
.
. \
+Y Axis /\ . (-T2) <--{rod 2 angle
^ / \ . |
| / o----------+----------o <--{tip of rod 2 is
length of rod}--------/ / \ \ end-effector
| / / \ --{2nd rod center of mass (CoM)
| / / \
| / / --{joint 2
| / /\ /
height of rod}-----/ + <--{1st rod center of mass (CoM)
/ / /
/|/ /
/ / / \
\/| / (+T1) <--{rod 1 angle
\|/ |
------------o------------> +X Axis
^\
: --{joint 1
:
Origin of inertial reference system
Notes: (1) ODE body 1 corresponding to rod 1 is positioned wrt to the
origin and the initial angle of rod 1 is set to Theta0_1,
but the angle of ODE joint 1, which attaches body 1 to the
origin, is always initialized to zero by ODE. Consequently,
Theta0_1 must be added to the values returned by the ODE
getAngle() method for joint 1 in order to match angle T1
depicted above.
(2) ODE body 2 corresponding to rod 2 is positioned wrt to body
1 and the initial angle of rod 2 is set to Theta0_2, but the
angle of ODE joint 2, which attaches body 2 to body 1, is
always initialized to zero by ODE. Consequently, Theta0_2
must be added to the values returned by the ODE getAngle()
method for joint 2 in order to match angle T2 depicted above.
Also, the sum of Theta0_1 and the getAngle value for joint 1
must be added to the sum of Theta0_2 and the getAngle value
for joint 2 in order obtain the absolute angle from the +X
axis for joint 2 rotations.
"""
ORIGIN = (0.0, 0.0, 0.0) # origin of inertial reference frame
Z_AXIS = (0.0, 0.0, 1.0) # axis of joint and body rotations
GRAVITY = (0.0, -9.81, 0.0) # gravitational acceleration (m/sec/sec)
# Physical specifications.
ROD_LEN1 = 1.0 # length (m)
ROD_HGT1 = ROD_LEN1/2.0 # height (m)
ROD_WID1 = 0.05 # width (m)
ROD_THK1 = 0.05 # thickness (m)
ROD_VOL1 = ROD_LEN1*ROD_WID1*ROD_THK1 # volume (m^3)
ROD_DEN1 = 7806 # density (kg/m^3)
ROD_M1 = ROD_DEN1*ROD_VOL1 # mass (kg)
ROD_LSQ1 = ROD_LEN1*ROD_LEN1 # length squared (m^2)
ROD_WSQ1 = ROD_WID1*ROD_WID1 # width squared (m^2)
ROD_TSQ1 = ROD_THK1*ROD_THK1 # thickness squared (m^2)
ROD_Ixxc1 = (ROD_M1/12)*(ROD_WSQ1 + ROD_TSQ1) # Ixx moment of inertia about CoM
ROD_Iyyc1 = (ROD_M1/12)*(ROD_TSQ1 + ROD_LSQ1) # Iyy moment of inertia about CoM
ROD_Izzc1 = (ROD_M1/12)*(ROD_WSQ1 + ROD_LSQ1) # Izz moment of inertia about CoM
ROD_Izzj1 = (ROD_M1/12)*(ROD_WSQ1 + 4*ROD_LSQ1) # Izz moment of inertia about joint
ROD_Icom1 = ((ROD_Ixxc1,0,0), (0,ROD_Iyyc1,0), (0,0,ROD_Izzc1))
ROD_Icor1 = ((ROD_Ixxc1,0,0), (0,ROD_Iyyc1,0), (0,0,ROD_Izzj1))
ROD_Iinv1 = ((1/ROD_Ixxc1,0,0), (0,1/ROD_Iyyc1,0), (0,0,1/ROD_Izzj1))
ROD_LEN2 = 0.5 # length (m)
ROD_HGT2 = ROD_LEN2/2.0 # height (m)
ROD_WID2 = 0.05 # width (m)
ROD_THK2 = 0.05 # thickness (m)
ROD_VOL2 = ROD_LEN2*ROD_WID2*ROD_THK2 # volume (m^3)
ROD_DEN2 = 7806 # density (kg/m^3)
ROD_M2 = ROD_DEN2*ROD_VOL2 # mass (kg)
ROD_LSQ2 = ROD_LEN2*ROD_LEN2 # length squared (m^2)
ROD_WSQ2 = ROD_WID2*ROD_WID2 # width squared (m^2)
ROD_TSQ2 = ROD_THK2*ROD_THK2 # thickness squared (m^2)
ROD_Ixxc2 = (ROD_M2/12)*(ROD_WSQ2 + ROD_TSQ2) # Ixx moment of inertia about CoM
ROD_Iyyc2 = (ROD_M2/12)*(ROD_TSQ2 + ROD_LSQ2) # Iyy moment of inertia about CoM
ROD_Izzc2 = (ROD_M2/12)*(ROD_WSQ2 + ROD_LSQ2) # Izz moment of inertia about CoM
ROD_Izzj2 = (ROD_M2/12)*(ROD_WSQ2 + 4*ROD_LSQ2) # Izz moment of inertia about joint
ROD_Icom2 = ((ROD_Ixxc2,0,0), (0,ROD_Iyyc2,0), (0,0,ROD_Izzc2))
ROD_Icor2 = ((ROD_Ixxc2,0,0), (0,ROD_Iyyc2,0), (0,0,ROD_Izzj2))
ROD_Iinv2 = ((1/ROD_Ixxc2,0,0), (0,1/ROD_Iyyc2,0), (0,0,1/ROD_Izzj2))
# Physical configuration
Theta0_1 = 10.0*RPD # initial angle of 1st rod attached to origin
Theta0_2 = 90.0*RPD # initial angle of 2nd rod attached to 1st rod
JOINT1_ANCHOR = ORIGIN
JOINT1_AXIS = Z_AXIS
JOINT2_ANCHOR = vecAdd(JOINT1_ANCHOR,distAngleToXYZ(ROD_LEN1,Theta0_1))
JOINT2_AXIS = Z_AXIS
ROD1_POS = vecAdd(JOINT1_ANCHOR,distAngleToXYZ(ROD_HGT1,Theta0_1))
ROD2_POS = vecAdd(JOINT2_ANCHOR,distAngleToXYZ(ROD_HGT2,Theta0_1+Theta0_2))
EEFF_POS = vecAdd(JOINT2_ANCHOR,distAngleToXYZ(ROD_LEN2,Theta0_1+Theta0_2))
#-----------------------------------------------------------------------------
# System of ordinary differential equations (ode) characterizing the motion
# of planar 2R robotic arm comprised of two rods as described on pages 92-93
# of reference [1]. Specifically, the state space form of the differential
# equations of motion is given as
#
# {tau} = [M(q)]*{d(dq/dt)/dt} + {V(q,dq/dt)} + {G(q)}
#
# The state variables (theta1, theta2) and associated state space equations
# for their time derivatives (thdot1, thdot2, thddot1, thddot2) comprise
# the "State Space" model.
#
# State Space Model
# ----------------------------
# dS[1] = d(theta1)/dt
# dS[2] = d(theta2)/dt
# dS[3] = d(d(theta1)/dt)/dt
# dS[4] = d(d(theta2)/dt)/dt
#
# The dotS function incorporates the sets of first order differential
# equations of motion. A Runge-Kutta 4th order integration method is
# applied to the dotS function to develop the motion of the robotic
# arm in terms of the state variables S[] by solving for S[] at each
# time step h from t0 to tStop using the following algorithm
#
# S[](t+h) = S[](t) + ((K1[] + 2*(K2[] + K3[]) + K4[])/6)*h
#
# where
#
# K1[] = dotS(S[](t))
# K2[] = dotS(S[](t) + K1[]*h/2)
# K3[] = dotS(S[](t) + K2[]*h/2)
# K4[] = dotS(S[](t) + K3[]*h)
#
# References:
#
# [1] Dr. Robert L. William II, "Robot Mechanics: Notesbook Supplement
# for ME 4290/5290 Mechanics and Control of Robotic Manipulators",
# Ohio University, Mech. Engineering, Spring 2015; web available at
# https://people.ohio.edu/williams/html/PDF/Supplement4290.pdf
#
# [2] John J. Craig, "Introduction to Robotics: Mechanics and Control",
# 3rd ed., Pearson Prentice Hall, Pearson Education, Inc., Upper
# Saddle River, NJ, 2005
# Differential equations of motion constants.
Theta1 = Theta0_1 # initial angle of joint 1 (rad)
Theta2 = Theta0_2 # initial angle of joint 2 (rad)
tStep = T_STEP # simulation and integration time step (sec)
tStop = T_STOP # simulation and integration stop time (sec)
Xrate = np.array([0.0,0.5]) # constant commanded end-effector linear rate (m/sec)
g = vecMag(GRAVITY) # gravitational acceleration magnitude (m/sec/sec)
def calcJ(th1,th2):
"""
Calculate Jacobian (pg 79 of reference [1]).
"""
global ROD_LEN1, ROD_LEN2
L1 = ROD_LEN1
L2 = ROD_LEN2
th12 = th1 + th2
cth1 = cos(th1)
sth1 = sin(th1)
cth12 = cos(th12)
sth12 = sin(th12)
J = np.zeros((2,2),dtype=np.float)
J[0,0] = -L1*sth1 - L2*sth12
J[0,1] = -L2*sth12
J[1,0] = L1*cth1 + L2*cth12
J[1,1] = L2*cth12
return J
def calcJdot(th1,th2,thdot1,thdot2):
"""
Calculate Jacobian time derivative (pg 79 of reference [1]).
"""
global ROD_LEN1, ROD_LEN2
L1 = ROD_LEN1
L2 = ROD_LEN2
th12 = th1 + th2
thdot12 = thdot1 + thdot2
cth1 = cos(th1)
sth1 = sin(th1)
cth12 = cos(th12)
sth12 = sin(th12)
Jdot = np.zeros((2,2),dtype=np.float)
Jdot[0,0] = -L1*cth1*thdot1 - L2*cth12*thdot12
Jdot[0,1] = -L2*cth12*thdot12
Jdot[1,0] = -L1*sth1*thdot1 - L2*sth12*thdot12
Jdot[1,1] = -L2*sth12*thdot12
return Jdot
def calcAngRate(Xrate,th1,th2):
"""
Calculate angular rates from commanded linear rate and initial
joint angles (pg 79 of reference [1]). Assumes Jacobian matrix
J is square.
"""
J = calcJ(th1,th2)
if np.linalg.matrix_rank(J) == J.ndim:
dq = la.solve(J,np.transpose(Xrate))
else:
dq = la.lstsq(J,np.transpose(Xrate))
return dq
# Initial values for state variables array
nSvar = 5 # number of state variables
S = np.zeros(nSvar) # state variables
dS = np.zeros(nSvar) # state derivatives
dq = calcAngRate(Xrate,Theta1,Theta2) # joint angular rates
S[0] = 0.0 # t0
S[1] = Theta1 # joint 1 angle
S[2] = Theta2 # joint 2 angle
S[3] = dq[0] # joint 1 angular rate
S[4] = dq[1] # joint 2 angular rate
def dotS(n,S):
"""
State derivatives function.
"""
global Xrate
# Use commanded rate and J to solve for angular rates.
th1 = S[1]
th2 = S[2]
J = calcJ(th1,th2)
dq = la.solve(J,np.transpose(Xrate))
# Use angular rates, J and Jdot to solve for angular
# accelerations.
Jdot = calcJdot(th1,th2,dq[0],dq[1])
ddq = la.solve(J,np.dot(-Jdot,dq))
dS = np.zeros(n)
dS[0] = 1.0 # d(t)/dt
dS[1] = dq[0] # d(th1)/dt
dS[2] = dq[1] # d(th2)/dt
dS[3] = ddq[0] # d(thdot1)/dt
dS[4] = ddq[1] # d(thdot2)/dt
return dS
def calcStateSpace(q,dq):
"""
Calculate matrix [M(q)], vectors {V(q,dq/dt)} and {G(q)} for the
the state-space representation of a planar 2R robotic arm system
given by equations on page 93 of reference [1].
"""
global g
global ROD_M1, ROD_Izzc1, ROD_LEN1
global ROD_M2, ROD_Izzc2, ROD_LEN2
th1 = q[0]
th2 = q[1]
Dth1 = dq[0]
Dth2 = dq[1]
costh1 = cos(th1)
costh2 = cos(th2)
sinth2 = sin(th2)
costh12 = cos(th1+th2)
m1 = ROD_M1
Izz1 = ROD_Izzc1
L1 = ROD_LEN1
L1sq = L1*L1
m2 = ROD_M2
Izz2 = ROD_Izzc2
L2 = ROD_LEN2
L2sq = L2*L2
L1L2 = L1*L2
M = np.zeros((2,2),dtype=np.float)
term1 = Izz2 + m2*L2sq/4
term2 = m2*L1L2*costh2
term3 = term1 + term2/2
M[0,0] = term1 + term2 + Izz1 + (m1/4 + m2)*L1sq
M[0,1] = term3
M[1,0] = term3
M[1,1] = term1
V = np.array([0,0],dtype=np.float)
term1 = m2*L1L2*sinth2
term2 = term1/2
V[0] = -(term2*Dth2 + term1*Dth1)*Dth2
V[1] = term2*Dth1**2
G = np.array([0,0],dtype=np.float)
term1 = g*L1*costh1
term2 = g*m2*L2*costh12/2
G[0] = m1*term1/2 + m2*term1 + term2
G[1] = term2
return (M,V,G)
def calcTorque(n,S):
"""
Solves {tau} = [M(q)]*{d(dq/dt)/dt} + {V(q,dq/dt)} + {G(q)}.
"""
dS = dotS(n,S)
q = (S[1],S[2])
dq = (dS[1],dS[2])
ddq = np.array([dS[3], dS[4]],dtype=np.float)
(M,V,G) = calcStateSpace(q,dq)
tau = np.dot(M,np.transpose(ddq)) + np.transpose(V+G)
return tau
def calcAngAccel(Tq,q,dq):
"""
Solves [M(q)]*{d(dq/dt)/dt} = {tau} - {V(q,dq/dt)} - {G(q)}
for {d(dq/dt)/dt}. Assumes mass matrix M is square.
"""
tau = np.array([Tq[0],Tq[1]],dtype=np.float)
(M,V,G) = calcStateSpace(q,dq)
b = np.transpose(tau-V-G)
if np.linalg.matrix_rank(M) == M.ndim:
ddq = la.solve(M,b,sym_pos=True)
else:
ddq = la.lstsq(M,b)
return ddq
def calcLinAccel12(S,dS):
"""
Calculates linear acceleration at center of mass for rods 1 and 2
using equations (8.11) and (8.12) on page 275 of reference [3].
"""
global ROD_LEN1
global ROD_LEN2
L1 = ROD_LEN1
hL1 = L1/2
L2 = ROD_LEN2
hL2 = L2/2
th1 = S[1]
th2 = S[2]
th12 = th1 + th2
c1 = cos(th1)
s1 = sin(th1)
c12 = cos(th12)
s12 = sin(th12)
thdot1 = dS[1]
thdot2 = dS[2]
thdot12 = thdot1 + thdot2
thdot1sq = thdot1*thdot1
thdot12sq = thdot12*thdot12
thddot1 = dS[3]
thddot2 = dS[4]
thddot12 = thddot1 + thddot2
a1x = -hL1*thdot1sq*c1 - hL1*thddot1*s1
a1y = -hL1*thdot1sq*s1 + hL1*thddot1*c1
a1z = 0.0
a1 = np.array([a1x, a1y, a1z], dtype=np.float)
a2x = -L1*thdot1sq*c1 - hL2*thdot12sq*c12 - L1*thddot1*s1 - hL2*thddot12*s12
a2y = -L1*thdot1sq*s1 - hL2*thdot12sq*s12 + L1*thddot1*c1 + hL2*thddot12*c12
a2z = 0.0
a2 = np.array([a2x, a2y, a2z], dtype=np.float)
return a1, a2
def calcLinAccelEE(n,S):
"""
Calculates linear acceleration of end-effector using equations
on pages 79 of reference [1].
"""
th1 = S[1]
th2 = S[2]
dS = dotS(n,S)
thdot1 = dS[1]
thdot2 = dS[2]
J = calcJ(th1,th2)
Jdot = calcJdot(th1,th2,thdot1,thdot2)
dq = np.array([dS[1], dS[2]],dtype=np.float)
ddq = np.array([dS[3], dS[4]],dtype=np.float)
a = np.dot(J,np.transpose(ddq)) + np.dot(Jdot,np.transpose(dq))
return (a[0], a[1], 0.0)
def calcInvDynamicsRK4(jList,S,dS):
"""
Calculates inverse dynamics for RK4 model from given joint list
with associated state variables and derivatives using Recursive
Newton-Euler Algorithm. See pages 76-78 and 84 of reference [1]
and pages 177-180 of reference [2].
"""
global Z_VEC, I_MAT
global ORIGIN
global ROD_LEN1, ROD_HGT1, ROD_M1, ROD_Icom1
global ROD_LEN2, ROD_HGT2, ROD_M2, ROD_Icom2
global g
m = [0.0, ROD_M1, ROD_M2]
I = [I_MAT, ROD_Icom1, ROD_Icom2]
Ag = (0.0, g, 0.0) # gravitational acceleration offset
if PRINT_EVAL:
print('calcInvDynamicsRK4:')
# Points of interest locations in local joint frame coordinates.
# Note: Element 0 corresponds to joint/body 0, which is the
# virtual robotic base fixed at the origin of the ODE
# environment, and element 3 corresponds to the end-
# effector (a virtual joint/body) located at the tip
# of rod 2.
# joint axes locations
Pja = [ORIGIN,ORIGIN,(ROD_LEN1,0.0,0.0),(ROD_LEN2,0.0,0.0)]
# body CoM locations wrt to inboard joint axis location
Pcg = [ORIGIN,(ROD_HGT1,0.0,0.0),(ROD_HGT2,0.0,0.0),(ROD_LEN2,0.0,0.0)]
# Recursion work space variables (measures in global reference frame).
# Note: The following variable lists contain a zero scalar or vector
# (Z_VEC) in the 0th entry to simplify outbound and inbound
# recursion computations below. Also, the 0th entry in state
# variables S and dS correspond to t and dt respectively, thus
# the rotation angle and rates for joints 1 and 2 occupy the
# 1st and 2nd entries of S for angle, and the 1st/3rd and
# 2nd/4th entries of dS for angular velocity/acceleration.
rth = [0.0,0.0,0.0,0.0] # joint relative rotation angles
ath = [0.0,0.0,0.0,0.0] # joint absolute rotation angles
Pj = [Z_VEC,Z_VEC,Z_VEC] # joint position vectors
Vj = [Z_VEC,Z_VEC,Z_VEC] # joint linear velocity vectors
Aj = [Z_VEC,Z_VEC,Z_VEC] # joint linear acceleration vectors
w = [Z_VEC,Z_VEC,Z_VEC] # joint/body angular velocity vectors
a = [Z_VEC,Z_VEC,Z_VEC] # joint/body angular acceleration vectors
Pb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body position vectors
Vb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body linear velocity vectors
Ab = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body linear acceleration vectors
Fb = [Z_VEC,Z_VEC,Z_VEC] # inertial forces (in joint/body frame)
Nb = [Z_VEC,Z_VEC,Z_VEC] # inertial moments (in joint/body frame)
fb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # internal forces (in joint/body frame)
nb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # total torque at inboard joint
nbo = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # moment about body cg from outboard joint
nbi = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # nb minus nbo
tau = [0.0,0.0,0.0] # total torque on inboard joint
# Outbound joint list traversal for joint/body positions, orientations,
# velocities and accelerations, inertial forces and moments.
if PRINT_EVAL:
print('...outbound kinematics iteration.')
nj = len(jList)
kj = nj - 1
ij = 1
for j in jList[ij:nj]:
# ... ith joint rotation angles and rates
rth[ij] = S[ij]
ath[ij] = ath[ij-1] + rth[ij]
Zdth = vecMulS(Z_AXIS,dS[ij])
alpha = vecMulS(Z_AXIS,dS[ij+kj])
# ... joint and body pose positions
Pj[ij] = vecAdd(Pj[ij-1],vecRotZ(Pja[ij],-ath[ij-1]))
Pb[ij] = vecAdd(Pj[ij],vecRotZ(Pcg[ij],-ath[ij]))
# ... get vectors from last joint to this joint (D0), and
# from this joint to its body COM (C1)
D0 = vecSub(Pj[ij],Pj[ij-1])
C1 = vecSub(Pb[ij],Pj[ij])
# ... joint angular velocity and acceleration, eqns (6.45 & 6.46) Ref [2]
w[ij] = vecAdd(w[ij-1],Zdth)
a[ij] = vecAdd(a[ij-1],vecAdd(vecCrossP(w[ij-1],Zdth),alpha))
# ... joint velocity and acceleration, eqns (5.47 & 6.47) Ref [2]
Vj[ij] = vecAdd(Vj[ij-1],vecCrossP(w[ij-1],D0))
Aj[ij] = vecAdd(Aj[ij-1],\
vecAdd(vecCrossP(a[ij-1],D0),\
vecCrossP(w[ij-1],\
vecCrossP(w[ij-1],D0))))
# ... body velocity and acceleration, eqn (6.48) Ref [2]
Vb[ij] = vecAdd(Vj[ij],vecCrossP(w[ij],C1))
Ab[ij] = vecAdd(Aj[ij],\
vecAdd(vecCrossP(a[ij],C1),\
vecCrossP(w[ij],\
vecCrossP(w[ij],C1))))
# ... inertial loading (kinetics)
Fb[ij] = vecMulS(vecRotZ(vecAdd(Ab[ij],Ag),ath[ij]),m[ij])
Nb[ij] = vecAdd(matDotV(I[ij],a[ij]),\
vecCrossP(w[ij],matDotV(I[ij],w[ij])))
if PRINT_EVAL:
printVec('j'+str(ij), 'w', w[ij])
printVec('j'+str(ij), 'a', a[ij])
printVec('j'+str(ij), 'Vj', Vj[ij])
printVec('j'+str(ij), 'Aj', Aj[ij])
printVec('b'+str(ij), 'Vb', Vb[ij])
printVec('b'+str(ij), 'Wb', w[ij]) # must be same as joint w[ij]
printVec('b'+str(ij), 'Ab', Ab[ij])
printVec('b'+str(ij), 'Fb', Fb[ij])
printVec('b'+str(ij), 'Nb', Nb[ij])
ij = ij + 1
# End-effector position, velocity and acceleration.
Pb[ij] = vecAdd(Pj[ij-1],vecRotZ(Pcg[ij],-ath[ij-1]))
Vb[ij] = vecAdd(Vj[ij-1],vecCrossP(w[ij-1],vecSub(Pb[ij],Pj[ij-1])))
Ab[ij] = vecAdd(Aj[ij-1],\
vecAdd(vecCrossP(a[ij-1],\
vecSub(Pb[ij],Pj[ij-1])),\
vecCrossP(w[ij-1],\
vecCrossP(w[ij-1],\
vecSub(Pb[ij],Pj[ij-1])))))
if PRINT_EVAL:
printVec('ee', 'Vb', Vb[ij])
printVec('ee', 'Ab', Ab[ij])
# Inbound iteration over reverse of joint list.
if PRINT_EVAL:
print('...inbound kinetics iteration.')
jList.reverse()
ij = kj
for j in jList[0:kj]:
# calculate fb in local joint/body frames, but display in global
# reference frame coordinates for comparision to feedback forces.
fb[ij] = vecAdd(vecRotZ(fb[ij+1],-rth[ij+1]),Fb[ij])
nb[ij] = vecAdd(vecRotZ(nb[ij+1],-rth[ij+1]),\
vecAdd(vecCrossP(Pcg[ij],Fb[ij]),\
vecAdd(vecCrossP(Pja[ij+1],\
vecRotZ(fb[ij+1],-rth[ij+1])),\
Nb[ij])))
tau[ij] = nb[ij][2]
# these inboard (nbi) and outboard (nbo) moments are for comparison
# to feedback torque fb[1] for joint[ij] and fb[3] for joint[ij+1].
nbo[ij] = vecAdd(vecRotZ(nb[ij+1],-rth[ij+1]),\
vecCrossP(vecSub(Pcg[ij],Pja[ij]),\
vecRotZ(fb[ij+1],-rth[ij+1])))
nbi[ij] = vecSub(nb[ij],nbo[ij])
nbo[ij] = vecSub(nbo[ij],nbi[ij+1])
if PRINT_EVAL:
printVec('j'+str(ij), 'fb', vecRotZ(fb[ij],-ath[ij]))
printVec('j'+str(ij), 'nb', nb[ij])
printVec('j'+str(ij), 'nbi', nbi[ij])
printVec('j'+str(ij), 'nbo', nbo[ij])
ij = ij - 1
if PRINT_EVAL:
print('...return.')
return (Pj,rth,ath,w,a,Pb,Vb,Ab,(tau[1],tau[2]))
def calcFwdDynamicsODE(jList):
"""
Calculates forward dynamics for ODE model from given joint list
using Articulated-Body Algorithm. Uses forces and torques from
joint feedback data to compute linear and angular accelerations
which are not provided by ODE. See pages 76-78 and 84 of ref [1]
and pages 177-180 of ref [2].
"""
global body1, body2, j2
global Theta0_1, Theta0_2
global I_MAT, ROD_HGT2
global g
th0 = [0.0,Theta0_1,Theta0_2] # initial joint angles
Ag = (0.0, g, 0.0) # gravitational acceleration offset
if PRINT_EVAL:
print('calcFwdDynamicsODE:')
# Recursion work space variables (measures in global reference frame).
# Note: The following variable lists contain a zero scalar or vector
# (Z_VEC) in the 0th entry to simplify outbound and inbound
# recursion computations below.
rth = [0.0,0.0,0.0,0.0] # joint relative rotation angles
ath = [0.0,0.0,0.0,0.0] # joint absolute rotation angles
Pj = [Z_VEC,Z_VEC,Z_VEC] # joint position vectors
Vj = [Z_VEC,Z_VEC,Z_VEC] # joint linear velocity vectors
Aj = [Z_VEC,Z_VEC,Z_VEC] # joint linear acceleration vectors
w = [Z_VEC,Z_VEC,Z_VEC] # joint/body angular velocity vectors
a = [Z_VEC,Z_VEC,Z_VEC] # joint/body angular acceleration vectors
Pb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body position vectors
Vb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body linear velocity vectors
Ab = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body linear acceleration vectors
Wb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # body angular velocity vectors
Fb = [Z_VEC,Z_VEC,Z_VEC] # inertial forces (in joint/body frame)
Nb = [Z_VEC,Z_VEC,Z_VEC] # inertial moments (in joint/body frame)
fb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # internal forces (in joint/body frame)
nb = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # total torque at inboard joint
nbo = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # moment about body cg from force at outboard joint
nbi = [Z_VEC,Z_VEC,Z_VEC,Z_VEC] # moment about inboard joint from force at body cg
tau = [0.0,0.0,0.0] # total torque on inboard joint
qdd = [Z_VEC,Z_VEC,Z_VEC] # inboard joint's generalized angular acceleration
# Outbound kinematics iteration.
if PRINT_EVAL:
print('...outbound kinematics iteration.')
nj = len(jList)
kj = nj - 1
ij = 1
for j in jList[ij:nj]:
# joint body mass, moments of inertia and rotation matrices
b = j.getBody(0)
bM = b.getMass()
m = bM.mass
I = bM.I
# ... joint rotation angle and rate
rth[ij] = th0[ij] + j.getAngle() # add initial offset (see diagram notes)
ath[ij] = ath[ij-1] + rth[ij]
Zdth = vecMulS(j.getAxis(),j.getAngleRate())
# ... joint and body pose positions
Pj[ij] = j.getAnchor()
Pb[ij] = b.getPosition()
# ... get vectors from last joint to this joint (D0), and
# from this joint to its body COM (C1)
D0 = vecSub(Pj[ij],Pj[ij-1])
C1 = vecSub(Pb[ij],Pj[ij])
# ... joint inertial angular velocity
w[ij] = vecAdd(w[ij-1],Zdth)
# ... joint inertial linear velocity
Vj[ij] = vecAdd(Vj[ij-1],vecCrossP(w[ij-1],D0))
# ... joint inertial linear acceleration
Aj[ij] = vecAdd(Aj[ij-1],\
vecAdd(vecCrossP(a[ij-1],D0),\
vecCrossP(w[ij-1],vecCrossP(w[ij-1],D0))))
# ... joint inertial angular acceleration (from joint feedback) by
# solving Ftot = m*(a[ij]xC1 + w[ij]x(w[ij]xC1) + Aj[ij]) for
# a[ij]
fbck1 = j.getFeedback() # for this joint's force on body
if PRINT_FBCK: printVec('j'+str(ij), 'fbck1[0]', fbck1[0])
if ij < kj:
# account for next joint's force on body
fbck2 = jList[ij+1].getFeedback()
if PRINT_FBCK: printVec('j'+str(ij), 'fbck2[2]', fbck2[2])
fbtot = vecAdd(fbck1[0],fbck2[2])
else:
fbtot = fbck1[0]
if PRINT_FBCK: printVec('j'+str(ij), 'fbtot', fbtot)
# note: remove gravitational offset applied by ODE
a[ij] = vecSub(vecSub(vecDivS(fbtot,m),Ag),\
vecAdd(vecCrossP(w[ij],vecCrossP(w[ij],C1)),Aj[ij]))
a[ij] = matDotV(la.pinv(transposeM(vecToSkewMat(C1))),a[ij])
qdd[ij] = vecSub(a[ij],a[ij-1])
# ... body inertial linear and angular velocity
Vb[ij] = b.getLinearVel()
Wb[ij] = b.getAngularVel()
# ... body inertial linear acceleration
Ab[ij] = vecAdd(Aj[ij],\
vecAdd(vecCrossP(a[ij],C1),\
vecCrossP(w[ij],vecCrossP(w[ij],C1))))
# ... inertial loading (kinetics)
Fb[ij] = vecMulS(vecAdd(Ab[ij],Ag),m) # reapply gravitational offset
Nb[ij] = vecAdd(matDotV(I,a[ij]),\
vecCrossP(w[ij],matDotV(I,w[ij])))
if PRINT_EVAL:
printVec('j'+str(ij), 'w', w[ij])
printVec('j'+str(ij), 'a', a[ij])
printVec('j'+str(ij), 'qdd', qdd[ij])
printVec('j'+str(ij), 'Vj', Vj[ij])
printVec('j'+str(ij), 'Aj', Aj[ij])
printVec('b'+str(ij), 'Vb', Vb[ij])
printVec('b'+str(ij), 'Wb', Wb[ij]) # must be same as joint w[ij]
printVec('b'+str(ij), 'Ab', Ab[ij])
printVec('b'+str(ij), 'Fb', Fb[ij])
printVec('b'+str(ij), 'Nb', Nb[ij])
ij = ij + 1
# End-effector position and velocity.
Pb[ij] = jList[nj-1].getBody(0).getRelPointPos((ROD_HGT2,0.0,0.0))
Vb[ij] = jList[nj-1].getBody(0).getRelPointVel((ROD_HGT2,0.0,0.0))
Ab[ij] = vecAdd(Aj[ij-1],\
vecAdd(vecCrossP(a[ij-1],\
vecSub(Pb[ij],Pj[ij-1])),\
vecCrossP(w[ij-1],\
vecCrossP(w[ij-1],\
vecSub(Pb[ij],Pj[ij-1])))))
if PRINT_EVAL:
printVec('ee', 'Vb', Vb[ij])
printVec('ee', 'Ab', Ab[ij])
# Inbound articulated body inertia and bias force recursion.
if PRINT_EVAL:
print('...inbound kinetics iteration.')
jList.reverse()
ij = kj
for j in jList[0:kj]:
# joint body mass, moments of inertia and rotation matrices
b = j.getBody(0)
bM = b.getMass()
m = bM.mass
I = bM.I
# body CoM and joint relative location vectors in global frame
C1 = vecSub(Pb[ij],Pj[ij])
# these inboard (nbi) and outboard (nbo) moments are for comparison
# to feedback torque fb[1] for joint[ij] and fb[3] for joint[ij+1]
if ij < kj: # contributions from inboard and outboard joints
D0 = vecSub(Pj[ij+1],Pj[ij])
fj = vecAdd(fb[ij+1],Fb[ij])
mj = vecAdd(vecAdd(vecAdd(Nb[ij],nb[ij+1]),\
vecCrossP(C1,Fb[ij])),\
vecCrossP(D0,fb[ij+1]))
nbo[ij] = vecCrossP(vecSub(Pb[ij],Pj[ij+1]),fb[ij+1])
nbi[ij] = vecAdd(mj,vecSub(nbo[ij],nbi[ij+1]))
else: # contributions from inboard joint only
fj = Fb[ij]
mj = vecAdd(Nb[ij],vecCrossP(C1,Fb[ij]))
nbi[ij] = vecCrossP(C1,Fb[ij])
nbo[ij] = Z_VEC
fb[ij] = fj
nb[ij] = mj
tau[ij] = nb[ij][2]
if PRINT_EVAL:
printVec('j'+str(ij), 'fb', fb[ij])
printVec('j'+str(ij), 'nb', nb[ij])
printVec('j'+str(ij), 'nbi', nbi[ij])
printVec('j'+str(ij), 'nbo', nbo[ij])
ij = ij - 1
if PRINT_EVAL:
print('...return.')
return (Pj,rth,ath,w,a,Pb,Vb,Wb,Ab,(qdd[1][2],qdd[2][2]),(tau[1],tau[2]))
#=============================================================================
# Initialize pygame
pygame.init()
# Open a display
screen = pygame.display.set_mode(WINDOW_RESOLUTION)
# Create an ODE world object
world = ode.World()
world.setGravity(GRAVITY)
world.setERP(1.0)
world.setCFM(1.0E-6)
# Create fixed base for robotic arm
body0 = ode.Body(world)
mass0 = ode.Mass()
mass0.setSphereTotal(50.0,ROD_WID1)
body0.setMass(mass0)
body0.setPosition(ORIGIN)
# Create two bodies in rest position (i.e., joint/motor angles at zero)
body1 = ode.Body(world)
mass1 = ode.Mass()
mass1.setBoxTotal(ROD_M1, ROD_LEN1, ROD_WID1, ROD_THK1)
body1.setMass(mass1)
body1.setPosition(ROD1_POS)
body1.setQuaternion((cos(Theta0_1/2),0,0,sin(Theta0_1/2)))
body2 = ode.Body(world)
mass2 = ode.Mass()
mass2.setBoxTotal(ROD_M2, ROD_LEN2, ROD_WID2, ROD_THK2)
body2.setMass(mass2)
body2.setPosition(ROD2_POS)
body2.setQuaternion((cos((Theta0_1+Theta0_2)/2),0,0,sin((Theta0_1+Theta0_2)/2)))
# Connect fixed base to environment
j0 = ode.FixedJoint(world)
j0.attach(body0,ode.environment)
j0.setFixed()
# Connect body1 with fixed base
j1 = ode.HingeJoint(world)
j1.attach(body1, body0)
j1.setAnchor(JOINT1_ANCHOR)
j1.setAxis(JOINT1_AXIS)
j1.setFeedback(True)
# Connect body2 with body1
j2 = ode.HingeJoint(world)
j2.attach(body2, body1)
j2.setAnchor(JOINT2_ANCHOR)
j2.setAxis(JOINT2_AXIS)
j2.setFeedback(True)
# Add axial motors to joints
am1 = ode.AMotor(world)
am1.attach(j1.getBody(0), j1.getBody(1))
am1.setMode(ode.AMotorUser)
am1.setNumAxes(1)
am1.setAxis(0, 1, JOINT1_AXIS)
am1.setAngle(0,Theta0_1)
am1.setFeedback(True)
if MOTOR_AVEL: am1.setParam(ode.ParamFMax,5000.0)
am2 = ode.AMotor(world)
am2.attach(j2.getBody(0), j2.getBody(1))
am2.setMode(ode.AMotorUser)
am2.setNumAxes(1)
am2.setAxis(0, 1, JOINT2_AXIS)
am2.setAngle(0,Theta0_2)
am2.setFeedback(True)
if MOTOR_AVEL: am2.setParam(ode.ParamFMax,1000.0)
# Define pygame circle radius for drawing each joint sphere
cir_rad = int(BOX_WIDTH)
# Define end-effector path line end points
path0 = (EEFF_POS[0], EEFF_POS[1], 0.0)
path1 = (EEFF_POS[0]+T_STOP*Xrate[0], EEFF_POS[1]+T_STOP*Xrate[1], 0.0)
# Create background for text and clearing screen
background = pygame.Surface(screen.get_size())
background = background.convert()
background.fill(FILL_COLOR)
# Write title
if pygame.font:
title = "PyODE Tutorial 2 - Planar 2R Robotic Arm Sim"
font = pygame.font.Font(None, 30)
text = font.render(title, 1, TEXT_COLOR)
textpos = text.get_rect(centerx=background.get_width()/2)
background.blit(text, textpos)
font = pygame.font.Font(None, 24)
# Clear screen
screen.blit(background, (0, 0))
# Simulation loop...
FPS = 50
F_TIME = 1.0/FPS
N_STEP = int(floor((F_TIME + 0.5*T_STEP)/T_STEP))
N_TIME = N_STEP*T_STEP
if __name__ == "__main__":
# Create simulation data collection arrays for plotting.
nSamples = int(ceil(T_STOP/N_TIME)) + 1
if PLOT_DATA:
Time = np.zeros(nSamples) # simulation time
EEXPode = np.zeros(nSamples) # end-effector X position from ODE
EEYPode = np.zeros(nSamples) # end-effector Y position from ODE