-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathDelteronKinematics.nb
3330 lines (3275 loc) · 154 KB
/
DelteronKinematics.nb
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
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 10.3' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 153725, 3321]
NotebookOptionsPosition[ 151501, 3242]
NotebookOutlinePosition[ 151880, 3258]
CellTagsIndexPosition[ 151837, 3255]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell[CellGroupData[{
Cell["\<\
Analisi Cinematica del Robot Delteron
(Robot Parallelo - 3 DOF Traslazionali)\
\>", "Title",
CellChangeTimes->{{3.704616443335868*^9, 3.7046164599101086`*^9}, {
3.7046164979061384`*^9, 3.704616513191779*^9}, {3.70461657064846*^9,
3.704616572128473*^9}, {3.704616616148283*^9, 3.7046166337567945`*^9},
3.7175808188607965`*^9, {3.720103111972574*^9, 3.720103134068536*^9}, {
3.72019069671072*^9, 3.720190701062237*^9}, {3.768092052386241*^9,
3.7680920612158537`*^9}, 3.768093133441784*^9}],
Cell[CellGroupData[{
Cell["Caricamento Pacchetti", "Section",
CellChangeTimes->{{3.7046167128349466`*^9, 3.704616722442237*^9}, {
3.704617719525082*^9, 3.704617751142871*^9}, {3.718147961110123*^9,
3.718147961797623*^9}}],
Cell["Assegnamento directory:", "Text",
CellChangeTimes->{{3.715547394914977*^9, 3.715547406477128*^9}}],
Cell[BoxData[
RowBox[{
RowBox[{"SetDirectory", "[",
RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}]], "Input",
CellChangeTimes->{{3.7046177622089834`*^9, 3.7046178934782166`*^9}, {
3.704617988873596*^9, 3.7046179987269096`*^9}, {3.7155473564848237`*^9,
3.715547368730485*^9}}],
Cell["Verifica directory corrente:", "Text",
CellChangeTimes->{{3.7046180714490523`*^9, 3.7046180897133083`*^9},
3.7046184122766666`*^9}],
Cell[BoxData[
RowBox[{"Directory", "[", "]"}]], "Input",
CellChangeTimes->{{3.704618104229276*^9, 3.704618121163308*^9}, {
3.704618271932515*^9, 3.704618284885499*^9}, 3.7046183330513115`*^9,
3.704618378896676*^9}],
Cell["Caricamento pacchetti addizionali:", "Text",
CellChangeTimes->{{3.704618459081975*^9, 3.7046184702103977`*^9}}],
Cell[BoxData[
RowBox[{"Get", "[",
RowBox[{"\"\<ScrewCalculusPro.m\>\"", ",",
RowBox[{"Path", "\[Rule]",
RowBox[{"Directory", "[", "]"}]}]}], "]"}]], "Input",
CellChangeTimes->{{3.704618570987689*^9, 3.7046186116154227`*^9}, {
3.7046186791418133`*^9, 3.704618708563212*^9}}],
Cell[BoxData[
RowBox[{"Get", "[",
RowBox[{"\"\<OdeSolve.m\>\"", ",",
RowBox[{"Path", "\[Rule]",
RowBox[{"Directory", "[", "]"}]}]}], "]"}]], "Input"]
}, Closed]],
Cell[CellGroupData[{
Cell["Cinematica Diretta e Inversa", "Section",
CellChangeTimes->{{3.70461884169506*^9, 3.704618854649294*^9}}],
Cell[CellGroupData[{
Cell["Definizione Parametri Geometrici del Robot", "Subsection",
CellChangeTimes->{{3.704619789935736*^9, 3.7046197989161377`*^9}, {
3.717574721208565*^9, 3.71757472404108*^9}, {3.717594097980035*^9,
3.7175941021306314`*^9}, {3.767911444094062*^9, 3.767911464029628*^9}}],
Cell[BoxData[
RowBox[{
RowBox[{"(*", " ",
RowBox[{
"le", " ", "seguenti", " ", "quantit\[AGrave]", " ", "sono", " ",
"espresse", " ",
RowBox[{"in", " ", "[", "cm", "]"}]}], "*)"}], "\[IndentingNewLine]",
"\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"d", "=", "6"}], ";"}],
RowBox[{"(*",
RowBox[{
RowBox[{
"distanza", " ", "di", " ", "un", " ", "vertice", " ", "del", " ",
"triangolo", " ",
RowBox[{"dell", "'"}], "end"}], "-",
RowBox[{
"effector", " ", "dal", " ", "centro", " ", "dello", " ", "stesso"}]}],
"*)"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"l", "=", "24"}], ";"}],
RowBox[{"(*",
RowBox[{"distanza", " ", "delle", " ", "guide", " ", "prismatiche", " ",
RowBox[{"dall", "'"}], "asse", " ", "di", " ", "simmetria", " ", "del",
" ", "robot"}], "*)"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"a", "=", "21"}], ";"}],
RowBox[{"(*",
RowBox[{"lunghezza", " ", "link1"}], "*)"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"b", "=", "21"}], ";",
RowBox[{"(*",
RowBox[{"lunghezza", " ", "link2"}], "*)"}], "\[IndentingNewLine]",
RowBox[{"\[Delta]", "=",
RowBox[{"Pi", "/", "8"}]}], ";",
RowBox[{"(*",
RowBox[{
"angolo", " ", "di", " ", "inclinazione", " ", "asse", " ", "dei", " ",
"giunti", " ", "rotoidali"}], "*)"}], "\[IndentingNewLine]",
"\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{"dimensioni", " ", "di", " ", "un", " ", "foglio", " ",
RowBox[{"A5", ":", " ",
RowBox[{"21", " ", "\[Times]", " ",
RowBox[{"14.8", " ", "[", "cm", "]"}]}]}]}], "*)"}],
"\[IndentingNewLine]",
RowBox[{"hSheet", "=", "21"}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"wSheet", "=", "14.8"}], ";"}]}]}]], "Input",
CellChangeTimes->{{3.704619845795904*^9, 3.7046198477912045`*^9}, {
3.7046201645121236`*^9, 3.704620187363022*^9}, {3.704620223984333*^9,
3.7046202444997463`*^9}, {3.704621041487157*^9, 3.7046210499238825`*^9}, {
3.7046211021698694`*^9, 3.7046211061751003`*^9}, {3.704621183892174*^9,
3.7046211986547318`*^9}, {3.704621379830632*^9, 3.7046214269405127`*^9}, {
3.7046216398127003`*^9, 3.704621711833827*^9}, {3.7046218119252057`*^9,
3.704621878016039*^9}, {3.704623993009026*^9, 3.7046240125298977`*^9}, {
3.7046246811568785`*^9, 3.704624682729144*^9}, 3.715547251433645*^9, {
3.7155478858424873`*^9, 3.71554814934501*^9}, {3.7175707422949033`*^9,
3.7175707521495447`*^9}, {3.7175707991011133`*^9,
3.7175708224863405`*^9}, {3.717574022412221*^9, 3.7175740786965847`*^9}, {
3.71757457746379*^9, 3.7175746826699023`*^9}, {3.717577975946779*^9,
3.717578041103798*^9}, {3.717580154854761*^9, 3.717580303606902*^9}, {
3.7175805219355946`*^9, 3.717580701638466*^9}, {3.717583952529131*^9,
3.7175840253710628`*^9}, {3.7175876469408793`*^9, 3.717587662157671*^9}, {
3.7179532280804205`*^9, 3.7179532805824804`*^9}, {3.7179539385593653`*^9,
3.7179539389812503`*^9}, {3.717954019207573*^9, 3.7179540226301126`*^9},
3.717954057786247*^9, {3.7179541473484955`*^9, 3.7179541477547493`*^9},
3.7179541958623304`*^9, {3.717954266369881*^9, 3.7179542681761084`*^9}, {
3.71795430281846*^9, 3.7179543031934643`*^9}, {3.717954353470421*^9,
3.7179543539236064`*^9}, {3.717954386058119*^9, 3.717954462540864*^9}, {
3.717954624034048*^9, 3.717954663658896*^9}, {3.7179546937940497`*^9,
3.717954696544077*^9}, 3.717954772430795*^9, {3.7179554826799335`*^9,
3.7179554840861998`*^9}, {3.717955584768729*^9, 3.717955585275668*^9}, {
3.7179557967773676`*^9, 3.7179558477557716`*^9}, {3.7179559115184298`*^9,
3.717955919128448*^9}, {3.7179559713450155`*^9, 3.717955971579392*^9},
3.7179562241301565`*^9, {3.717960621108081*^9, 3.717960651689415*^9}, {
3.717961006998584*^9, 3.7179610072485886`*^9}, {3.717961464768468*^9,
3.717961465252847*^9}, {3.7179617389656725`*^9, 3.7179617405300727`*^9}, {
3.717961831957975*^9, 3.7179618324423914`*^9}, {3.7179618694443865`*^9,
3.7179618695850167`*^9}, {3.717961929441596*^9, 3.7179619322385387`*^9}, {
3.7179623001155343`*^9, 3.7179623010061684`*^9}, {3.717962349916611*^9,
3.7179623504322433`*^9}, {3.71796279262108*^9, 3.7179628081151357`*^9}, {
3.7179628387609425`*^9, 3.717962847340144*^9}, {3.717962888568347*^9,
3.7179628890058517`*^9}, 3.717962927840393*^9, {3.7179629685961685`*^9,
3.717962968955547*^9}, {3.718126828916546*^9, 3.7181268319323344`*^9}, {
3.7181497193126626`*^9, 3.7181497194689093`*^9}, 3.7181497839255857`*^9, {
3.7181786836113625`*^9, 3.7181787271232834`*^9}, {3.718178824243655*^9,
3.718178854756361*^9}, {3.7181791023693495`*^9, 3.7181791213095517`*^9}, {
3.7181791965388765`*^9, 3.718179220441242*^9}, {3.7181792715451326`*^9,
3.718179289721856*^9}, 3.7181793415768576`*^9, {3.7181793956884637`*^9,
3.7181793963603425`*^9}, {3.7181794356401215`*^9, 3.718179458229323*^9}, {
3.718179909384305*^9, 3.718179909384305*^9}, {3.718180694356785*^9,
3.7181806948411617`*^9}, 3.718180757988782*^9, {3.7181808882919617`*^9,
3.7181808886357594`*^9}, {3.71818095177999*^9, 3.718180956300188*^9}, {
3.718181028481447*^9, 3.7181810544476*^9}, 3.71818173832417*^9, {
3.7181818882052183`*^9, 3.718181891785306*^9}, {3.7181819906277585`*^9,
3.7181820587499514`*^9}, {3.7181821051773963`*^9,
3.7181821104911957`*^9}, {3.71818214801298*^9, 3.718182148388037*^9}, {
3.718182196573349*^9, 3.718182261376249*^9}, 3.718182771517669*^9, {
3.7181828052269435`*^9, 3.718182805734454*^9}, {3.718211270259056*^9,
3.718211271076628*^9}, {3.718211303163082*^9, 3.718211303835554*^9}, {
3.718211342733775*^9, 3.718211372298515*^9}, {3.718211529861727*^9,
3.7182115300518694`*^9}, {3.718212005396508*^9, 3.71821200626913*^9}, {
3.7182121144958563`*^9, 3.718212177470937*^9}, 3.7182122585266485`*^9, {
3.7201033717351522`*^9, 3.7201034128719544`*^9}, {3.7201060267717314`*^9,
3.7201060282404966`*^9}, 3.720120630713071*^9, {3.7201232105427113`*^9,
3.720123210730214*^9}, {3.7201232767003193`*^9, 3.720123307797906*^9}, {
3.720124654799051*^9, 3.720124688576375*^9}, {3.7201247301264076`*^9,
3.7201247867296433`*^9}, {3.7201248226377125`*^9,
3.7201248334620695`*^9}, {3.7201248641373615`*^9,
3.7201248891258717`*^9}, {3.7201249409665656`*^9, 3.720124942782949*^9}, {
3.7201249769213552`*^9, 3.7201249790463705`*^9}, {3.720125018724803*^9,
3.720125020923778*^9}, 3.720125089253411*^9, {3.7201286276691685`*^9,
3.720128665887329*^9}, 3.720128825319318*^9, {3.720128923030013*^9,
3.7201289255299845`*^9}, 3.720128994504773*^9, {3.7201292145114207`*^9,
3.720129217245823*^9}, {3.7201292493991117`*^9, 3.7201292511491303`*^9}, {
3.720129317058316*^9, 3.7201293208865356`*^9}, {3.7201294423781633`*^9,
3.7201294438469253`*^9}, {3.7201295658299685`*^9, 3.720129582843134*^9}, {
3.720129630173691*^9, 3.7201296377537413`*^9}, {3.720129673459996*^9,
3.7201296738193207`*^9}, {3.7201297255421596`*^9, 3.720129729332733*^9}, {
3.720129763875763*^9, 3.720129801411173*^9}, {3.7201298490230045`*^9,
3.720129886194564*^9}, 3.7201299272562523`*^9, {3.7201299938209624`*^9,
3.720130071891758*^9}, {3.7201301126047335`*^9, 3.7201301138703156`*^9}, {
3.720130171954895*^9, 3.720130186830633*^9}, 3.720130222967718*^9,
3.7201302692265577`*^9, 3.720130357722585*^9, {3.720130413769019*^9,
3.720130415519039*^9}, {3.720130493664395*^9, 3.7201304993780165`*^9}, {
3.720130575163962*^9, 3.72013059715632*^9}, {3.7201306680673494`*^9,
3.7201306770987253`*^9}, {3.720131192257682*^9, 3.7201311942889504`*^9}, {
3.720131284496112*^9, 3.720131310818842*^9}, 3.720131369797206*^9, {
3.7201314504125834`*^9, 3.7201314532934804`*^9}, 3.720131673072618*^9,
3.720131847975151*^9, {3.7201318906933517`*^9, 3.7201319099967337`*^9}, {
3.720131961942581*^9, 3.7201319621925898`*^9}, {3.720132047514728*^9,
3.7201320477178555`*^9}, {3.7201351271565237`*^9, 3.720135128874876*^9}, {
3.720135173959338*^9, 3.7201351912917747`*^9}, {3.7201352382595873`*^9,
3.7201352797554765`*^9}, {3.720135331393784*^9, 3.720135333644704*^9}, {
3.720135382222816*^9, 3.720135418636297*^9}, {3.7201355012764244`*^9,
3.72013550467936*^9}, 3.7201355565989285`*^9, {3.7201355886154785`*^9,
3.720135614783696*^9}, {3.720135705716287*^9, 3.720135707153824*^9}, {
3.720135893576185*^9, 3.7201358976961017`*^9}, {3.720136011700798*^9,
3.720136012125111*^9}, 3.720136120890083*^9, {3.72013629870926*^9,
3.7201363088394346`*^9}, 3.720139448839486*^9, {3.720177149248945*^9,
3.7201771533590374`*^9}, {3.7201797579973807`*^9, 3.720179763058188*^9}, {
3.7201798635971947`*^9, 3.7201798693150635`*^9}, {3.720179951094851*^9,
3.720179951329282*^9}, {3.7201802186591606`*^9, 3.7201802188935375`*^9}, {
3.7201802521741877`*^9, 3.7201802955025787`*^9}, {3.7201803353142796`*^9,
3.720180342006097*^9}, {3.7201804469247336`*^9, 3.720180455316495*^9}, {
3.720183421210743*^9, 3.7201834262843666`*^9}, {3.7201834799595222`*^9,
3.7201834870688734`*^9}, {3.720184832885539*^9, 3.7201848370868225`*^9}, {
3.7201849429144883`*^9, 3.7201849446332607`*^9}, {3.72018498227321*^9,
3.7201849857452993`*^9}, {3.7201853803214283`*^9, 3.720185382337073*^9}, {
3.7201856376490493`*^9, 3.7201856426248274`*^9}, {3.720185718650935*^9,
3.7201857199478283`*^9}, {3.7201884128690557`*^9,
3.7201884163596535`*^9}, {3.720188450123927*^9, 3.7201884608872447`*^9}, {
3.720188502184938*^9, 3.7201885119143286`*^9}, 3.7201885732625875`*^9, {
3.720188755871995*^9, 3.720188757542878*^9}, {3.720188947309746*^9,
3.720189100731425*^9}, {3.720189179928912*^9, 3.720189202315136*^9}, {
3.720189569435239*^9, 3.7201895697008686`*^9}, {3.7201897146585455`*^9,
3.72018974590432*^9}, {3.7201898289359007`*^9, 3.720189830857748*^9}, {
3.7201898878226705`*^9, 3.7201898963739185`*^9}, {3.720189941018135*^9,
3.7201900906036406`*^9}, {3.720190146244335*^9, 3.7201901922958736`*^9},
3.7201902285301065`*^9, 3.720190295358468*^9, {3.7201903875657873`*^9,
3.7201903876907883`*^9}, {3.7201904202502046`*^9, 3.720190420640833*^9}, {
3.7201904585305815`*^9, 3.7201904587962093`*^9}, {3.7201912594177585`*^9,
3.7201913016383643`*^9}, {3.7201914289847918`*^9,
3.7201914511563053`*^9}, {3.720201358203447*^9, 3.7202013610096846`*^9}, {
3.720201489639562*^9, 3.7202014912323465`*^9}, 3.720203249753636*^9,
3.720203542254617*^9, {3.7202035803483233`*^9, 3.720203580756407*^9},
3.7202041585118885`*^9, {3.7202042027998734`*^9, 3.720204203239069*^9}, {
3.7202048988693285`*^9, 3.720204899291212*^9}, {3.7202107363485575`*^9,
3.7202107389169397`*^9}, {3.720210769212072*^9, 3.720210819765787*^9}, {
3.720216468444911*^9, 3.7202164940497217`*^9}, {3.720281927806385*^9,
3.720281957153165*^9}, 3.720281994542614*^9, {3.720282090194948*^9,
3.7202821183858933`*^9}, {3.72028218071599*^9, 3.7202821844426374`*^9}, {
3.7202832555647326`*^9, 3.720283256019038*^9}, 3.7202834499812264`*^9,
3.720866320327871*^9, {3.7209034926181755`*^9, 3.720903498588154*^9}, {
3.720903909237928*^9, 3.7209039129793453`*^9}, {3.7246012941662984`*^9,
3.7246013179363685`*^9}, {3.7246039083187666`*^9,
3.7246039085319567`*^9}, {3.7291061909561157`*^9, 3.729106203682843*^9},
3.7354155902501125`*^9, {3.7679063970921087`*^9, 3.7679064249900475`*^9}, {
3.7679064637927275`*^9, 3.7679064857351036`*^9}, {3.767906861664832*^9,
3.7679068969895105`*^9}, 3.7679070496576605`*^9, {3.767907352185574*^9,
3.767907352373468*^9}, {3.767911437283969*^9, 3.7679114908891344`*^9}, {
3.7679471861024017`*^9, 3.7679471863952336`*^9}, {3.7679472553665447`*^9,
3.7679472646292095`*^9}, 3.7679472953155394`*^9, 3.7679473268304176`*^9, {
3.7679473757252865`*^9, 3.7679474451833124`*^9}, {3.7679528601004725`*^9,
3.7679529710838857`*^9}, {3.767954718592348*^9, 3.767954723819837*^9}, {
3.767954774261859*^9, 3.7679547883543205`*^9}, 3.767955009295497*^9, {
3.7679551083363533`*^9, 3.7679551308923874`*^9}, {3.7679555849997673`*^9,
3.76795558556644*^9}, 3.768230590920995*^9, {3.769171643935831*^9,
3.769171655590122*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell["Definizione Traiettoria dell\[CloseCurlyQuote]End-Effector", \
"Subsection",
CellChangeTimes->{{3.7175747084818373`*^9, 3.7175747152247686`*^9},
3.717594095167487*^9, {3.7175941588343983`*^9, 3.7175941685366545`*^9}}],
Cell[BoxData[{
RowBox[{
RowBox[{
RowBox[{"T", "=",
RowBox[{"Range", "[", "500", "]"}]}], ";"}],
RowBox[{"(*",
RowBox[{"lista", " ", "degli", " ", "istanti", " ", "di", " ", "tempo"}],
"*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{
"parametri", " ", "di", " ", "una", " ", "curva", " ", "di", " ",
"Lissajous"}], " ", "*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"offset", " ", "=", " ", "3"}], ";"}], " ",
RowBox[{"(*",
RowBox[{
"spessore", " ", "cornice", " ", "di", " ", "foglio", " ", "bianco"}],
"*)"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"A1", "=",
RowBox[{
RowBox[{"hSheet", "/", "2"}], "-", "offset"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"A2", "=",
RowBox[{
RowBox[{"wSheet", "/", "2"}], "-", "offset"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"\[Gamma]1", "=", "5"}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"\[Gamma]2", "=", "6"}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"zPlane", " ", "=", " ", "0"}], ";"}], " ",
RowBox[{"(*", " ",
RowBox[{
"coordinata", " ", "z", " ", "del", " ", "piano", " ", "orizzontale", " ",
"su", " ", "cui", " ", "giace", " ", "la", " ", "curva"}], "*)"}],
"\[IndentingNewLine]", "\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{
"rappresentazione", " ", "della", " ", "traiettoria", " ", "nello", " ",
"spazio", " ", "in", " ", "forma", " ", "parametrica"}],
"*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"xt", "[", "t_", "]"}], ":=",
RowBox[{"A1", "*",
RowBox[{"Cos", "[",
RowBox[{"2", "*", "Pi", "*",
RowBox[{"t", "/",
RowBox[{"Last", "[", "T", "]"}]}], "*", "\[Gamma]1"}], "]"}]}]}],
";"}], " "}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"yt", "[", "t_", "]"}], ":=",
RowBox[{"A2", "*",
RowBox[{"Sin", "[",
RowBox[{"2", "*", "Pi", "*",
RowBox[{"t", "/",
RowBox[{"Last", "[", "T", "]"}]}], "*", "\[Gamma]2"}], "]"}]}]}],
";"}], "\n",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"zt", "[", "t_", "]"}], ":=", "zPlane"}], ";"}],
"\[IndentingNewLine]", "\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{"campionamento", " ", "della", " ", "traiettoria"}],
"*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"xe", "=",
RowBox[{"Array", "[",
RowBox[{"xt", ",",
RowBox[{"Length", "[", "T", "]"}], ",",
RowBox[{"First", "[", "T", "]"}]}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"ye", "=",
RowBox[{"Array", "[",
RowBox[{"yt", ",",
RowBox[{"Length", "[", "T", "]"}], ",",
RowBox[{"First", "[", "T", "]"}]}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"ze", "=",
RowBox[{"Array", "[",
RowBox[{"zt", ",",
RowBox[{"Length", "[", "T", "]"}], ",",
RowBox[{"First", "[", "T", "]"}]}], "]"}]}], ";"}]}], "Input",
CellChangeTimes->{{3.717574798005174*^9, 3.7175748053801436`*^9}, {
3.7175748443421946`*^9, 3.7175748673261156`*^9}, {3.7175749127619867`*^9,
3.717574932584942*^9}, {3.7175749630625916`*^9, 3.7175750292086735`*^9}, {
3.717579946090192*^9, 3.7175799497452536`*^9}, 3.717580009122388*^9,
3.7175801321677113`*^9, {3.7175807367457204`*^9, 3.7175807539194436`*^9}, {
3.7175821887660275`*^9, 3.7175821961512594`*^9}, 3.717582304555684*^9, {
3.7175832576298323`*^9, 3.7175832637688017`*^9}, {3.7175835162687597`*^9,
3.7175835170656414`*^9}, {3.7175836945639887`*^9,
3.7175837070289097`*^9}, {3.717583761128835*^9, 3.717583827434718*^9}, {
3.717583927265937*^9, 3.7175839505999155`*^9}, 3.7175839994611373`*^9, {
3.7175841289233365`*^9, 3.717584179422759*^9}, {3.717584226357025*^9,
3.7175842353446245`*^9}, {3.717584979766354*^9, 3.71758498068824*^9}, {
3.717585878231065*^9, 3.7175858792859936`*^9}, {3.7175859438783107`*^9,
3.7175859660814*^9}, {3.7175860055418615`*^9, 3.717586077050455*^9}, {
3.717586204113171*^9, 3.7175862371812096`*^9}, {3.7175863267689767`*^9,
3.717586327378357*^9}, {3.717586395658413*^9, 3.7175864434867506`*^9}, {
3.7175866462971215`*^9, 3.717586706286687*^9}, {3.7175867736688876`*^9,
3.717584537600088*^9}, {3.7175847073402133`*^9, 3.7175847094136877`*^9}, {
3.7175848592499437`*^9, 3.7175849124303455`*^9}, 3.71758500882713*^9, {
3.7175850426762605`*^9, 3.717585045413355*^9}, {3.7175857838823814`*^9,
3.7175857854609985`*^9}, {3.717585821796056*^9, 3.7175858281536427`*^9}, {
3.7175861838792033`*^9, 3.717586188734514*^9}, {3.7175862259859076`*^9,
3.717586228148685*^9}, {3.717586357418152*^9, 3.717586360070413*^9}, {
3.7175863951486664`*^9, 3.717586427881503*^9}, {3.7175864610260825`*^9,
3.7175865432402325`*^9}, {3.717586585366743*^9, 3.717586588717196*^9}, {
3.7175867743178625`*^9, 3.717586794408307*^9}, {3.717587444941975*^9,
3.717587462767373*^9}, {3.7175874960669737`*^9, 3.7175875853350887`*^9},
3.7175879140897417`*^9, 3.717587965675816*^9, 3.7175880309321413`*^9,
3.717594279434106*^9, {3.717594333446311*^9, 3.717594386138562*^9}, {
3.71759609211257*^9, 3.7175961157502823`*^9}, {3.717596147591979*^9,
3.7175961484687567`*^9}, {3.717599898309224*^9, 3.71759991719543*^9}, {
3.717599965280183*^9, 3.7175999654763823`*^9}, {3.7176629956976376`*^9,
3.717663039289768*^9}, 3.7176631012822576`*^9, {3.717704741329564*^9,
3.717704742588539*^9}, {3.7177048004016156`*^9, 3.7177048027881794`*^9}, {
3.717706193356456*^9, 3.7177061950895834`*^9}, {3.717706247155391*^9,
3.7177062793711734`*^9}, 3.7177063130716095`*^9, {3.717706359177305*^9,
3.7177063616204343`*^9}, {3.7177063989782333`*^9,
3.7177064021626883`*^9}, {3.7177064509404726`*^9, 3.717706479268721*^9}, {
3.7177065118718243`*^9, 3.717706545472046*^9}, {3.717706579756098*^9,
3.7177065816153173`*^9}, 3.7179523225681*^9, {3.717952491756158*^9,
3.717952495474948*^9}, 3.7179526617868195`*^9, {3.717952706940653*^9,
3.717952715980218*^9}, {3.7179530996589003`*^9, 3.7179531020653753`*^9},
3.7179532890424957`*^9, {3.7179534914076004`*^9, 3.7179534916107264`*^9}, {
3.7179535846620483`*^9, 3.717953586708947*^9}, {3.717953856798334*^9,
3.7179538614188633`*^9}, {3.717954563808139*^9, 3.7179545892811604`*^9},
3.717955524062598*^9, {3.717955595940713*^9, 3.7179555968938503`*^9}, {
3.717956035346651*^9, 3.7179560709418154`*^9}, {3.717956227692696*^9,
3.717956228210065*^9}, {3.7179569355471992`*^9, 3.717956936015954*^9}, {
3.7179569746867075`*^9, 3.7179569748272877`*^9}, {3.717959633471734*^9,
3.717959636979941*^9}, {3.717959682313484*^9, 3.717959684426021*^9},
3.717959764135418*^9, 3.7179598146917996`*^9, 3.717960919513852*^9,
3.7179609591545753`*^9, {3.717961001731309*^9, 3.717961001996892*^9}, {
3.7179615236187625`*^9, 3.7179615324202676`*^9}, {3.7179615967715664`*^9,
3.717961702516388*^9}, 3.7179619997613525`*^9, {3.717962206955903*^9,
3.717962207252782*^9}, {3.7181400128948326`*^9, 3.71814001330996*^9}, {
3.7181430430287247`*^9, 3.718143064239955*^9}, {3.718143309690119*^9,
3.718143334438257*^9}, {3.718143373488019*^9, 3.718143384965206*^9}, {
3.718178881427683*^9, 3.718178881693306*^9}, 3.718178937346341*^9, {
3.7181789808239713`*^9, 3.7181789813083997`*^9}, {3.71817990939993*^9,
3.71817990939993*^9}, 3.7181799674866824`*^9, {3.7181807540512977`*^9,
3.7181807549106855`*^9}, 3.718180929591588*^9, {3.7181810007175055`*^9,
3.718181001342491*^9}, {3.718211424802206*^9, 3.718211425174466*^9}, {
3.7182114603140583`*^9, 3.7182115223945007`*^9}, 3.7182115795895376`*^9, {
3.7182116504411087`*^9, 3.7182116764433174`*^9}, {3.718211789771612*^9,
3.718211818850962*^9}, {3.7182119048791656`*^9, 3.718211905159361*^9}, {
3.7182122375039372`*^9, 3.718212263649234*^9}, {3.71821230984256*^9,
3.7182123101707892`*^9}, {3.7193035820321383`*^9, 3.719303586351198*^9}, {
3.720106208648089*^9, 3.720106253273095*^9}, 3.720120798994834*^9, {
3.720127716687674*^9, 3.7201277225359936`*^9}, 3.7201290049560156`*^9,
3.7201293242771916`*^9, {3.720129520410446*^9, 3.720129521019827*^9}, {
3.7201312032041793`*^9, 3.720131207163101*^9}, {3.720183369156906*^9,
3.720183371427568*^9}, {3.7201867140422344`*^9, 3.7201867160398645`*^9}, {
3.720188046507043*^9, 3.720188063136635*^9}, {3.7201906346417866`*^9,
3.7201906349230423`*^9}, {3.7201913530625277`*^9,
3.7201913574186115`*^9}, {3.720191758922907*^9, 3.720191792891841*^9}, {
3.7201918334028254`*^9, 3.7201918353132544`*^9}, {3.7201950047515173`*^9,
3.7201950334428434`*^9}, {3.7201950818918705`*^9,
3.7201950840325146`*^9}, {3.7201951180951204`*^9, 3.720195118626374*^9}, {
3.7201951536111116`*^9, 3.720195177128419*^9}, {3.720195231749281*^9,
3.720195238329764*^9}, {3.7202032565033545`*^9, 3.7202032610877733`*^9}, {
3.720203296343479*^9, 3.7202033525507774`*^9}, {3.7202037091920795`*^9,
3.7202037456913834`*^9}, {3.7202039279101915`*^9,
3.7202039464888573`*^9}, {3.720204099692763*^9, 3.7202041055827165`*^9}, {
3.7202042555416803`*^9, 3.7202042560885534`*^9}, {3.7202141011467247`*^9,
3.7202141239044423`*^9}, {3.720214154172813*^9, 3.720214154500944*^9}, {
3.7202142165458155`*^9, 3.720214219549713*^9}, {3.720214250667666*^9,
3.7202142580892467`*^9}, {3.7202143027701025`*^9, 3.7202143279262123`*^9},
3.720214442533927*^9, {3.7202144968588395`*^9, 3.7202145733396273`*^9},
3.729106386221324*^9, {3.7351330694323063`*^9, 3.7351330879354725`*^9}, {
3.7351331869373665`*^9, 3.735133190508854*^9}, {3.735133271417666*^9,
3.735133287005291*^9}, {3.7351335402388325`*^9, 3.735133550798811*^9}, {
3.735133625354515*^9, 3.7351336256543674`*^9}, {3.735133780117727*^9,
3.7351337979290657`*^9}, {3.735133921800862*^9, 3.7351339220227957`*^9}, {
3.735415372106287*^9, 3.735415383457579*^9}, {3.7679057472445107`*^9,
3.7679057517139516`*^9}, {3.7679064299921646`*^9, 3.767906438196458*^9}, {
3.7679069072795887`*^9, 3.767906928993081*^9}, {3.7679070339167166`*^9,
3.767907054769722*^9}, {3.7679114265331535`*^9, 3.767911431063559*^9},
3.7679479221170588`*^9, {3.7679507473310256`*^9, 3.7679507490680466`*^9},
3.767952192894903*^9, 3.7679522683394413`*^9, 3.767952308332387*^9, {
3.767952899633109*^9, 3.7679529217631826`*^9}, {3.767954729375912*^9,
3.767954733773399*^9}, {3.767954771448422*^9, 3.767954799854164*^9},
3.76795557063744*^9, {3.7679558215218186`*^9, 3.767955857832781*^9}, {
3.767955984906099*^9, 3.7679559851316595`*^9}, {3.767956056749593*^9,
3.7679560570168743`*^9}, {3.767956122929246*^9, 3.7679561232040787`*^9},
3.7679561833269367`*^9, {3.7679574872152753`*^9, 3.7679574887453604`*^9}, {
3.7679576102906637`*^9, 3.7679576337029085`*^9}, {3.767960128179699*^9,
3.7679602253649454`*^9}, {3.767960314964196*^9, 3.7679603453124466`*^9}, {
3.767960388801792*^9, 3.7679604065828133`*^9}, 3.7679604444440775`*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell["Definizione Funzioni e Matrici Utili", "Subsection",
CellChangeTimes->{{3.717575040489135*^9, 3.717575053065399*^9}, {
3.717575089938222*^9, 3.717575101990176*^9}, {3.7175940847142177`*^9,
3.7175940898705587`*^9}}],
Cell[CellGroupData[{
Cell["Oggetti Ausiliari", "Subsubsection",
CellChangeTimes->{{3.7175778641335297`*^9, 3.717577908713647*^9},
3.717594080323584*^9, {3.768508032214821*^9, 3.768508033884864*^9}}],
Cell[BoxData[{
RowBox[{
RowBox[{
RowBox[{
RowBox[{"ReduceMatrix", "[", "M_", "]"}], ":=",
RowBox[{"Transpose", "[",
RowBox[{"Delete", "[",
RowBox[{
RowBox[{"Transpose", "[", "M", "]"}], ",",
RowBox[{"-", "1"}]}], "]"}], "]"}]}], ";"}],
RowBox[{"(*", " ",
RowBox[{"rimuove", " ",
RowBox[{"l", "'"}], "ultima", " ", "riga", " ", "della", " ", "matrice",
" ", "M"}], " ", "*)"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"origin", "=",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], ";"}], " ",
RowBox[{"(*",
RowBox[{
"origine", " ", "del", " ", "sistema", " ", "di", " ", "riferimento", " ",
"in", " ", "coordinate", " ", "omogenee"}], "*)"}],
"\n"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"R", "=", " ",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"RotZ", "[",
RowBox[{
RowBox[{"2", "/", "3"}], "*", "Pi"}], "]"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], " ", ";"}],
RowBox[{"(*",
RowBox[{
"rotazione", " ", "che", " ", "mi", " ", "permetter\[AGrave]", " ", "di",
" ", "selezionare", " ", "ciascuna", " ", "delle", " ", "tre", " ",
"catene", " ", "cinematiche", " ", "del", " ", "robot"}], "*)"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"L", "[", "d_", "]"}], ":=", " ",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"RotX", "[", "\[Delta]", "]"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "d"}], "}"}]}], "]"}]}], ";"}], " ",
RowBox[{"(*",
RowBox[{
"rototraslazione", " ", "che", " ", "mi", " ", "permetter\[AGrave]", " ",
"di", " ", "disegnare", " ", "i", " ", "giunti", " ", "rotoidali"}],
"*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{
"sistema", " ", "di", " ", "equazioni", " ", "che", " ", "governa", " ",
"la", " ", "cinematica", " ", "del", " ", "robot"}],
"*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"system", "[",
RowBox[{
"\[Zeta]1_", ",", "\[Zeta]2_", ",", "\[Zeta]3_", ",", "\[Lambda]1_", ",",
"\[Lambda]2_", ",", "\[Lambda]3_", ",", "\[Mu]1_", ",", "\[Mu]2_", ",",
"\[Mu]3_", ",", "x_", ",", "y_", ",", "z_"}], "]"}], ":=",
RowBox[{"{", "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"{",
RowBox[{"x", ",", "y", ",", "z"}], "}"}], "\[Equal]",
RowBox[{"{",
RowBox[{
RowBox[{"(",
RowBox[{"l", "+", "\[Lambda]1", "+", "\[Mu]1"}], ")"}], ",",
RowBox[{
RowBox[{
RowBox[{"Cos", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{
SqrtBox[
RowBox[{
SuperscriptBox["a", "2"], "-",
SuperscriptBox["\[Lambda]1", "2"]}]], "-",
SqrtBox[
RowBox[{
SuperscriptBox["b", "2"], "-",
SuperscriptBox["\[Mu]1", "2"]}]]}], ")"}]}], " ", "-", "d"}],
" ", ",",
RowBox[{"\[Zeta]1", "+",
RowBox[{
RowBox[{"Tan", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{"d", "+", "y"}], ")"}]}]}]}], "}"}]}], ",",
RowBox[{
RowBox[{"{",
RowBox[{"x", ",", "y", ",", "z"}], "}"}], "\[Equal]",
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"-",
FractionBox["1", "2"]}], "*",
RowBox[{"(",
RowBox[{"l", "+", "\[Lambda]2", "+", "\[Mu]2"}], ")"}]}], "-",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*",
RowBox[{"Cos", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{
SqrtBox[
RowBox[{
SuperscriptBox["a", "2"], "-",
SuperscriptBox["\[Lambda]2", "2"]}]], "-",
SqrtBox[
RowBox[{
SuperscriptBox["b", "2"], "-",
SuperscriptBox["\[Mu]2", "2"]}]]}], ")"}]}], "+",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*", "d"}]}], " ", ",",
RowBox[{
RowBox[{
RowBox[{"+",
FractionBox[
RowBox[{
SqrtBox["3"], " "}], "2"]}], "*",
RowBox[{"(",
RowBox[{"l", "+", "\[Lambda]2", "+", "\[Mu]2"}], ")"}]}], "-",
RowBox[{
FractionBox["1", "2"], "*",
RowBox[{"Cos", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{
SqrtBox[
RowBox[{
SuperscriptBox["a", "2"], "-",
SuperscriptBox["\[Lambda]2", "2"]}]], "-",
SqrtBox[
RowBox[{
SuperscriptBox["b", "2"], "-",
SuperscriptBox["\[Mu]2", "2"]}]]}], ")"}]}], "+",
RowBox[{
FractionBox["1", "2"], "*", "d"}]}], " ", ",",
RowBox[{"\[Zeta]2", "+",
RowBox[{
RowBox[{"Tan", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{"d", "-",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*", "x"}], "-",
RowBox[{
FractionBox["1", "2"], "*", "y"}]}], ")"}]}]}]}], "}"}]}], ",",
RowBox[{
RowBox[{"{",
RowBox[{"x", ",", "y", ",", "z"}], "}"}], "\[Equal]",
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"-",
FractionBox["1", "2"]}], "*",
RowBox[{"(",
RowBox[{"l", "+", "\[Lambda]3", "+", "\[Mu]3"}], ")"}]}], "+",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*",
RowBox[{"Cos", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{
SqrtBox[
RowBox[{
SuperscriptBox["a", "2"], "-",
SuperscriptBox["\[Lambda]3", "2"]}]], "-",
SqrtBox[
RowBox[{
SuperscriptBox["b", "2"], "-",
SuperscriptBox["\[Mu]3", "2"]}]]}], " ", ")"}]}], "-",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*", "d"}]}], ",",
RowBox[{
RowBox[{
RowBox[{"-",
FractionBox[
RowBox[{
SqrtBox["3"], " "}], "2"]}], "*",
RowBox[{"(",
RowBox[{"l", "+", "\[Lambda]3", "+", "\[Mu]3"}], ")"}]}], "-",
RowBox[{
FractionBox["1", "2"], "*",
RowBox[{"Cos", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{
SqrtBox[
RowBox[{
SuperscriptBox["a", "2"], "-",
SuperscriptBox["\[Lambda]3", "2"]}]], "-",
SqrtBox[
RowBox[{
SuperscriptBox["b", "2"], "-",
SuperscriptBox["\[Mu]3", "2"]}]]}], ")"}]}], "+",
RowBox[{
FractionBox["1", "2"], "*", "d"}]}], " ", ",",
RowBox[{"\[Zeta]3", "+",
RowBox[{
RowBox[{"Tan", "[", "\[Delta]", "]"}], "*",
RowBox[{"(",
RowBox[{"d", "+",
RowBox[{
FractionBox[
SqrtBox["3"], "2"], "*", "x"}], "-",
RowBox[{
FractionBox["1", "2"], "*", "y"}]}], ")"}]}]}]}], "}"}]}]}],
"}"}]}], ";"}]}], "Input",
CellChangeTimes->{{3.7175751081311245`*^9, 3.717575159077505*^9},
3.7175752134148617`*^9, {3.7175753788346624`*^9, 3.7175754268397994`*^9}, {
3.7175754754817715`*^9, 3.717575515482402*^9}, {3.717575546091428*^9,
3.7175755756875477`*^9}, {3.7175756079207816`*^9, 3.717575630277918*^9}, {
3.7175756903797646`*^9, 3.717575708413273*^9}, {3.7175757637567434`*^9,
3.717575830872756*^9}, {3.7175758934250317`*^9, 3.717575912351015*^9}, {
3.717575945835435*^9, 3.717576135511351*^9}, {3.717576178793522*^9,
3.71757619595115*^9}, {3.7175764555590787`*^9, 3.7175764585231466`*^9}, {
3.7175768362622366`*^9, 3.7175768368559914`*^9}, 3.717582380999995*^9, {
3.717963639077422*^9, 3.717963647783747*^9}, {3.7179636894414444`*^9,
3.7179636930912867`*^9}, 3.71796381929185*^9, {3.7179641798954053`*^9,
3.717964180286034*^9}, {3.7181084300686073`*^9, 3.7181091912052884`*^9}, {
3.7181791965388765`*^9, 3.718179220488119*^9}, {3.7181792715451326`*^9,
3.71817928969061*^9}, {3.7201036180170574`*^9, 3.7201036202670784`*^9}, {
3.720103651888521*^9, 3.720103671345882*^9}, {3.7201037119732065`*^9,
3.720103839847393*^9}, {3.7201038759584475`*^9, 3.7201039105461483`*^9}, {
3.7201115137492404`*^9, 3.720111728029869*^9}, {3.720119887362857*^9,
3.7201203334705715`*^9}, {3.7201203679588785`*^9, 3.720120527673716*^9}, {
3.7201238312775564`*^9, 3.720123918264882*^9}, {3.7201241787639055`*^9,
3.720124188170556*^9}, {3.720136324962839*^9, 3.7201363554724183`*^9}, {
3.720139461810608*^9, 3.7201394634825015`*^9}, {3.720176250966983*^9,
3.7201762726379404`*^9}, {3.7201763257157335`*^9,
3.7201763261219873`*^9}, {3.7201764175281067`*^9,
3.7201764395163674`*^9}, {3.7201764756854277`*^9,
3.7201765523426704`*^9}, {3.7201767164665546`*^9,
3.7201767192478724`*^9}, {3.720176994781811*^9, 3.720177032142747*^9}, {
3.720177256265038*^9, 3.7201772714682813`*^9}, {3.7201797098481565`*^9,
3.7201797196112723`*^9}, {3.720184060241525*^9, 3.720184074570068*^9}, {
3.720184131415008*^9, 3.720184165305525*^9}, {3.720184195476755*^9,
3.7201842155516586`*^9}, {3.7201842885698028`*^9, 3.720184674304763*^9}, {
3.7201847420513334`*^9, 3.720184760900202*^9}, {3.7201849070544615`*^9,
3.7201849331174355`*^9}, {3.720185035774066*^9, 3.7201850470705247`*^9}, {
3.7201851689611425`*^9, 3.720185169473146*^9}, {3.720185260348776*^9,
3.720185362773838*^9}, {3.720185415291174*^9, 3.720185524145991*^9}, {
3.720185567227916*^9, 3.7201856099367943`*^9}, {3.720185696696512*^9,
3.7201857026026263`*^9}, {3.720186260478575*^9, 3.7201863141972375`*^9}, {
3.720186358720671*^9, 3.720186388005596*^9}, 3.720186539092401*^9, {
3.7201871333227324`*^9, 3.720187175005459*^9}, {3.720187244900875*^9,
3.7201872710495467`*^9}, {3.720189138776144*^9, 3.720189156865613*^9}, {
3.7201908172143774`*^9, 3.7201909046263914`*^9}, {3.720195440877761*^9,
3.7201954944083443`*^9}, {3.7201956363923955`*^9, 3.720195640142886*^9}, {
3.7202016829532404`*^9, 3.720201717500081*^9}, {3.7202855337274237`*^9,
3.720285536216028*^9}, {3.7679556539936447`*^9, 3.7679556551020064`*^9}, {
3.768230624226122*^9, 3.768230649596884*^9}}]
}, Closed]],
Cell[CellGroupData[{
Cell["Matrici di Trasformazione Omogenee", "Subsubsection",
CellChangeTimes->{{3.7175762088322897`*^9, 3.7175762527318745`*^9}, {
3.717577178118696*^9, 3.7175771784839754`*^9}, {3.7175940746985188`*^9,
3.7175940772610493`*^9}}],
Cell[BoxData[{
RowBox[{
RowBox[{
RowBox[{
RowBox[{"Tbh", "[", "\[Zeta]_", "]"}], ":=",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"RotX", "[", "\[Delta]", "]"}], ",",
RowBox[{"{",
RowBox[{"l", ",", "0", ",", "\[Zeta]"}], "}"}]}], "]"}]}], ";"}], " ",
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"Thc", "[", "\[Lambda]_", "]"}], ":=",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"IdentityMatrix", "[", "3", "]"}], ",",
RowBox[{"{",
RowBox[{"\[Lambda]", ",",
RowBox[{"Sqrt", "[",
RowBox[{
RowBox[{"a", "^", "2"}], "-",
RowBox[{"\[Lambda]", "^", "2"}]}], "]"}], ",", "0"}], "}"}]}],
"]"}]}], ";"}], " ", "\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"Tcv", "[", "\[Mu]_", "]"}], ":=",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"IdentityMatrix", "[", "3", "]"}], ",",
RowBox[{"{",
RowBox[{"\[Mu]", ",",
RowBox[{"-",
RowBox[{"Sqrt", "[",
RowBox[{
RowBox[{"b", "^", "2"}], "-",
RowBox[{"\[Mu]", "^", "2"}]}], "]"}]}], ",", "0"}], "}"}]}],
"]"}]}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"Tve", ":=",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"RotX", "[",
RowBox[{"-", "\[Delta]"}], "]"}], ",",
RowBox[{
RowBox[{"RotX", "[",
RowBox[{"-", "\[Delta]"}], "]"}], ".",
RowBox[{"{",
RowBox[{"0", ",",
RowBox[{"-", "d"}], ",", "0"}], "}"}]}]}], "]"}]}], ";"}], " ",
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"Tbe", "[",
RowBox[{"x_", ",", "y_", ",", "z_"}], "]"}], ":=",
RowBox[{"RPToHomogeneous", "[",
RowBox[{
RowBox[{"IdentityMatrix", "[", "3", "]"}], ",",
RowBox[{"{",
RowBox[{"x", ",", "y", ",", "z"}], "}"}]}], "]"}]}], ";"}]}], "Input",
CellChangeTimes->{{3.7175762849428735`*^9, 3.7175763157467766`*^9}, {
3.7175763547517104`*^9, 3.717576434153909*^9}, {3.717576466182445*^9,
3.717576529214995*^9}, {3.7175766247268353`*^9, 3.7175766684219446`*^9}, {
3.7175767277178617`*^9, 3.7175768013360023`*^9}, {3.717576862075164*^9,
3.7175768803364525`*^9}, 3.7175769143518343`*^9, {3.7175769927960033`*^9,
3.7175771507267036`*^9}, {3.7181791965701303`*^9, 3.718179220488119*^9}, {
3.7181792715607624`*^9, 3.7181792897062373`*^9}, {3.7181793510144835`*^9,
3.718179364530095*^9}, {3.7201112204280767`*^9, 3.7201114997993507`*^9}, {
3.720176360439474*^9, 3.7201763817323885`*^9}, {3.720176728107585*^9,
3.720176732301654*^9}, {3.720177362025738*^9, 3.7201773657166657`*^9}, {
3.720177399890169*^9, 3.720177477983283*^9}, {3.720179943171651*^9,
3.7201799442966623`*^9}, {3.720184024118945*^9, 3.720184031492941*^9},
3.720184814460866*^9, 3.7201867714623594`*^9, 3.720201737687024*^9, {
3.768091276832874*^9, 3.7680913542757053`*^9}, 3.768230654698383*^9, {
3.7685080748202915`*^9, 3.7685080752910337`*^9}}]
}, Closed]]
}, Open ]],
Cell[CellGroupData[{
Cell["Risoluzione del Sistema di Equazioni (Cinematica Diretta)", "Subsection",
CellChangeTimes->{{3.7179517065358515`*^9, 3.7179517294699235`*^9},
3.767948048077453*^9}],
Cell[BoxData[{
RowBox[{
RowBox[{
RowBox[{"step", "=",
RowBox[{"Range", "[", "8", "]"}]}], ";"}], " ",
RowBox[{"(*",
RowBox[{"lista", " ", "dei", " ", "passi"}],
"*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"amp", "=", "8"}], ";"}],
RowBox[{"(*",
RowBox[{"ampiezza", " ",
RowBox[{"dell", "'"}], "intervallo", " ", "di", " ", "scorrimento", " ",
"guida", " ", "prismatica"}], "*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"sup", "=",
RowBox[{"-", "1.5"}]}], ";"}],
RowBox[{"(*",
RowBox[{"estremo", " ", "superiore", " ",
RowBox[{"dell", "'"}], "intervallo"}], "*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"shift", "=",
RowBox[{"sup", "-",
RowBox[{"amp", "*",
RowBox[{
RowBox[{"(",
RowBox[{"step", "-", "1"}], ")"}], "/",
RowBox[{"(",
RowBox[{
RowBox[{"Last", "[", "step", "]"}], "-", "1"}], ")"}]}]}]}]}], ";"}],
RowBox[{"(*",
RowBox[{
"coordinata", " ", "di", " ", "traslazione", " ", "guida", " ",
"prismatica"}], "*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]",
RowBox[{"(*",
RowBox[{
"soluzione", " ", "del", " ", "sistema", " ", "non", " ", "lineare"}],
"*)"}]}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{
RowBox[{"DirectKinematics", "[",
RowBox[{"k1_", ",", "k2_", ",", "k3_"}], "]"}], ":=",
RowBox[{"FindRoot", "[",
RowBox[{
RowBox[{"system", "[",
RowBox[{
RowBox[{"shift", "[",
RowBox[{"[", "k1", "]"}], "]"}], ",",
RowBox[{"shift", "[",
RowBox[{"[", "k2", "]"}], "]"}], ",",
RowBox[{"shift", "[",
RowBox[{"[", "k3", "]"}], "]"}], ",", "\[Lambda]1d", ",",
"\[Lambda]2d", ",", "\[Lambda]3d", ",", "\[Mu]1d", ",", "\[Mu]2d",
",", "\[Mu]3d", ",", "xd", ",", "yd", ",", "zd"}], "]"}], ",",
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{"xd", ",", "0."}], "}"}], ",",
RowBox[{"{",
RowBox[{"yd", ",", "0."}], "}"}], ",",
RowBox[{"{",
RowBox[{"zd", ",", "0."}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Lambda]1d", ",",
RowBox[{"-", "10."}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Lambda]2d", ",",
RowBox[{"-", "10."}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Lambda]3d", ",",
RowBox[{"-", "10."}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Mu]1d", ",",
RowBox[{"-", "10."}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Mu]2d", ",",
RowBox[{"-", "10."}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"\[Mu]3d", ",",
RowBox[{"-", "10."}]}], "}"}]}], "}"}]}], "]"}]}], ";"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"rule1", "=",
RowBox[{"Map", "[",
RowBox[{
RowBox[{"Function", "[",
RowBox[{"k1", ",",
RowBox[{"Map", "[",
RowBox[{
RowBox[{"Function", "[",
RowBox[{"k2", ",",
RowBox[{"Map", "[",
RowBox[{
RowBox[{"Function", "[",
RowBox[{"k3", ",",
RowBox[{"DirectKinematics", "[",
RowBox[{"k1", ",", "k2", ",", "k3"}], "]"}]}], "]"}], ",",
"step"}], "]"}]}], "]"}], ",", "step"}], "]"}]}], "]"}], ",",
"step"}], "]"}]}], ";"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"xd", "=",
RowBox[{"Re", "[",
RowBox[{"xd", "/.", "rule1"}], "]"}]}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"yd", "=",
RowBox[{"Re", "[",
RowBox[{"yd", "/.", "rule1"}], "]"}]}], ";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"zd", "=",
RowBox[{"Re", "[",
RowBox[{"zd", "/.", "rule1"}], "]"}]}], ";"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"\[Lambda]1d", "=",
RowBox[{"Re", "[",
RowBox[{"\[Lambda]1d", "/.", "rule1"}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"\[Lambda]2d", "=",
RowBox[{"Re", "[",
RowBox[{"\[Lambda]2d", "/.", "rule1"}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{
RowBox[{"\[Lambda]3d", "=",
RowBox[{"Re", "[",
RowBox[{"\[Lambda]3d", "/.", "rule1"}], "]"}]}], ";"}],
"\[IndentingNewLine]"}], "\[IndentingNewLine]",
RowBox[{