From 1ee59c5679834dac3bdcd42fa575612d74964402 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 12 Oct 2023 10:27:44 +0200
Subject: [PATCH] main.py in manipulator_viz does inverse kinematics and it
 works. calibration is off, but thats fixable. there is also rendering, but
 its way too slow for real-time running. it should be either a separate
 thread, updating slowly, or a implemented differently. adding some ros
 bindings and doing rviz is one option, but hopefully there is something
 simpler too

---
 .../__pycache__/make_run.cpython-310.pyc      | Bin 0 -> 1487 bytes
 .../__pycache__/make_run.cpython-311.pyc      | Bin 0 -> 3068 bytes
 manipulator_viz/arms/another_debug_dh         |   9 +
 manipulator_viz/arms/j2n6s300_dh_params       |   6 +
 manipulator_viz/arms/j2s7s300_dh_params       |   7 +
 manipulator_viz/arms/kinova_jaco_params       |   7 +
 manipulator_viz/arms/kuka_lbw_iiwa_dh_params  |   7 +
 manipulator_viz/arms/lbr_iiwa_jos_jedan_dh    |   7 +
 manipulator_viz/arms/robot_parameters2        |   9 +
 manipulator_viz/arms/testing_dh_parameters    |   6 +
 .../arms/ur10e_dh_parameters_from_the_ur_site |   6 +
 manipulator_viz/arms/ur5e_dh                  |   6 +
 manipulator_viz/ellipse.py                    |  56 ++
 manipulator_viz/embedding_in_tk_sgskip.py     | 196 ++++++
 manipulator_viz/main.py                       |  71 +++
 manipulator_viz/make_run.py                   |  45 ++
 .../manipulator_visual_motion_analyzer.py     | 601 ++++++++++++++++++
 .../robot_stuff/InverseKinematics.py          | 204 ++++++
 .../InverseKinematics.cpython-310.pyc         | Bin 0 -> 4522 bytes
 .../InverseKinematics.cpython-311.pyc         | Bin 0 -> 7195 bytes
 .../__pycache__/drawing.cpython-310.pyc       | Bin 0 -> 1424 bytes
 .../__pycache__/drawing.cpython-311.pyc       | Bin 0 -> 2272 bytes
 .../drawing_for_anim.cpython-310.pyc          | Bin 0 -> 1220 bytes
 .../drawing_for_anim.cpython-311.pyc          | Bin 0 -> 2032 bytes
 .../__pycache__/follow_curve.cpython-310.pyc  | Bin 0 -> 2363 bytes
 .../__pycache__/follow_curve.cpython-311.pyc  | Bin 0 -> 4189 bytes
 .../__pycache__/forw_kinm.cpython-310.pyc     | Bin 0 -> 11436 bytes
 .../__pycache__/forw_kinm.cpython-311.pyc     | Bin 0 -> 27512 bytes
 .../__pycache__/inv_kinm.cpython-310.pyc      | Bin 0 -> 6565 bytes
 .../__pycache__/inv_kinm.cpython-311.pyc      | Bin 0 -> 14346 bytes
 .../joint_as_hom_mat.cpython-310.pyc          | Bin 0 -> 3429 bytes
 .../joint_as_hom_mat.cpython-311.pyc          | Bin 0 -> 7553 bytes
 .../__pycache__/utils.cpython-310.pyc         | Bin 0 -> 1462 bytes
 .../__pycache__/utils.cpython-311.pyc         | Bin 0 -> 2594 bytes
 .../webots_api_helper_funs.cpython-310.pyc    | Bin 0 -> 4103 bytes
 .../webots_api_helper_funs.cpython-311.pyc    | Bin 0 -> 11565 bytes
 manipulator_viz/robot_stuff/drawing.py        |  36 ++
 .../robot_stuff/drawing_for_anim.py           |  38 ++
 manipulator_viz/robot_stuff/follow_curve.py   | 102 +++
 manipulator_viz/robot_stuff/forw_kinm.py      | 540 ++++++++++++++++
 manipulator_viz/robot_stuff/inv_kinm.py       | 434 +++++++++++++
 .../robot_stuff/joint_as_hom_mat.py           | 146 +++++
 manipulator_viz/robot_stuff/utils.py          |  36 ++
 .../robot_stuff/webots_api_helper_funs.py     | 191 ++++++
 manipulator_viz/run_classic_ik_algs.py        |  57 ++
 simple_raw_comm/.main.cpp.swp                 | Bin 0 -> 20480 bytes
 46 files changed, 2823 insertions(+)
 create mode 100644 manipulator_viz/__pycache__/make_run.cpython-310.pyc
 create mode 100644 manipulator_viz/__pycache__/make_run.cpython-311.pyc
 create mode 100644 manipulator_viz/arms/another_debug_dh
 create mode 100644 manipulator_viz/arms/j2n6s300_dh_params
 create mode 100644 manipulator_viz/arms/j2s7s300_dh_params
 create mode 100644 manipulator_viz/arms/kinova_jaco_params
 create mode 100644 manipulator_viz/arms/kuka_lbw_iiwa_dh_params
 create mode 100644 manipulator_viz/arms/lbr_iiwa_jos_jedan_dh
 create mode 100644 manipulator_viz/arms/robot_parameters2
 create mode 100644 manipulator_viz/arms/testing_dh_parameters
 create mode 100644 manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site
 create mode 100644 manipulator_viz/arms/ur5e_dh
 create mode 100644 manipulator_viz/ellipse.py
 create mode 100644 manipulator_viz/embedding_in_tk_sgskip.py
 create mode 100644 manipulator_viz/main.py
 create mode 100644 manipulator_viz/make_run.py
 create mode 100644 manipulator_viz/manipulator_visual_motion_analyzer.py
 create mode 100644 manipulator_viz/robot_stuff/InverseKinematics.py
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
 create mode 100644 manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
 create mode 100644 manipulator_viz/robot_stuff/drawing.py
 create mode 100644 manipulator_viz/robot_stuff/drawing_for_anim.py
 create mode 100644 manipulator_viz/robot_stuff/follow_curve.py
 create mode 100644 manipulator_viz/robot_stuff/forw_kinm.py
 create mode 100644 manipulator_viz/robot_stuff/inv_kinm.py
 create mode 100644 manipulator_viz/robot_stuff/joint_as_hom_mat.py
 create mode 100644 manipulator_viz/robot_stuff/utils.py
 create mode 100644 manipulator_viz/robot_stuff/webots_api_helper_funs.py
 create mode 100644 manipulator_viz/run_classic_ik_algs.py
 create mode 100644 simple_raw_comm/.main.cpp.swp

diff --git a/manipulator_viz/__pycache__/make_run.cpython-310.pyc b/manipulator_viz/__pycache__/make_run.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..666063b969d262a0388b6791d5e746142929737c
GIT binary patch
literal 1487
zcmd1j<>g{vU|@)xtCs4|!NBks#6iYP3=9ko3=9m#77PpwDGVu$ISjdsQH+crHd78$
z2!v*a(kx(_IfpfhErlh8wS^&yJ%u@#L6hwz$V^SfTOyu$WvNBQsot4+skw<InaRbj
zd1ac6w-~kjl374%pqQV5fdS+&XOPu%7#SE!7#1+rFx4<-G1V|jGGsBQF)d`oEtbNF
zkWXPuVN7F&sYa1uLa2htFJw$%p2JbgQo>Th)XeC@5ZhnNTEhUdw}vr`wVA1wt%h*{
zTMb(ZOAYfvW=4?P*i%?j*jnJ~vN&8Ani&@|*0R^I*RZ6pOET23+A!3x)v%;+K-n;#
zaMrL1Fx0T5aH5HG!R*Z9%HkGasO2bOSin=lkj1-zuZCkG$ZXCU&Ki~!E{Hy`3_nOF
zhPjrjmb;b*q`roGA!7|o3O7Ve4NnbA3J;XcUBkUVU?GDfLoF{@uOQq$A&@U67#1?t
z^40Lwu%z%pb--LKRKt?O2N5f@sbL9b(B$`f$;`mO@RE&zfkBhw7E@vIEw;k=l>Cz7
zTf(`Cd6@<AsX00EshR1id1Z+?NP?LK@x^5+@kJn&ECunY#kZKsif{3zWEPhc$Cu>C
zr{^c;+~S28k(rm0nw(mk4oM*&?2{QGEHDX5rXVK-gVKu-0|P@kLk&YLYb|39V-~|i
zrb4D*hLsGOjJH@zGEz$tHJNTP=^5N&&C1WrD=Fe*U|;|fzryu1@^e%5)ANf9@>3Fv
zz&s-aH>Wf&MZcgZu_CirzqBa6I5W2(CpA7fKd+=HKSv+z%F>*~lKi6hvdk*|+{EnE
z_@dH0y@JYHTsAqG#U;u4xdnEzAQyt1E5uMG4N?<Snx|)zlb@WJQ*5V)P*(&B%p!J>
z+qgjlFNoj(5u6}`2Sk8!h^Fu@ro4h%tW~K+`Ng-`it>}dfti<*pLUD2C^0WR^%e^_
z7`Y(s4K7J7xW$@SRFqhGiwztA#kV-J5|i_jG86M|v1cVF$Cngk-eL^7#Zp{YRC0?g
zCo?ZGC;b+4N@~e1=HjxHTWpC%>A9JCw^;J>i*j!<7sRJ(^55cu218D2(Ji*j?D*8Y
zvRmwV@tGy5Ma8$c!LEu2#bCuPmXySj#9NG+V87jB14l^lEk@s4+~5#|s$z`4#gUSk
zm<~!Gx0s9aOKu4k=O*Ulq!yRJ6VfdnP(p#F6L1tl2v7pJ#SRTzP)HS<F)%QQFpDtq
zF!C|-F!C^RFmf=m{ovsiV&r3#VFHOrFi9~AG4e19F$#hCQj9!|VvH<|MW&EMEer8O
zaY<=fnjS_eRs@RCTf)d{Qi>AGGxO4Ki69GR=9R@~XXfSJ5=0g)Ey>I&zQvkXnp;p=
rBm#<Akfjj6z*A=t*jF4jx%nxjIjMG_)L#tp8xI4>GaQT@OgwA=DtnVD

literal 0
HcmV?d00001

diff --git a/manipulator_viz/__pycache__/make_run.cpython-311.pyc b/manipulator_viz/__pycache__/make_run.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b6e66b210515ea6eaaccbc1a3ece27817db58f85
GIT binary patch
literal 3068
zcmZ3^%ge>Uz`zhWS1r|_gMr~Ohy%k+P{wBq1_p-d3@HpLj5!Rsj8Tk?AU0DDQwW4+
zhSDrxnmLCxiY<kug&~SPg*li(ll3LY7){1oBA$6=sYS)9-kEu+xrrs2$;GaDWtxn)
z7`6P8SwL!_7~~*k1_p-D7OWs^OBkWz3=9leaCQw-4PzF}febawl5j;?AiZE-8q+dH
z28PveJ+wC|g^^f0Qy5bi)0l~K9|@*15o<qjHlwOaVV=WL%TmG#4qPx%gBHRr3=<e*
z57e?UG1M^Nik}+BELN~)1_p*@ObiUG;jviDR>Qb}4JL<7*RZ9q)G#k&W?)zik6lIv
zl=x(asZU{T!D)9E2TYX<!!ia2hSe}G1FBDJ*=yKqSW?&!sj7z6hJk^hhOLGrg&ji_
zS4xD3R1I4d3j;$9O9}_kdbwau!xfHMa1~kHFts3CYB^BCg$E{A0%L&k5ig9hfDgt(
zrfWD*!vmF9%UQ!&!;->@?iU<3@FSZHW24&;!_2@?%T>!=%Y)tZHQcCfs9{OrLbtz$
zr-mhk8$*=4hI@ek%wA*~H3cB_)$-!-i6F9W7#nB&2qCF}r}!!^2K2PXSHoAslEQ<q
zADY&1r7DEEEGfL`x+XC89I9anX3*sGdkM-aFG2Z9lj9asVeu`t!uXW@lHyy!xruq1
z1@WmlIq|8P>8W{Ti8)AunFaC1WhwDRAeAfy@u|hPn97Q8@up-JmlVgB<j1GyC+6JZ
zg&2{Umy(*CTAU8?=?D8{cv;K9zyPvd7*vkSfUA#mh8l)g&RWJA#w>_(1_p+SOg&t|
z3@aHl8E>(cWTciPYBJqo(lfZlnw6iKS5gcr859(Lh3jYJ=cekX=NA{`rz94Ec}56s
zPHA3>enC-UMP{*nX;FM}W^O@FYJ75jUP)1Yjy~9-r8$Wu`9<+%nN|9^iP@>~MWuOq
z1(m<JY;rP-OOo?*3+$?-K@vfwd3rWE`N@en#ddlKNm&L4hGHoO28ITP3sPYaw8C_T
z=^Sea`+`*1XON$YK*dWDDAyN(GFTBP<rjgnX%Q%g7J;%_5hy=u3g2SNE4am4m0FZv
ze2c9pKM5Szc`5m6w^)l3^U_mqv4De_3*wpJlGK7*tcgWMiIun5AVG7BBP%gEKPfXY
z?-qMjVsd;*QRXeikXtOpg+(Q|*m5%S5_8gTF{h-K++r>+OS#3CSd^ZdnRkmNFTW`F
z7IQ&-swV#}E@&9#q!!&`%gl~X%`3abo)@24l3G-JiyQ2!cu)*i++s;dEJ?h@m<jgV
zEjDme72jg?y~Pa<UZ^U@_*)z)nThG3^mB{3D8J;EU~z6@PEKlZ2|S_Q;sGTjSUM^O
zg^&U&C=z90V7SE&jgBHq1_lODm=(J-fx`bQ8v_qtkHv)W8EJEJS9o8Mv$!H>dqKqR
zBDeh&Zu`6Zf;|;8#22V7(cQp(MakxhlH&ylr;Gf~SNNUp%E-;}SdhM=a6!yP8KWyQ
zMsR@*iVNZ|%9vh}F}=&n-xD#Rd;#NxoQu55S9q20a`S>MOPt_ykz4T!x8hwfi76Q~
z$}ftkUlCJB;d~I}<YD{3z{$hb;PO?BflqjXd1qWt+y@3>R?CkdqJ!~<p!5W(8E$iY
zS7=|6G26kmhx?+S%N0SF4z?Q{eAhW7FL6lDP+Oq9p!y<*@f8l^3mnEz1%;<bKoZ#o
zw=KR0w6B=>T`>v0AQg5|D(s3>*hRtcD}v!2Y&Rt2I#@bFdblPScJkhkQR-mni0I*)
zP<W9;@_~fh9Nq=lS0s$CNLcSkyC~s;q>@Ld$9IPI6&|%KJbD|%w#e?NzGCQo#W47a
zVdzDk&<;kB57Xx+&B>gfJ1cjE<3%~WD{^`l#q_U;>34A7P}b{U>4?9=Av+`SB8S3V
zY1uiJ^KEC@t`NK^t$jsW`yz)#2V;lDgy5%|+H1r&NL<vkx}s^-!E%9D=?aJP0>_IS
zYBw}>Q28<{3nG@sFN(jYYIa4{?23%}1rEu(;*wJ;r`JuYTadOScZ2s8HHRx|E(gkv
zR9{pJx+oreMLf8J=Z3OAvRiIQDa_Gb!F@%_<cgF<2TMm-56^_aPX12*8{GU2-bLaJ
z3=Enaw`3t{ytt$^Elm%jby@@}z-|d6t4S$JEYHkKza@e!n3-1=pPiYPdrJ^mw6r8M
zr}!3YUTJPYWsw8}0|PiE7lEn}u=U_%{)@vVH$SB`C)KXVo`Hb@RC*RCFfcHDU}j`w
tyuo010Tta~P`m&`HyD&Iz|ai_`3o@gfI;X241M5HWEA+ofJuN|007Nrs%HQI

literal 0
HcmV?d00001

diff --git a/manipulator_viz/arms/another_debug_dh b/manipulator_viz/arms/another_debug_dh
new file mode 100644
index 0000000..ab4e0ac
--- /dev/null
+++ b/manipulator_viz/arms/another_debug_dh
@@ -0,0 +1,9 @@
+0.0;0.0;1.0;0.0
+0.0;0.0;0.5;0.0
+0.0;0.0;1.0;0.0
+0.0;0.0;0.2;0.0
+0.0;0.0;1.0;0.0
+0.0;0.0;0.2;0.0
+0.0;0.0;1.0;0.0
+0.0;0.0;1.0;0.0
+0.0;0.0;1.0;0.0
diff --git a/manipulator_viz/arms/j2n6s300_dh_params b/manipulator_viz/arms/j2n6s300_dh_params
new file mode 100644
index 0000000..44eed8e
--- /dev/null
+++ b/manipulator_viz/arms/j2n6s300_dh_params
@@ -0,0 +1,6 @@
+0.2766;0.0;0.0;1.5707966
+0.0;0.0;0.41;3.14159265
+-0.0098;0.0;0.0;1.57079632
+-0.25008;0.0;0.0;1.04719755
+-0.085563;0.0;0.0;1.04719755
+-0.202781;0.0;0.0;3.1415926
diff --git a/manipulator_viz/arms/j2s7s300_dh_params b/manipulator_viz/arms/j2s7s300_dh_params
new file mode 100644
index 0000000..7d1b3fa
--- /dev/null
+++ b/manipulator_viz/arms/j2s7s300_dh_params
@@ -0,0 +1,7 @@
+-0.2755;0.0;0.0;1.5708
+0.0;0.0;0.0;1.5708
+-0.41;0.0;0.0;1.5708
+-0.0098;0.0;0.0;1.5708
+-0.31110;0.0;0.0;1.5708
+0.0;0.0;0.0;1.5708
+-0.20380;0.0;0.0;3.14159
diff --git a/manipulator_viz/arms/kinova_jaco_params b/manipulator_viz/arms/kinova_jaco_params
new file mode 100644
index 0000000..57b26c9
--- /dev/null
+++ b/manipulator_viz/arms/kinova_jaco_params
@@ -0,0 +1,7 @@
+-0.2755;;0;1.5707963
+0;;0;1.5707963
+-0.4100;;0;1.5707963
+-0.0098;;0;1.5707963
+-0.3111;;0;1.5707963
+0;;0;1.5707963
+-0.2638;;0;3.1415926
diff --git a/manipulator_viz/arms/kuka_lbw_iiwa_dh_params b/manipulator_viz/arms/kuka_lbw_iiwa_dh_params
new file mode 100644
index 0000000..3411eae
--- /dev/null
+++ b/manipulator_viz/arms/kuka_lbw_iiwa_dh_params
@@ -0,0 +1,7 @@
+0.36;0.0;0.0;-1.5708
+0.0;0.0;0.0;1.5708
+0.42;0.0;0.0;1.5708
+0.0;0.0;0.0;-1.5708
+0.40;0.0;0.0;-1.5708
+0.0;0.0;0.0;1.5708
+0.126;0.0;0.0;0.0
diff --git a/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh b/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh
new file mode 100644
index 0000000..69b01f6
--- /dev/null
+++ b/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh
@@ -0,0 +1,7 @@
+0.34;0.0;0.0;-1.5708
+0.0;0.0;0.0;1.5708
+0.40;0.0;0.0;1.5708
+0.0;0.0;0.0;-1.5708
+0.40;0.0;0.0;-1.5708
+0.0;0.0;0.0;1.5708
+0.125;0.0;0.0;0.0
diff --git a/manipulator_viz/arms/robot_parameters2 b/manipulator_viz/arms/robot_parameters2
new file mode 100644
index 0000000..6487a20
--- /dev/null
+++ b/manipulator_viz/arms/robot_parameters2
@@ -0,0 +1,9 @@
+.20;.02;.37;.00
+.00;.55;.91;.17
+.05;.39;.76;.00
+-.10;.10;.48;-.17
+.05;.19;.58;.00
+-.10;.10;.50;-.17
+.05;.19;.60;.00
+-.10;.10;.52;-.17
+.05;.19;.61999;.00
diff --git a/manipulator_viz/arms/testing_dh_parameters b/manipulator_viz/arms/testing_dh_parameters
new file mode 100644
index 0000000..8c60750
--- /dev/null
+++ b/manipulator_viz/arms/testing_dh_parameters
@@ -0,0 +1,6 @@
+0.1273;0.0;0.0;1.5708
+0.0;0.0;-0.612;0.0
+0.0;0.0;-0.5723;0.0
+0.1639;0.0;0.0;1.5708
+0.1157;0.0;0.0;-1.5708
+0.0922;0.0;0.0;0.0
diff --git a/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site b/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site
new file mode 100644
index 0000000..991315d
--- /dev/null
+++ b/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site
@@ -0,0 +1,6 @@
+0.1807;0.0;0;1.5707963
+0.0;0.0;-0.6127;0.0
+0.0;0.0;-0.57155;0.0
+0.17415;0.0;0.0;1.5707963
+0.11985;0.0;0.0;-1.5707963
+0.11655;0.0;0.0;0.0
diff --git a/manipulator_viz/arms/ur5e_dh b/manipulator_viz/arms/ur5e_dh
new file mode 100644
index 0000000..09af722
--- /dev/null
+++ b/manipulator_viz/arms/ur5e_dh
@@ -0,0 +1,6 @@
+0.163;0.0;0;1.5707963
+0.0;0.0;-0.4250;0.0
+0.0;0.0;-0.39225;0.0
+0.134;0.0;0.0;1.5707963
+0.100;0.0;0.0;-1.5707963
+0.100;0.0;0.0;0.0
diff --git a/manipulator_viz/ellipse.py b/manipulator_viz/ellipse.py
new file mode 100644
index 0000000..9cacb93
--- /dev/null
+++ b/manipulator_viz/ellipse.py
@@ -0,0 +1,56 @@
+import numpy as np 
+from numpy import linalg 
+from mpl_toolkits.mplot3d import Axes3D 
+import matplotlib.pyplot as plt 
+from matplotlib import cm 
+import matplotlib.colors as colors 
+fig = plt.figure(figsize=(8,8)) 
+ax = fig.add_subplot(111, projection='3d') # number of ellipsoids  
+ellipNumber = 1
+#set colour map so each ellipsoid as a unique colour 
+norm = colors.Normalize(vmin=0, vmax=ellipNumber) 
+cmap = cm.jet 
+m = cm.ScalarMappable(norm=norm, cmap=cmap) #compute each and plot each ellipsoid iteratively 
+indx = 10
+A = np.array([[np.random.random_sample(),0,0],
+            [0,np.random.random_sample(),0],
+            [0,0,np.random.random_sample()]])
+center = [indx*np.random.random_sample(),indx*np.random.random_sample(),indx*np.random.random_sample()]
+# find the rotation matrix and radii of the axes     
+U, s, rotation = linalg.svd(A)     
+radii = 1.0/np.sqrt(s) * 0.3 #reduce radii by factor 0.3
+# calculate cartesian coordinates for the ellipsoid surface     
+u = np.linspace(0.0, 2.0 * np.pi, 60)     
+v = np.linspace(0.0, np.pi, 60)     
+x = radii[0] * np.outer(np.cos(u), np.sin(v))     
+y = radii[1] * np.outer(np.sin(u), np.sin(v))     
+z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+for i in range(len(x)):         
+    for j in range(len(x)):
+        [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + center     
+ellipse = ax.plot_surface(x, y, z,  rstride=3, cstride=3,
+                color=m.to_rgba(indx), linewidth=0.1,
+                alpha=0.3, shade=True) 
+
+# repeat jsut to udapte
+indx = 10
+A = np.array([[np.random.random_sample(),0,0],
+            [0,np.random.random_sample(),0],
+            [0,0,np.random.random_sample()]])
+center = [indx*np.random.random_sample(),indx*np.random.random_sample(),indx*np.random.random_sample()]
+# find the rotation matrix and radii of the axes     
+U, s, rotation = linalg.svd(A)     
+radii = 1.0/np.sqrt(s) * 0.3 #reduce radii by factor 0.3
+# calculate cartesian coordinates for the ellipsoid surface     
+u = np.linspace(0.0, 2.0 * np.pi, 60)     
+v = np.linspace(0.0, np.pi, 60)     
+x = radii[0] * np.outer(np.cos(u), np.sin(v))     
+y = radii[1] * np.outer(np.sin(u), np.sin(v))     
+z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+for i in range(len(x)):         
+    for j in range(len(x)):
+        [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + center     
+
+ellipse.set_verts(x,y)
+
+plt.show()
diff --git a/manipulator_viz/embedding_in_tk_sgskip.py b/manipulator_viz/embedding_in_tk_sgskip.py
new file mode 100644
index 0000000..1c1abd9
--- /dev/null
+++ b/manipulator_viz/embedding_in_tk_sgskip.py
@@ -0,0 +1,196 @@
+#import tkinter
+# override ugly tk widgets with system-based ttk ones
+# change the import * thing if it's better for your ide
+from tkinter import * 
+from tkinter.ttk import *
+
+from matplotlib.backends.backend_tkagg import (
+    FigureCanvasTkAgg, NavigationToolbar2Tk)
+import matplotlib.pyplot as plt
+# Implement the default Matplotlib key bindings.
+from matplotlib.backend_bases import key_press_handler
+from matplotlib.figure import Figure
+
+from robot_stuff.InverseKinematics import InverseKinematicsEnv
+from robot_stuff.drawing import *
+from robot_stuff.inv_kinm import *
+
+import numpy as np
+
+
+def policy(robot, desired_goal):
+#    del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal)
+#    del_thet = invKinm_Jac_T(robot, desired_goal)
+#    del_thet = invKinm_PseudoInv(robot, desired_goal)
+#    del_thet = invKinm_dampedSquares(robot, desired_goal)
+    del_thet = invKinm_PseudoInv_half(robot, desired_goal)
+#    del_thet = invKinmQP(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidE_kM(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidManipMax(robot, desired_goal)
+
+    return del_thet
+
+# TODO move all tkinter stuff to one file,
+# and all plot stuff to one file to avoid this schizo mess
+root = Tk()
+root.wm_title("Embedding in Tk")
+
+# mock data 'cos i was lazy with cleaning the rest
+start = 0
+end = 10
+n_pts = 2000
+dt = (end - start) / n_pts
+time = np.linspace(start,end,n_pts)
+
+angular_v_x = np.sin(time/2)
+angular_v_y = np.cos(time/3)
+angular_v_z = np.sin(time/5)
+v_x = time / 2
+v_y = time **2 / (time + 2)
+v_z = np.cos(time / 10)
+
+# integrate velocities to get positions
+angular_p_x = np.cumsum(angular_v_x * dt)
+angular_p_y = np.cumsum(angular_v_y * dt)
+angular_p_z = np.cumsum(angular_v_z * dt)
+p_x = np.cumsum(v_x * dt)
+p_y = np.cumsum(v_y * dt)
+p_z = np.cumsum(v_z * dt)
+
+
+# when you plot something on the axis, it returns you the object of that plot
+# you then update this later on
+# this is much much faster than replotting everything every time (even without blitting,
+# which can be implemented if necessary)
+
+# the functions to update these plot objects can be found on matplotlib pages by just googling
+
+
+
+# the whole figure
+fig = Figure()
+
+# init robot stuff
+ik_env = InverseKinematicsEnv()
+# no way this flies, but we have to try
+ik_env.ax = fig.add_subplot(121, projection='3d') 
+# these are for axes scaling which does not happen automatically
+ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b')
+ik_env.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b')
+# NOTE hope this applies just to this ax, otherwise change to ax.xlim or whatever
+ik_env.ax.set_xlim([-1.5,1.5])
+ik_env.ax.set_ylim([-0.5,0.5])
+color_link = 'black'
+ik_env.robot.initDrawing(ik_env.ax, color_link)
+
+# do the run
+
+
+# TODO may use this as trajectory later?
+#position_line, = ax.plot(p_x, p_y, p_z, color='blue')
+## the quive sucks for this purpose as the tangets stick to the curve and you don't see anything
+##mask = np.arange(n_pts) % 200 == 0
+##ax.quiver(p_x[mask], p_y[mask], p_z[mask], v_x[mask], v_y[mask], v_z[mask], color='orange')
+## there's no colormap over a line, so just plot colored line segments (many lines)
+#for i in range(n_pts - 1):
+#    ax.plot(p_x[i:i+2], p_y[i:i+2], p_z[i:i+2], color=plt.cm.plasma((i)/n_pts))
+#fig.colorbar(plt.cm.ScalarMappable(norm=None, cmap=plt.cm.plasma), orientation='horizontal')
+#
+## needs to be a line 'cos that's what we can update
+#point_in_time_position_line, = ax.plot(np.zeros(1), np.zeros(1),np.zeros(1),
+#                                       marker='.', markersize=12, linestyle='None', color='red')
+
+
+
+# whatever side bullshit
+ax = fig.add_subplot(322) 
+ax.set_xlabel('t/s')
+ang_v_x_line, = ax.plot(time, angular_v_x)
+point_in_time_angular_v_x_line, = ax.plot(np.zeros(1), np.zeros(1), 
+                                          marker='.', markersize=12, linestyle='None', color='red')
+
+ax = fig.add_subplot(324) 
+ax.set_xlabel('t/s')
+ang_v_z_line, = ax.plot(time, angular_v_y)
+point_in_time_angular_v_y_line, = ax.plot(np.zeros(1), np.zeros(1), 
+                                          marker='.', markersize=12, linestyle='None', color='red')
+
+ax = fig.add_subplot(326) 
+ax.set_xlabel('t/s')
+ang_v_y_line, = ax.plot(time, angular_v_z)
+point_in_time_angular_v_z_line, = ax.plot(np.zeros(1), np.zeros(1), 
+                                          marker='.', markersize=12, linestyle='None', color='red')
+
+
+
+
+
+canvas = FigureCanvasTkAgg(fig, master=root) 
+canvas.draw()
+
+# pack_toolbar=False will make it easier to use a layout manager later on.
+toolbar = NavigationToolbar2Tk(canvas, root, pack_toolbar=False)
+toolbar.update()
+
+canvas.mpl_connect(
+    "key_press_event", lambda event: print(f"you pressed {event.key}"))
+canvas.mpl_connect("key_press_event", key_press_handler)
+
+button_quit = Button(master=root, text="Quit", command=root.destroy)
+
+
+# new_val is what the widget gives you
+# you define what needs to happen to plots in Figure below,
+# and call canvas.draw
+def update_point(new_val):
+    new_val = int(np.floor(float(new_val)))
+    
+    # all these are in lists as that's what the line plot wants, 
+    # but we use single points
+#    point_in_time_position_line.set_data_3d([p_x[new_val]], [p_y[new_val]],  [p_z[new_val]])
+    point_in_time_angular_v_x_line.set_data([time[new_val]], [angular_v_x[new_val]])
+    point_in_time_angular_v_y_line.set_data([time[new_val]], [angular_v_y[new_val]])
+    point_in_time_angular_v_z_line.set_data([time[new_val]], [angular_v_z[new_val]])
+
+    # required to update canvas and attached toolbar!
+    canvas.draw()
+
+slider_update = Scale(root, from_=0, to=n_pts - 1, length=500, orient=HORIZONTAL,
+                              command=update_point)#, label="Frequency [Hz]")
+
+# TODO no way in hell this flies, but we have to try
+# does not update main frame until it finishes
+def updateIKAnim():
+    # do the step
+    for i in range(50):
+        action = policy(ik_env.robot, ik_env.goal)
+        action_fix = list(action)
+        action_fix.append(1.0)
+        action_fix = np.array(action_fix)
+        obs, reward, done, info = ik_env.step(action_fix)
+        # animate
+        ik_env.robot.drawStateAnim()
+        ik_env.ax.set_title(str(ik_env.n_of_tries_for_point) + 'th iteration toward goal')
+        #ik_env.ax.set_title(str(np.random.random()) + 'th iteration toward goal')
+        drawPoint(ik_env.ax, ik_env.goal, 'red')
+        canvas.draw()
+        root.update()
+
+button_anim = Button(master=root, text="anim_step", command=updateIKAnim)
+
+
+
+# Packing order is important. Widgets are processed sequentially and if there
+# is no space left, because the window is too small, they are not displayed.
+# The canvas is rather flexible in its size, so we pack it last which makes
+# sure the UI controls are displayed as long as possible.
+# NOTE: imo it's fine to just pack now and work on the layout later once
+# everything we want to plot is known
+button_anim.pack(side=BOTTOM)
+button_quit.pack(side=BOTTOM)
+slider_update.pack(side=BOTTOM)
+toolbar.pack(side=BOTTOM, fill=X)
+canvas.get_tk_widget().pack(side=TOP, fill=BOTH, expand=True)
+
+mainloop()
diff --git a/manipulator_viz/main.py b/manipulator_viz/main.py
new file mode 100644
index 0000000..ff53d75
--- /dev/null
+++ b/manipulator_viz/main.py
@@ -0,0 +1,71 @@
+# idc if i need this
+# Implement the default Matplotlib key bindings.
+from robot_stuff.InverseKinematics import InverseKinematicsEnv
+from robot_stuff.drawing import *
+from robot_stuff.inv_kinm import *
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+
+rtde_c = RTDEControl("192.168.1.102")
+
+rtde_r = RTDEReceive("192.168.1.102")
+q = rtde_r.getActualQ()
+
+# Parameters
+acceleration = 0.5
+dt = 1.0/500  # 2ms
+
+ik_env = InverseKinematicsEnv()
+ik_env.damping = 25
+ik_env.goal[0] = 0.21
+ik_env.goal[1] = 0.38
+ik_env.goal[2] = 0.5
+#ik_env.render()
+
+
+ik_env.robot.setJoints(q)
+ik_env.robot.calcJacobian()
+print(ik_env.robot.p_e)
+#print(type(init_q))
+#exit()
+
+# putting it into this class so that python remembers it 'cos reasons, whatever
+controller = invKinm_dampedSquares
+#controller = invKinmSingAvoidanceWithQP_kI
+#while True:
+while True:
+    q = rtde_r.getActualQ()
+    ik_env.robot.setJoints(q)
+    ik_env.robot.calcJacobian()
+    q_dots = controller(ik_env.robot, ik_env.goal)
+    thetas = np.array([joint.theta for joint in ik_env.robot.joints])
+#    ik_env.render()
+    #print(ik_env.robot.p_e.copy())
+    #print(ik_env.goal)
+    distance = np.linalg.norm(ik_env.robot.p_e.copy() - ik_env.goal)
+    print(distance)
+    if distance < 0.01:
+        t_start = rtde_c.initPeriod()
+        t_start = rtde_c.initPeriod()
+        rtde_c.speedJ(np.zeros(6), acceleration, dt)
+        rtde_c.waitPeriod(t_start)
+        break
+
+#    print(q_dots)
+#    print(thetas)
+    
+    t_start = rtde_c.initPeriod()
+    rtde_c.speedJ(q_dots, acceleration, dt)
+    rtde_c.waitPeriod(t_start)
+
+for i in range(20):
+    t_start = rtde_c.initPeriod()
+    rtde_c.speedJ(np.zeros(6), acceleration, dt)
+    rtde_c.waitPeriod(t_start)
+
+
+print("done")
diff --git a/manipulator_viz/make_run.py b/manipulator_viz/make_run.py
new file mode 100644
index 0000000..240c7fd
--- /dev/null
+++ b/manipulator_viz/make_run.py
@@ -0,0 +1,45 @@
+from robot_stuff.InverseKinematics import InverseKinematicsEnv
+from robot_stuff.drawing import *
+from robot_stuff.inv_kinm import *
+from robot_stuff.utils import *
+
+import numpy as np
+
+# starting off with random goals, who cares rn
+# policy is one of the inverse kinematics control algoritms
+# TODO (optional): make it follow a predefined trajectory
+# TODO (optional): vectorize it if it isn't fast enough (at least you avoid dynamic allocations)
+def makeRun(controller, ik_env, n_iters, robot_index):
+    # we'll shove everything into lists, and have it ready made
+    data = {
+    "qs" : np.zeros((n_iters, ik_env.robots[robot_index].ndof)),
+    "q_dots" : np.zeros((n_iters, ik_env.robots[robot_index].ndof)),
+    "manip_ell_eigenvals" : np.zeros((n_iters, 3)) ,
+    "manip_elip_svd_rots" : np.zeros((n_iters, 3, 3)),
+    "p_es" : np.zeros((n_iters, 3)),
+    "vs" : np.zeros((n_iters, 6)), # all links linear velocities # NOT USED ATM
+    "dists_to_goal" : np.zeros(n_iters),
+    "manip_indeces" : np.zeros(n_iters),
+    }
+
+    for i in range(n_iters):
+        q_dots = controller(ik_env.robots[robot_index], ik_env.goal)
+        ik_env.simpleStep(q_dots, 1.0, robot_index)
+
+        thetas = np.array([joint.theta for joint in ik_env.robots[robot_index].joints])
+        data['qs'][i] = thetas
+        data['q_dots'][i] = q_dots
+        # NOTE: this is probably not correct, but who cares
+        data['vs'][i] = ik_env.robots[robot_index].jacobian @ q_dots
+        M = ik_env.robots[robot_index].jac_tri @ ik_env.robots[robot_index].jac_tri.T
+        manip_index = np.sqrt(np.linalg.det(M))
+        data['manip_indeces'][i] = manip_index
+        _, diag_svd, rot = np.linalg.svd(M)
+        # TODO check this: idk if this or the square roots are eigenvalues 
+        data["manip_ell_eigenvals"][i] = np.sqrt(diag_svd)
+        data["manip_elip_svd_rots"][i] = rot
+        smallest_eigenval = diag_svd[diag_svd.argmin()]
+        dist_to_goal = np.linalg.norm(ik_env.robots[robot_index].p_e - ik_env.goal)
+        data['dists_to_goal'][i] = dist_to_goal
+        data['p_es'][i] = ik_env.robots[robot_index].p_e
+    return data
diff --git a/manipulator_viz/manipulator_visual_motion_analyzer.py b/manipulator_viz/manipulator_visual_motion_analyzer.py
new file mode 100644
index 0000000..d15e61d
--- /dev/null
+++ b/manipulator_viz/manipulator_visual_motion_analyzer.py
@@ -0,0 +1,601 @@
+# override ugly tk widgets with system-based ttk ones
+# change the import * thing if it's better for your ide
+from tkinter import * 
+from tkinter.ttk import *
+
+from matplotlib.backends.backend_tkagg import (
+    FigureCanvasTkAgg, NavigationToolbar2Tk)
+import matplotlib.pyplot as plt
+# Implement the default Matplotlib key bindings.
+from matplotlib.backend_bases import key_press_handler
+from matplotlib.figure import Figure
+import pyautogui
+from matplotlib.gridspec import GridSpec
+
+from robot_stuff.InverseKinematics import InverseKinematicsEnv
+from robot_stuff.drawing import *
+from robot_stuff.inv_kinm import *
+from make_run import makeRun
+
+import numpy as np
+import sys
+
+sys.path.append('../geometry_matplotlib')
+#from common import exit_on_x, make_rotation_matrix3D
+#from plot3D import plot3D_camera, plot3D, set_equal_axis3D, camera_data_for_plot3D
+
+DPI = 80
+SLIDER_SIZE = 200
+#DPI = 200
+
+# TODO: call update_point function with the current slider value after updating 
+# stuff like eliipse on/off so that you don't need to move the button to get it
+# just call the slider string value to get the current slider value
+# TODO: finish writing up reseting to the same starting position (joint values)
+# TODO: readjust figure size so that it is not so javla small
+
+
+#######################################################################
+#                            CONTROL STUFF                            #
+#######################################################################
+
+
+def getController(controller_name):
+    
+    if controller_name == "invKinmQPSingAvoidE_kI":
+        return invKinmQPSingAvoidE_kI
+    if controller_name == "invKinm_Jac_T":
+        return invKinm_Jac_T
+    if controller_name == "invKinm_PseudoInv":
+        return invKinm_PseudoInv
+    if controller_name == "invKinm_dampedSquares":
+        return invKinm_dampedSquares
+    if controller_name == "invKinm_PseudoInv_half":
+        return invKinm_PseudoInv_half
+    if controller_name == "invKinmQP":
+        return invKinmQP
+    if controller_name == "invKinmQPSingAvoidE_kI":
+        return invKinmQPSingAvoidE_kI
+    if controller_name == "invKinmQPSingAvoidE_kM":
+        return invKinmQPSingAvoidE_kM
+    if controller_name == "invKinmQPSingAvoidManipMax":
+        return invKinmQPSingAvoidManipMax
+
+    return invKinm_dampedSquares
+
+
+root = Tk()
+root.wm_title("Embedding in Tk")
+
+n_iters = 200
+time = np.arange(n_iters)
+
+# do the run
+ik_env = InverseKinematicsEnv()
+ik_env.robots.append(Robot_raw(robot_name="no_sim"))
+ik_env.damping = 25
+# putting it into this class so that python remembers it 'cos reasons, whatever
+controller1 = getController("")
+controller2 = getController("invKinm_Jac_T")
+ik_env.data.append(makeRun(controller1, ik_env, n_iters, 0))
+ik_env.data.append(makeRun(controller2, ik_env, n_iters, 1))
+
+screensize = pyautogui.size()
+SCREEN_WIDTH = screensize.width
+SCREEN_HEIGHT = screensize.height
+SCALING_FACTOR_HEIGHT = 0.8
+SCALING_FACTOR_WIDTH = 0.5
+
+#######################################################################
+#                             YARA'S PLOT                             #
+#######################################################################
+
+fig_ee = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+#exit_on_x(fig_ee)
+#gs = GridSpec(3, 3, figure=fig_manip_graphs)
+
+# Instantiate 3D reconstruction plot
+#import scipy.io 
+#data = scipy.io.loadmat('../geometry_matplotlib/data/orebro_castle')
+##viewR = make_rotation_matrix3D(0, 0, 0)
+#recX, recP = data['U'], data['P'][0]
+#scale = recX.std() / 10
+#cam_indices = np.arange(0,recP.shape[0],4) # np.random.permutation(recP.shape[0])[:200]
+#rec3D_ax = fig.add_subplot(gs[:,0], projection='3d') 
+rec3D_ax = fig_ee.add_subplot(projection='3d') 
+rec3D_ax.set(xticklabels=[], yticklabels=[], zticklabels=[])
+#X_indices = np.arange(0,recX.shape[1],15) #np.random.permutation(recX.shape[1])
+#num_pts_subset = 25
+#for P_ in recP[cam_indices]:
+#    camera_center_ = plot3D_camera(P_, ax=rec3D_ax, scale=scale, only_center=True)
+#    camera_center_.set(markersize=1)
+#camera_center, camera_coordinate_system = plot3D_camera(recP[0], ax=rec3D_ax, scale=scale)
+#rec_points3D = plot3D(recX[:,X_indices[:num_pts_subset]], ax=rec3D_ax, title='')[0][0]
+#set_equal_axis3D(rec3D_ax)
+#rec3D_ax.view_init(elev=0, azim=0, roll=0)
+#rec3D_ax.set_title('3D Reconstruction')
+###########################################################################
+
+#######################################################################
+#                          MANIPULATOR PLOT                           #
+#######################################################################
+
+# robot plot
+#ik_env.ax = fig.add_subplot(132, projection='3d') 
+#ik_env.ax = fig.add_subplot(gs[:,1], projection='3d') 
+fig_manipulator = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ik_env.ax = fig_manipulator.add_subplot(projection='3d') 
+ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b')
+ik_env.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b')
+ik_env.ax.set_xlim([-1.0,1.0])
+ik_env.ax.set_ylim([-1.0,1.0])
+link_colors = ['black', 'violet']
+trajectory_plots = []
+for robot_index, robot in enumerate(ik_env.robots):
+    robot.initDrawing(ik_env.ax, link_colors[robot_index])
+    # ee point
+    ik_env.p_e_point_plots.append(drawPoint(ik_env.ax, ik_env.data[robot_index]['p_es'][0], 'red', 'o'))
+    # let's add the trajectory to this
+    trajectory_plot, = ik_env.ax.plot(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1], ik_env.data[robot_index]['p_es'][:,2], color='blue')
+    trajectory_plots.append(trajectory_plot)
+# goal point
+ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'maroon', '*')
+
+
+# let's add the manipulability ellipse
+for robot_index, robot in enumerate(ik_env.robots):
+    radii = 1.0/ik_env.data[robot_index]["manip_ell_eigenvals"][0] * 0.1
+    u = np.linspace(0.0, 2.0 * np.pi, 60)     
+    v = np.linspace(0.0, np.pi, 60)     
+    x = radii[0] * np.outer(np.cos(u), np.sin(v))     
+    y = radii[1] * np.outer(np.sin(u), np.sin(v))     
+    z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+    for i in range(len(x)):         
+        for j in range(len(x)):
+            [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], ik_env.data[robot_index]["manip_elip_svd_rots"][0]) + ik_env.data[robot_index]['p_es'][0]
+
+    ik_env.ellipsoid_surf_plots.append(ik_env.ax.plot_surface(x, y, z,  rstride=3, cstride=3,
+                    color='pink', linewidth=0.1,
+                    alpha=0.3, shade=True))
+
+#######################################################################
+#                        manipulability plots                         #
+#######################################################################
+
+# manipulability ellipsoid eigenvalues
+fig_manip_graphs = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+#ax = fig_manip_graphs.add_subplot(gs[0,-1])
+ax = fig_manip_graphs.add_subplot(311)
+ax.set_title('Manipulability ellipsoid eigenvalues')
+ax.grid()
+ax.set_xticks([])
+eigen1_lines = []
+eigen2_lines = []
+eigen3_lines = []
+for robot_index, robot in enumerate(ik_env.robots):
+    eigen1_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,0], color=link_colors[robot_index])
+    eigen1_lines.append(eigen1_line)
+    eigen2_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,1], color=link_colors[robot_index])
+    eigen2_lines.append(eigen2_line)
+    eigen3_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,2], color=link_colors[robot_index])
+    eigen3_lines.append(eigen3_line)
+point_in_time_eigen1_line = ax.axvline(x=0, color='red')
+
+# manipulability index (volume of manipulability ellipsoid)
+#ax = fig.add_subplot(gs[1,-1]) 
+ax = fig_manip_graphs.add_subplot(312) 
+ax.set_title('Manipulability index')
+ax.grid()
+ax.set_xticks([])
+manip_index_lines = []
+for robot_index, robot in enumerate(ik_env.robots):
+    manip_index_line, = ax.plot(time, ik_env.data[robot_index]['manip_indeces'], color=link_colors[robot_index])
+    manip_index_lines.append(manip_index_line)
+point_in_time_manip_index_line = ax.axvline(x=0, color='red')
+
+# dist to goal (this could be/should be elsewhere)
+#ax = fig.add_subplot(gs[2,-1]) 
+ax = fig_manip_graphs.add_subplot(313) 
+ax.set_title('Distance to goal')
+ax.grid()
+ax.set_xlabel('iter')
+dist_to_goal_lines = []
+for robot_index, robot in enumerate(ik_env.robots):
+    dist_to_goal_line, = ax.plot(time, ik_env.data[robot_index]['dists_to_goal'], color=link_colors[robot_index])
+    dist_to_goal_lines.append(dist_to_goal_line)
+point_in_time_dist_to_goal_line = ax.axvline(x=0, color='red')
+
+
+#######################################################################
+#                              IMU plots                              #
+#######################################################################
+
+fig_imu = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+#ax = fig_manip_graphs.add_subplot(gs[0,-1])
+v_x_lines = []
+v_y_lines = []
+v_z_lines = []
+omega_x_lines = []
+omega_y_lines = []
+omega_z_lines = []
+
+ax_v_x = fig_imu.add_subplot(321)
+ax_v_x.grid()
+ax_v_y = fig_imu.add_subplot(322)
+ax_v_y.grid()
+ax_v_z = fig_imu.add_subplot(323)
+ax_v_z.grid()
+ax_omega_x = fig_imu.add_subplot(324)
+ax_omega_x.grid()
+ax_omega_y = fig_imu.add_subplot(325)
+ax_omega_y.grid()
+ax_omega_z = fig_imu.add_subplot(326)
+ax_omega_z.grid()
+ax_v_x.set_title('Linear velocity x')
+ax_v_y.set_title('Linear velocity y')
+ax_v_z.set_title('Linear velocity z')
+ax_omega_x.set_title('Angular velocity x')
+ax_omega_y.set_title('Angular velocity y')
+ax_omega_z.set_title('Angular velocity z')
+ax_v_x.set_xticks([])
+ax_v_y.set_xticks([])
+ax_v_z.set_xticks([])
+ax_omega_x.set_xticks([])
+ax_omega_y.set_xticks([])
+ax_omega_z.set_xticks([])
+for robot_index, robot in enumerate(ik_env.robots):
+    v_x_line, = ax_v_x.plot(time, ik_env.data[robot_index]["vs"][:,0], color=link_colors[robot_index])
+    v_y_line, = ax_v_y.plot(time, ik_env.data[robot_index]["vs"][:,1], color=link_colors[robot_index])
+    v_z_line, = ax_v_z.plot(time, ik_env.data[robot_index]["vs"][:,2], color=link_colors[robot_index])
+    omega_x_line, = ax_omega_x.plot(time, ik_env.data[robot_index]["vs"][:,3], color=link_colors[robot_index])
+    omega_y_line, = ax_omega_y.plot(time, ik_env.data[robot_index]["vs"][:,4], color=link_colors[robot_index])
+    omega_z_line, = ax_omega_z.plot(time, ik_env.data[robot_index]["vs"][:,5], color=link_colors[robot_index])
+    v_x_lines.append(v_x_line)
+    v_y_lines.append(v_y_line)
+    v_z_lines.append(v_z_line)
+    omega_x_lines.append(omega_x_line)
+    omega_y_lines.append(omega_y_line)
+    omega_z_lines.append(omega_z_line)
+
+point_in_time_ax_v_x_line = ax_v_x.axvline(x=0, color='red')
+point_in_time_ax_v_y_line = ax_v_y.axvline(x=0, color='red')
+point_in_time_ax_v_z_line = ax_v_z.axvline(x=0, color='red')
+point_in_time_ax_omega_x_line = ax_omega_x.axvline(x=0, color='red')
+point_in_time_ax_omega_y_line = ax_omega_y.axvline(x=0, color='red')
+point_in_time_ax_omega_z_line = ax_omega_z.axvline(x=0, color='red')
+
+
+#######################################################################
+#           putting plots into: frames -> notebooks -> tabs           #
+#######################################################################
+# adding whatever into this
+notebook_left = Notebook(root, height=int(SCREEN_HEIGHT))#, width=700)
+notebook_right = Notebook(root, height=int(SCREEN_HEIGHT))#, width=700)
+frame_manipulator = Frame(notebook_left)#, width=400, height=400)
+frame_manip_graphs = Frame(notebook_right)#, width=400, height=400)
+frame_imu = Frame(notebook_right)#, width=400, height=400)
+frame_ee = Frame(notebook_left)#, width=400, height=400)
+tabs_left = notebook_left.add(frame_manipulator, text='manipulator')
+tabs_left = notebook_left.add(frame_ee, text="end-effector")
+tabs_right = notebook_right.add(frame_manip_graphs, text='manipulator-graphs')
+tabs_right = notebook_right.add(frame_imu, text="ee-graphs")
+#frame_manipulator.pack(fill='both', expand=True)
+#frame_manip_graphs.pack(fill='both', expand=True)
+notebook_left.grid(row=0, column=0, sticky='ew')
+notebook_right.grid(row=0, column=1, sticky='ew')
+#notebook_left.pack(fill='both', expand=True)
+#canvas = FigureCanvasTkAgg(fig, master=root) 
+
+# tkinterize these plots
+canvas_manipulator = FigureCanvasTkAgg(fig_manipulator, master=frame_manipulator) 
+canvas_manipulator_widget = canvas_manipulator.get_tk_widget()     
+canvas_manipulator_widget.grid(row=0, column=0) 
+canvas_manipulator._tkcanvas.grid(row=1, column=0)   
+canvas_manipulator.draw()
+
+canvas_ee = FigureCanvasTkAgg(fig_ee, master=frame_ee) 
+canvas_ee_widget = canvas_ee.get_tk_widget()     
+canvas_ee_widget.grid(row=0, column=0) 
+canvas_ee._tkcanvas.grid(row=1, column=0)   
+canvas_ee.draw()
+
+canvas_manip_graphs = FigureCanvasTkAgg(fig_manip_graphs, master=frame_manip_graphs) 
+canvas_manip_graphs_widget = canvas_manip_graphs.get_tk_widget()     
+canvas_manip_graphs_widget.grid(row=0, column=0) 
+canvas_manip_graphs._tkcanvas.grid(row=1, column=0)   
+canvas_manip_graphs.draw()
+
+canvas_imu = FigureCanvasTkAgg(fig_imu, master=frame_imu) 
+canvas_imu_widget = canvas_manip_graphs.get_tk_widget()     
+canvas_imu_widget.grid(row=0, column=0) 
+canvas_imu._tkcanvas.grid(row=1, column=0)   
+canvas_imu.draw()
+
+# pack_toolbar=False will make it easier to use a layout manager later on.
+toolbar_manipulator = NavigationToolbar2Tk(canvas_manipulator, frame_manipulator, pack_toolbar=False)
+toolbar_manipulator.update()
+toolbar_manipulator.grid(column=0, row=2)
+toolbar_manip_graphs = NavigationToolbar2Tk(canvas_manip_graphs, frame_manip_graphs, pack_toolbar=False)
+toolbar_manip_graphs.update()
+toolbar_manip_graphs.grid(column=0, row=2)
+toolbar_ee = NavigationToolbar2Tk(canvas_ee, frame_ee, pack_toolbar=False)
+toolbar_ee.update()
+toolbar_ee.grid(column=0, row=2)
+toolbar_imu = NavigationToolbar2Tk(canvas_imu, frame_imu, pack_toolbar=False)
+toolbar_imu.update()
+toolbar_imu.grid(column=0, row=2)
+
+#######################################################################
+#                 functions for widgets to control the plots          #
+#######################################################################
+
+# keyboard inputs
+# whatever, doesn't hurt, could be used
+#canvas.mpl_connect(
+#    "key_press_event", lambda event: print(f"you pressed {event.key}"))
+#canvas.mpl_connect("key_press_event", key_press_handler)
+
+# new_val is what the widget gives you
+# you define what needs to happen to plots in Figure below,
+# and call canvas.draw
+def update_points(new_val):
+# ee plot
+###########################################################################
+    index = int(np.floor(float(new_val)))
+    
+    # Update 3D reconstruction plot
+    # plot3D_camera(recP[index], scale=scale)
+#    C, Q = camera_data_for_plot3D(recP[cam_indices[index]])
+#    camera_center.set_data_3d(*C)
+#    C = C.flatten()
+#    for ii in range(len(camera_coordinate_system)):
+#        camera_coordinate_system[ii].set_data([C[0], C[0] + scale * Q[0,ii]],
+#                                              [C[1], C[1] + scale * Q[1,ii]])
+#        camera_coordinate_system[ii].set_3d_properties([C[2], C[2] + scale * Q[2,ii]])
+#    recX_ = recX[:,X_indices[:num_pts_subset*(index+1)]]
+#    rec_points3D.set_data_3d(recX_[0,:], recX_[1,:], recX_[2,:])
+###########################################################################
+
+    ik_env.ax.set_title(str(index) + 'th iteration toward goal')
+    # animate 3d manipulator
+    for robot_index, robot in enumerate(ik_env.robots):
+        ik_env.robots[robot_index].setJoints(ik_env.data[robot_index]["qs"][index])
+        ik_env.robots[robot_index].drawStateAnim()
+        # all these are in lists as that's what the line plot wants, 
+        # despite the fact that we have single points
+        point_in_time_eigen1_line.set_xdata([time[index]])
+        point_in_time_manip_index_line.set_xdata([time[index]])
+        point_in_time_dist_to_goal_line.set_xdata([time[index]])
+        point_in_time_ax_v_x_line.set_xdata([time[index]])
+        point_in_time_ax_v_y_line.set_xdata([time[index]])
+        point_in_time_ax_v_z_line.set_xdata([time[index]])
+        point_in_time_ax_omega_x_line.set_xdata([time[index]])
+        point_in_time_ax_omega_y_line.set_xdata([time[index]])
+        point_in_time_ax_omega_z_line.set_xdata([time[index]])
+
+        # ellipsoid update
+        radii = 1.0/ik_env.data[robot_index]["manip_ell_eigenvals"][index] * 0.1
+        u = np.linspace(0.0, 2.0 * np.pi, 60)     
+        v = np.linspace(0.0, np.pi, 60)     
+        x = radii[0] * np.outer(np.cos(u), np.sin(v))     
+        y = radii[1] * np.outer(np.sin(u), np.sin(v))     
+        z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+        for i in range(len(x)):         
+            for j in range(len(x)):
+                [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], ik_env.data[robot_index]["manip_elip_svd_rots"][index]) + ik_env.data[robot_index]['p_es'][index]
+
+        if ellipse_on_off_var.get():
+            try:
+                ik_env.ellipsoid_surf_plots[robot_index].remove()
+            except ValueError:
+                pass
+            ik_env.ellipsoid_surf_plots[robot_index] = ik_env.ax.plot_surface(x, y, z,  rstride=3, cstride=3,
+                            color='pink', linewidth=0.1,
+                            alpha=0.3, shade=True) 
+
+        ik_env.p_e_point_plots[robot_index].set_data([ik_env.data[robot_index]['p_es'][index][0]], [ik_env.data[robot_index]['p_es'][index][1]])
+        ik_env.p_e_point_plots[robot_index].set_3d_properties([ik_env.data[robot_index]['p_es'][index][2]])
+    canvas_ee.draw()
+    canvas_manipulator.draw()
+    canvas_manip_graphs.draw()
+    canvas_imu.draw()
+    # TODO update may not be needed as we're going by slider here
+    root.update()
+
+
+def update_goal_x(new_val):
+    goal_x = float(new_val)
+    ik_env.goal[0] = goal_x
+    ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]])
+    ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]])
+    canvas_ee.draw()
+    canvas_manipulator.draw()
+    canvas_manip_graphs.draw()
+    # TODO update may not be needed as we're going by slider here
+    root.update()
+
+def update_goal_y(new_val):
+    goal_y = float(new_val)
+    ik_env.goal[1] = goal_y
+    ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]])
+    ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]])
+    canvas_ee.draw()
+    canvas_manipulator.draw()
+    canvas_manip_graphs.draw()
+    # TODO update may not be needed as we're going by slider here
+    root.update()
+
+def update_goal_z(new_val):
+    goal_z = float(new_val)
+    ik_env.goal[2] = goal_z
+    ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]])
+    ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]])
+    canvas_ee.draw()
+    canvas_manipulator.draw()
+    canvas_manip_graphs.draw()
+    # TODO update may not be needed as we're going by slider here
+    root.update()
+
+
+
+def reset():
+    ik_env.reset()
+#    ik_env.goal_point_plot.remove()
+#    ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'red', 'o')
+#    print(controller_string1.get())
+#    print(controller_string2.get())
+    controller1 = getController(controller_string1.get())
+    controller2 = getController(controller_string2.get())
+    controllers = [controller1, controller2]
+    for robot_index, robot in enumerate(ik_env.robots):
+        ik_env.data.append(makeRun(controllers[robot_index], ik_env, n_iters, robot_index))
+        trajectory_plots[robot_index].set_data(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1])
+        trajectory_plots[robot_index].set_3d_properties(ik_env.data[robot_index]['p_es'][:,2])
+        eigen1_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,0])
+        eigen2_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,1])
+        eigen3_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,2])
+        manip_index_lines[robot_index].set_ydata(ik_env.data[robot_index]['manip_indeces'])
+        dist_to_goal_lines[robot_index].set_ydata(ik_env.data[robot_index]['dists_to_goal'])
+    update_points(0)
+    canvas_ee.draw()
+    canvas_manipulator.draw()
+    canvas_manip_graphs.draw()
+    root.update()
+
+def play():
+    pass
+
+# ellipse on/off
+def add_remove_ellipse():
+    try:
+        for robot_index, robot in enumerate(ik_env.robots):
+            ik_env.ellipsoid_surf_plots[robot_index].remove()
+    except ValueError:
+        pass
+
+
+# NOTE: this thing is not used
+same_starting_position_on_off_var = IntVar()   
+same_starting_position_on_off_var.set(0)
+same_starting_position_checkbutton= Checkbutton(root, text = "same starting position on/off",
+                      variable = same_starting_position_on_off_var,
+                      onvalue = 1,
+                      offvalue = 0,
+#                      command=same_starting_position_cmd,
+                      )
+
+
+#######################################################################
+#                     PLACING ELEMENTS IN THE GUI                     #
+#######################################################################
+
+# LEFT PANE BELOW MANIP/EE PLOTS
+frame_below_manipulator_plot = Frame(frame_manipulator)
+frame_below_manipulator_plot.grid(column=0, row=3)
+
+# set controller 1
+frame_controller_menu1 = Frame(frame_below_manipulator_plot)
+controller_string1 = StringVar(frame_controller_menu1) 
+controller_string1.set("invKinm_dampedSquares") # default value 
+controller_menu1 = OptionMenu(frame_controller_menu1, controller_string1, 
+                "invKinmQPSingAvoidE_kI",
+                "invKinm_Jac_T",
+                "invKinm_PseudoInv",
+                "invKinm_dampedSquares",
+                "invKinm_PseudoInv_half",
+                "invKinmQP",
+                "invKinmQPSingAvoidE_kI",
+                "invKinmQPSingAvoidE_kM",
+                "invKinmQPSingAvoidManipMax",
+               ) 
+controller_string1.set("invKinm_dampedSquares")
+controller_menu1.grid(column=1, row=0)
+Label(frame_controller_menu1, text="Robot 1 controller:", background='yellow').grid(row=0, column=0, pady=4, padx = 4)
+frame_controller_menu1.grid(column=0, row=0)
+
+# set controller 2
+frame_controller_menu2 = Frame(frame_below_manipulator_plot)
+controller_string2 = StringVar(frame_controller_menu2) 
+controller_menu2 = OptionMenu(frame_controller_menu2, controller_string2, 
+                "invKinmQPSingAvoidE_kI",
+                "invKinm_Jac_T",
+                "invKinm_PseudoInv",
+                "invKinm_dampedSquares",
+                "invKinm_PseudoInv_half",
+                "invKinmQP",
+                "invKinmQPSingAvoidE_kI",
+                "invKinmQPSingAvoidE_kM",
+                "invKinmQPSingAvoidManipMax",
+               ) 
+controller_string2.set("invKinm_Jac_T") # default value 
+Label(frame_controller_menu1, text="Robot 1 controller:", background='black', foreground='white').grid(row=0, column=0, pady=4, padx = 4)
+controller_menu2.grid(column=1, row=0)
+Label(frame_controller_menu2, text="Robot 2 controller:", background='#EE82EE').grid(row=0, column=0, pady=4, padx = 4)
+frame_controller_menu2.grid(column=0, row=1)
+
+
+ellipse_on_off_var = IntVar()   
+ellipse_on_off_var.set(1)
+
+ellipse_checkbutton= Checkbutton(frame_below_manipulator_plot, text = "ellipse on/off",
+                      variable = ellipse_on_off_var,
+                      onvalue = 1,
+                      offvalue = 0,
+                      command=add_remove_ellipse,
+                      )
+ellipse_checkbutton.grid(column=1, row=0)
+
+
+frame_goal_x = Frame(frame_below_manipulator_plot)
+Label(frame_goal_x, text="goal x").grid(row=0, column=0, pady=4, padx = 4)
+slider_goal_x = Scale(frame_goal_x, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
+                              command=update_goal_x)
+slider_goal_x.grid(column=1, row=0)
+frame_goal_x.grid(column=1, row=1)
+
+frame_goal_y = Frame(frame_below_manipulator_plot)
+Label(frame_goal_y, text="goal y").grid(row=0, column=0, pady=4, padx = 4)
+slider_goal_y = Scale(frame_goal_y, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
+                              command=update_goal_y)
+slider_goal_y.grid(column=1, row=0)
+frame_goal_y.grid(column=1, row=2)
+
+
+frame_goal_z = Frame(frame_below_manipulator_plot)
+Label(frame_goal_z, text="goal z").grid(row=0, column=0, pady=4, padx = 4)
+slider_goal_z = Scale(frame_goal_z, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
+                              command=update_goal_z)
+slider_goal_z.grid(column=1, row=0)
+frame_goal_z.grid(column=1, row=3)
+
+
+frame_update = Frame(frame_manip_graphs)
+Label(frame_update, text="Time").grid(row=0, column=0, pady=4, padx = 4)
+slider_update = Scale(frame_update, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL,
+                              command=update_points)#, label="Frequency [Hz]")
+slider_update.grid(column=1, row=0)
+frame_update.grid(column=0, row=3)
+
+frame_update2 = Frame(frame_imu)
+Label(frame_update2, text="Time").grid(row=0, column=0, pady=4, padx = 4)
+slider_update = Scale(frame_update2, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL,
+                              command=update_points)#, label="Frequency [Hz]")
+slider_update.grid(column=1, row=0)
+frame_update2.grid(column=0, row=3)
+
+
+button_quit = Button(master=frame_manip_graphs, text="Quit", command=root.destroy)
+button_reset = Button(master=frame_manip_graphs, text="New run", command=reset)
+button_reset.grid(column=0, row=4)
+button_quit.grid(column=0, row=5)
+
+button_quit2 = Button(master=frame_imu, text="Quit", command=root.destroy)
+button_reset2 = Button(master=frame_imu, text="New run", command=reset)
+button_reset2.grid(column=0, row=4)
+button_quit2.grid(column=0, row=5)
+
+update_points(0)
+slider_goal_x.set(ik_env.goal[0])
+slider_goal_y.set(ik_env.goal[1])
+slider_goal_z.set(ik_env.goal[2])
+mainloop()
diff --git a/manipulator_viz/robot_stuff/InverseKinematics.py b/manipulator_viz/robot_stuff/InverseKinematics.py
new file mode 100644
index 0000000..71c50f5
--- /dev/null
+++ b/manipulator_viz/robot_stuff/InverseKinematics.py
@@ -0,0 +1,204 @@
+# kinematics related imports
+from robot_stuff.forw_kinm import *
+from robot_stuff.inv_kinm import *
+from robot_stuff.follow_curve import *
+from robot_stuff.utils import *
+
+# general imports
+import numpy as np
+
+# ######################################### NOTE #########################
+# i'm half-adding another robot for the purposes of the plot.
+# this means that half of the code will end up being broken (as a lot of it is already anyway).
+# in case you want to use this elsewhere, just find the commit before the change
+# and run with that.
+##################################################################
+
+
+class InverseKinematicsEnv:
+
+# set damping (i.e. dt i.e. precision let's be real)
+    def __init__(self, model_path=None, initial_qpos=None, n_actions=None, n_substeps=None ):
+        print('env created')
+        self.chill_reset = False
+        # TODO write a convenience dh_parameter loading function
+        self.robot = Robot_raw(robot_name="no_sim")
+        self.init_qs_robot = []
+        self.damping = 5
+        self.error_vec = None
+        self.n_of_points_done = 0
+        # keep track of the timesteps (to be reset after every episode)
+        self.n_of_tries_for_point = 0
+        # for storing whole runs of all robots
+        self.data = []
+        self.p_e_point_plots = []
+        self.ellipsoid_surf_plots = []
+        self.same_starting_position_on_off = 0
+
+        # init goal
+        self.goal = np.random.random(3) * 0.7
+        # needed for easy initialization of the observation space
+
+        # TODO enable setting the other one with greater ease
+        self.reward_type = 'dense'
+        #self.reward_type = 'sparse'
+        self.episode_score = 0
+
+
+# NOTE: BROKEN AS IT'S FOR ONLY ONE ROBOT
+    def compute_reward(self, achieved_goal, goal, info):
+        # Compute distance between goal and the achieved goal.
+        if self.reward_type == 'sparse':
+            if not error_test(self.robot.p_e, self.goal):
+                return np.float32(-1.0)
+            else:
+                return np.float32(10.0)
+        if self.reward_type == 'dense':
+            distance = goal_distance(achieved_goal, goal)
+            if not error_test(self.robot.p_e, self.goal):
+                #reward = -1 * distance + 1 / distance
+                reward = -1 * distance 
+            else:
+                reward = 100
+            return reward
+            
+
+
+    def simpleStep(self, q_dots, damping, index):
+        self.robot.forwardKinmViaPositions(q_dots / self.damping, damping)
+        self.n_of_tries_for_point += 1
+#        done = False
+#        if error_test(self.robot.p_e, self.goal):
+#            info = {
+#                'is_success': np.float32(1.0),
+#            }
+#        else:
+#            info = {
+#                'is_success': np.float32(0.0),
+#            }
+#        return done
+
+
+# NOTE: BROKEN AS IT'S FOR ONLY ONE ROBOT
+    def step(self, action):
+        self.robot.forwardKinmViaPositions(action[:-1] / self.damping, action[-1])
+        self.n_of_tries_for_point += 1
+        obs = self._get_obs()
+
+        done = False
+        if error_test(self.robot.p_e, self.goal):
+            info = {
+                'is_success': np.float32(1.0),
+            }
+        else:
+            info = {
+                'is_success': np.float32(0.0),
+            }
+        reward = self.compute_reward(obs['achieved_goal'], self.goal, info)
+        self.episode_score += reward
+        return obs, reward, done, info
+
+
+
+
+    def reset(self):
+        # TODO: initialize robot joints state to a random (but valid (in joint range)) initial state
+        #       when using joint clamping
+        self.episode_score = 0
+        self.n_of_points_done += 1
+        self.n_of_tries_for_point = 0
+        self.data = []
+
+        # generate new point
+        # NOTE: simply taken away 'cos now it's set with a slider
+        #self.goal = np.array([random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70)])
+        
+        # initialize to a random starting state and check whether it makes any sense
+        thetas = []
+        for joint in self.robot.joints:
+             thetas.append(6.28 * np.random.random() - 3.14)
+        self.robot.forwardKinmViaPositions(thetas, 1)
+
+        if self.chill_reset == True:
+            sensibility_check = False
+            while not sensibility_check:
+                thetas = []
+                for joint in self.robot.joints:
+                     thetas.append(6.28 * np.random.random() - 3.14)
+                self.robot.forwardKinmViaPositions(thetas , 1)
+                if calculateManipulabilityIndex(self.robot) > 0.15:
+                    sensibility_check = True
+        
+#        for i, joint in enumerate(self.robots[robot_index].joints):
+#            joint.theta = self.robots[0].joints[i].theta
+
+# NOTE: not needed and i'm not fixing _get_obs for more robots
+        obs = self._get_obs()
+        return obs
+
+    def render(self, mode='human', width=500, height=500):
+        try:
+            self.drawingInited == False
+        except AttributeError:
+            self.drawingInited = False
+
+        if self.drawingInited == False:
+            plt.ion()
+            self.fig = plt.figure()
+            #self.ax = self.fig.add_subplot(111, projection='3d')
+            self.ax = self.fig.add_subplot(111, projection='3d')
+            # these are for axes scaling which does not happen automatically
+            self.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b')
+            self.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b')
+            plt.xlim([-1.5,1.5])
+            plt.ylim([-0.5,1.5])
+            color_link = 'black'
+            self.robot.initDrawing(self.ax, color_link)
+            self.drawingInited = True
+        self.robot.drawStateAnim()
+        self.ax.set_title(str(self.n_of_tries_for_point) + 'th iteration toward goal')
+        drawPoint(self.ax, self.goal, 'red', 'o')
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+
+
+    def reset_test(self):
+        self.episode_score = 0
+        self.n_of_points_done += 1
+        self.n_of_tries_for_point = 0
+
+        # generate new point
+        self.goal = np.array([random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70)])
+        
+        # DO NOT initialize to a random starting state, keep the previous one
+
+        obs = self._get_obs()
+        return obs
+
+
+    def close(self):
+        # close open files if any are there
+        pass
+
+
+    # various uitility functions COPIED from fetch_env
+
+    def seed(self, seed=None):
+        self.np_random, seed = seeding.np_random(seed)
+        return [seed]
+
+
+    def _get_obs(self):
+        thetas = []
+        for joint in self.robot.joints:
+            thetas.append(joint.theta)
+        thetas = np.array(thetas , dtype=np.float32)
+        obs = self.robot.p_e.copy()
+        obs = np.append(obs, thetas)
+
+        return {
+            'observation': obs,
+            'achieved_goal': self.robot.p_e.copy(),
+            'desired_goal': self.goal.copy(),
+        }
+
diff --git a/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cac3d1c8e224c623351da026775d35ef4b973cbb
GIT binary patch
literal 4522
zcmd1j<>g{vU|^VdKrMBpGy}t95C<7EGcYhXFfcF_TQM*&q%fo~<}idnXhtZ_1f`k5
zG*b>s6stQ!3Udle3quM^GgA~>3R5tHChJR(shW(p7`6P8K}=)}w<L*yfgzP4iZO*D
ziYY}Rg(-!(g)xdbg(Zcxg&~S1g)N1>g&~SHg(HQtg&~S9g)4=-g&~SPMKXmig};R{
ziX%lJMX-e-iZewhMYx3_iYrAbMI=SEg)xddMJz?Ug&~S3g)x{xQ~H*OXI@!qQE{qw
zW?pJ;Vo7Flv1?wLUotC_t3Yg!!<<3BFJfR|C}BupY-VI+NMWjBh-WNe0{O9*v4$a@
zIh~<~A)Y0LwT2;{H3h_GOJS>Fh-U|r9AH^aFv$fbxxwN*CA?XD3;0sl7c!=Bh=6qQ
zr*PIV#0!9B1ydM;88o^4s<>10$`q1|QWHy3Q`~N`<>kj0XXa`$-r_3CPs%Te&r8fr
zeF<`E5hDWw!%L8ZpQnMrGy7YtDXDqIshUE!SPP0W^Ga@UCud~l<ir=H7N?fn;tT>C
zP?T7Hixq6vE#Az$%#!%R;&=#`JtZ->ATuxh7H4WvQGQW;S!(hvfxP(qwD^Mj%)FA~
z_>}y-)LSB8k&>d!)Z+NG{33|lEtZtTlEhp51@Wm6f%t-){F35ZBB?n!nFYo9nJMwb
zrA28_QQ6|e+|>BulEk8t%)IpYg8bsllFa<Pco0fUyTz1OaEq-dF)t-Q_ZCZfeqzoo
z?xNK4#G;h=lFEYATfC_SnZ@}jsqw|h`9-OkthZQ-Q*+X8apmTxq~^pIB$i~{;sJRq
zGchN=upqzq7H3|3Vlv2r;#*vK@x`S{#U-f)#VZ+#6d4#8e&y(A<maa9r{@<J<fkMS
zfq6y<Zcb@lihe;+Vnt@LerXXXE(&r|<CF9AN{aGx^m7yQG7Cy`5=%hAm06_^4#@c8
zlG3y^eT)>RS5SG2BR(GFw)l8aP%4vSU|`^2WMkxEVq=6tCN?Nrh?$Lri>XQ$!x-1R
zGCi0n$t*|-n2mvf0hAuV2{?;^fuV*Wiy?(Eo2f{=gt3Mpiz$n_h9QfknW<Q)gmnR1
z3e!TyU<NRY8N>psX31tMGA?1SVXR?lW~yaD)2E-pTEim3P|I4w8qAQw2Bkp>m)-9c
zTX8{RQE_TI#6=(WLm4U#MVz3N`v3p`|1VP*7#K7;ili7A7;bSvQdvoAaY+#`NPxK@
zKD9`afq|h&9K>Z$%gIkHF*drz3rYj=DVfD3iFwJXnruZfAZgyj<c!SJvecA#kTh7|
zEtbr@wESBfFr~NHAn67i(GWrjq?Io@KewQ?BsCr)Z3v1A2?hoRE+zp+E=DdUE=Dd!
z0Y(lc7G{?JEQ~BatCX=u0;(w}Q2~k;5C%tu8Z;^vFxG&hL7Jh4X(1CMLk$B9Ll$!i
zqd5Z;Lp(D$|1<f$1Z8VY7D$MQr{x!c625n4UT#=sVgNLw6c_P>LP-b|m@JUcU@MGI
z$uEIO++xklOG&MOco>w%i@?6-D$dL;$Vm+@Ni6_73*<f#Mj^&3d91!es7_{rL@AgA
zB{ER)12$qBlFvYOMJtmeDA6%VGt@AqFiJ8Y`H~6Z%Nm9SEDITGnNpZ*nM+tfNiK`6
zh9Qf+nW;!Og{6dJ0cQ&9LdHzST9!O^D4z|&uVGlgRl}6R4z_{4hGijhEh{(#xocR#
zW^>mt)v(qu*RZ6q1T$!I_!X&w0?B}Zfx+z-S7vd1acOdLYH=|%S$?pGG8h<&v_S#M
z0}k>cP+~v??=6n_^wg60{G{R{b&zQqAOfC5i!?!8eGnlHG6I@1*&wCOE#~~B;#(}B
zLbFH*q(%=!D8dPFq_BX>qF`78RAA&|<X~h0B||2*|6Hsr|3#Sj7<m}0B(X<PNoqkd
zD7}DVhJk?r6fvL(0!PjfPz4UEt{@4GF$EF@j42>7CUC{XoX$`SN|G#DtS$_(e6>t9
zj0@OOm`m8RI2Ld$WDsFUVUb{1$jHc0C<ZDy7I4-ura+T!7FP;uHd9ed3R^964Rdh}
z(I#1=npDCK;x{v<uxGOrMWwKUOe-?2VOYSkkfD~P1{#$hPZoiqGF`s)b>I#;Ne3u{
z(;>ty-53lG+iUU`f#RV^5ERCs+)*S9Vu1@6wk&WRc8e{spdd9b1zf}wfigWL#<@Y|
zf=qH^PBN%8PxXbC>q(h8nI)B;;Ot}uiegr1b}Pw9ElDiC#R@j{mSAyeUU4QwaeQ({
zYI1gw1;}Q!$Yups#2KL21|=jmW<GGK&%(~bD8<Of$i>9M2!aqEABg>ngNKE&N(yW2
zgAKdIno*jYnD_DvBLf3U2?R<C91IK$;G|&0$iPs=4$5ty>VT0UoFR{gg`tG8Qkane
z1i?vyA(A1Fu^g0tA(ci^3DW}Rg$#@gB}||^3u^1sFl4c2v4NUtDJ;F9RuRZd_AHJP
z&IMd4tm%vk8PQm5Ah8to6pmgNP<fYyPc0|1+7hM(+$o$XZ0Ss(<X6J9fG33u#!KO@
zWvOAv;$6T8DmiLcK&4CyPYpvn$nCrf_(5(4$t(~k5v*az5^82l;S~ob;}T(zcnw3A
z2&e(s3~>`{mgoX8s2&ho9K;5f#sYqBFY`eOgUL7r)B-Cg%Fjv#w<N%gFK7Ys!~Pay
z5~!h@3=@D=eGm5EVol0POwJCek|@bg$Sg@MN(8k-6iV_zrMd#B;=09Ll$vsjF<(>S
z7H>*XVtHm>x@TTyNovY1KF5-hqRgbyl2lhvbL<v#K~Bjn=FI%OTWo2W>7_-fx0us1
z({FJnrhpn(NuUPTEvCeZTPz?JIDOw@O)M%(th~iik&~Hwi={FrGZ$RQ-QotdL0llV
zL97ifNi0cq%*)KZ#aWzM5?_*8l9PIixwxdL2$YX*ae|x{0Lnp-ik2-oF|RBURJwth
zlRRlTrNtTXsb#5oCB>T1EW!e6kKJM|&rB)FxW$%{nwg$aa*HcDKPSH^J|{CT8@+mB
zD@x5vNiFII<qj<d1_mZZP|3%|$n>9!nS+swQHV*0Nr#b-5tJ_k7}*%P7{wTc7<m{4
z7^|dl<PVT}DCHxlB7<j&0KDa63G)ILNZTldWg!zbRyy-SP)W*O!vHEPYZ+@mJq=br
zaMFO3Rp4fM+Oz#dAUicV(F-R~E2k(FR8D}(p<C>wd6{YXMY-S*E&`V}OyJT6Ygxht
z&i?VB*3(i@knu1uFt9O#3KT774i*+%wK_r<O29BOFff2HxU>K@3_$6BC8HlWk~A41
z&Ok5Cl5_HlQ#XSw2ZbvO6OM8UtPG_M0a6ZcsEUDmTA;SkLdF<SnZgJP%5=tHh9XdY
z(qy{DUYwem0`8aO6~sgO1)7kM0X6+oQ^1ad5TH7>D4T(SVHe0*Adi5O3a;uiH8lk#
z6hP`h7#s=-3=9m=N{u0l(S;$F16(~a)i7l-gUX~rwi1>s)*8kVmMpdu#$G0HMaq!H
z4o(crB`gb=YM5&n7czk=G*HP4k*Q(G;smKkVVc7n%%I8ar^$GWH3ihK(PX~Gou5>k
zT2uxuBfyQ4TRbVL#hFFWHm)WYIQJG=gOZF5s9a(NS3}^u4oOJh1_`Kcxy6#4Ur-57
zN}7-+Vv!w4n>~oYQa6Fq+kQ}>i7+rQurTs4@-Yf9@-Xr+GBI;8axk*}#nlsmwvRMf
z{QUg<G?|N>KsJCvun0_m<B%8B?*a8(<Ku5}#mDF7r<CTT#>d~{iH|QVP0WGF6e)vr
zf?HE2AQor<qR0`%0`(e-(m*Uw-3g9guzMkKj+!isKnV*}W)*`H76$_tGlvX^5QhMV
zD3>ya6bBy{I}a;ZjVAjoQKasg9;m4upPiYPdrJgaI5V#dA}WC_nwFoFlV2X6Tv}9?
vdP@*lrnDq8r}!3YUTJPYB_v`%4#MWrTO2l!fUpDQykbxW=3s<EW+5&BFOgx)

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e19567ae0945d05a0ef9d5b7593f2eaf8ca7e1b2
GIT binary patch
literal 7195
zcmZ3^%ge>Uz`#(sR4w(tI0M6D5C?`?pp4HR3=9m@8B!Qh7;_jxAT%SCW`fepV45k1
zC5n}a!JQ$6xrHHxWf>C#!)hj|DGX6;DNMl(nyfED0-B7s7`6P8K};BiDTG>=$`Hkv
z!Vtxj!kxm@!WhL2GBS!Kg{6fdiZz9`1!Nps3qurp3VRDf6h{h23qurV3Qr1W3u6>l
z3Req56n6?^FoP!VEfLSWvecsDRPW5Z)ZE09%;aL%yfVLJR+zmE3=GUr{Mn5W<kJ#H
zgj@>aGDZf5)o>9;h7_h6hIlx?1j(Ef<~1y+s?!;37~(;ugAGh)s9}hQg(^b|YYjs@
zTz3jcjt#7gfq@}~t%f0<9fijM<AF`#MB#zs5F`SY;|8%n7;Fj;hy}tWQXmq7v-lu1
z1H%G13nl`hQ`k}ck-|~M$^dd9KS&mYQ#flF;srn~5C+>M2x5V73PUi1CYN6ocWPdl
zLUK`RVo7R>+by=d{P^O`TusJXTt)dw`6cmriMgpSK><(%iu#wJV0xYg2G8tov8JTv
z6{l(n-C`{$%FHXd#hsjynUfP=lv<oxa*HzvY(P<B`7JiES;e>bGV?M^;tPx8p?vm~
z#N2|+y!2a~sYONkMe${+$+raZ;`7tu3-UAbN{ZuC^7B$}iGW2)iZWA+<J0nsAab`@
zQW8rNZ}Atzr$Pkc3v%*Hif@Ue=Hz4+6z6BA#21$qr9nkyixYEG<BLlYi%K%{(&G#A
zi!)0y^Yh|CC@t+4Q(nO>wxYzml>FRVEa~})Ik&irQp*#IQsPT03sP_KrWRxt=clB`
z7boWzrE0R?Vku6|NxQ|Bo1c=J6JL;6l5vX%<g?7gocO|m{Nh`jdGU$KAPb6baplDq
zmnIdLq!ttxgMv{31b*e{XXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y0C_)Ny
zQsa~J^Gb^HbM$i)^D+xca}rBHA(mOC4-U-u;*!#|G<}S0q*qXRiz7ZB<h=O!Dp?G9
z*Ss=4n6xMZ14Ho&1_p)(hPxcR9k~}dWUg??T;PzoA+6BCa)m?Uskp)lj|<|a7sXAl
zh?`yzGwWdK;l3dUmXy9BF44i#!*fGYwu7aI?}maRNQ>+P6;%+e_&`LggQbV_hCEnU
z=7x$ISonsn9*EZZ%EllnKE?C8h{`1ql?8zpMbxi|sIO4ID549}^?`#yMj50|;(@Fj
zh?cs*0YaZa2{{>_Ss54@Kx|MN{cHp-Bx)G4KuW+kg)tkHgc*w5OF$~XVxa7j1r`Ny
zv*6-23|SzPK!OaY1!S>E2|VjEFk~SL118kW7|eh}9W%N*6gRR!-Dp_?&%_K2HH<Y(
zsII7Gp~w$xDXcXtpu(b-HIFHUt(LWhHJAZhAh7%0Vk<64EGkY-2Ya63!+t13#i0mP
zO1=F5|NsA&DGUq@njA&43=9mnxFC7AB(=EY7ArU{-eN9@Pc4#RU|=W$6=%2D({l0?
zON@<f@q+SJd`f0<Nn&1dswP{JJjejv#N>?3)Uwo+c#t$$-z}ERytMpV95AJ~*dY0_
z7!=;19HgL7qyo~*mz<wlP+F2250S4@#+txU6&Zq(`7CfUf56Rug<E<?#U*a_3*72=
zg=J@?%*mRQw<2Xt=0#zPE5a5Z7?^oA?utuJsal|NQC$6sxcUVF^`~h38xT{<FLA3~
z;8wdUuCgF@gUdy6%PZoR9UfQ>W#N^5ASpc~W2Wo`wh3$xBxNSB-4K@S@cqEgz@vVF
zTdhcqfq?;g!ma?<^T>&&hFKC4R}2gc3*dDgT!;ajfEW<Tyo#HF0X;!6GSo0sFd{NQ
z3S$*714A+sqFw|i4<^5tAPY2Eia@o>E%CJcB2Y2uotc*#mYEm;Emn(*gg|i*Do!+6
zAaTrA7@v|~0+G1Inwgi9T2TzjVW7B##3Wa7W^O@FYH&$vL6tn#SVSmr2F0KQI0kQU
z@bqv`aJ|AIb)CcL5{J<SvmGvbd@dS$oKU~W5p;zk=mJO34Gx|Q9Ew*s6c+?u<WPe!
zFK{Sc;828wGrRy`U|;~1(V$}B^A3yx1XtLC+FY$nh&ZT04{OF0MuaSCM#cztCiHNx
zVORhPDCFn_vzIY2Fsuf%KtwH53Ue(pa*>0aLJ-AN4MP^Z#e(XRBJmWK5_s!?fgy_%
zWG)D&u%Z@OnT)k8d15$Kvtg>PVORif{eVn_;TonCc0|Y^yRC+088ZXJYEVpo71y$Y
z(<i*>tzm_R6GDWkhP8&dh9!+9m_d`nuSgS=1kD&27~F1gWfsR5mnJ8t78gScvk&%A
z1_MKp0jMnFDbfL{HwO{$#CwY)K0UQ0K0m3rNE;-f3nJjfT9F=zYXTzVKt@1IGd4)g
ze~URksrVKPr~y+1DsYO7L1`Af_+kMy45}otC-;)nf?$xQTkwR=b)7@&5{K3bvkl4{
zj5a8*vECuFN9Llo>w&TpNk^*iruQ3sLjAd2xt;kv`5nv;_(eKc?sAJxNSTr~C2v8>
zlFW<T23NQZJ}|Iz@;wlcnVU2xYlZO4{5km_n3+WQI$SzjK5#Jz%3_FrU}F#xyDq45
zNl;}$@Cx}Ws+K!U_E;WBxhUv;MbNv04V?3&u5d`rNV~|Pa)ATHyumLt!EJ`i0+kE=
zY8N=5MIESA0Y@|g0|O|KKoS4hjTzL?PXRU0VRDdS3a&Z@mR%4cAl2{;4Q^_K%mV9A
zXQ*XFuGx{ZWfmyogH^dO#9G!e)i5pqxdSYMOr$WEAT`Fb;0^u-a2B#^)JzJhaZ^~T
zxEL5vvj`(Yj}<O=Er4f3WOHj6Q<zY*FC#+@Ll!)frLaQV5mQsxYME=8izg0mzn6$3
zLI+n!qK0`2dp5XOEn-Sx1%=T1i6w}Vh#`fkL>jw^sHULCfeS;dXDv$!s8s~jjO>;g
z7K8<e)P`!F3q!1W3=;!GEo&_kvR>>dwT2bRM0k6#hAAG-?$PM+0$1D|e&GIF5vV{+
zmv4O?xI<3T0m|TX2ysg{27|-)ngT_j(yT}XRIY()@FFn~3tZi^Wr2HTx7ZR33R3e@
zz>T{iP&gHV%S}+exFwUEn3D|ZoTU0fyD3STIhiGuo}eoK7Hd&rUV7>+=A6{LTb!wR
zrMam^i6yDGSW7ZeOA=x2K{hb2_!cYJx?6(9sd>ej5L4olGg6bYZ*ha0nepIe#4W~5
zj8-8lxCdG#g|%D-%VmHH*6oa-uFeNG203|frvhAB$zI`*T_AXoL+Jtsh<O9rf4Kqa
zzkJ|h5EAKNyCEt$C3=D2l(;LRS|1n~Ic>p2hwBYq;r`gJ*cm2Em6m8;S2DV!WOPx<
z<cgBXMQM`_B^RZwFY?-4;kCH{Mv$h9V-MeT4!uhpdMnB{6kg=8yux95fx{BoFfF;n
zt$cx7`G$h(1s>TOV(K3l*m!ksNJ>v&yCJJMCwhh8oVY8pMjsd$1zi~-<OJ6nqSDi2
zC&ex>S*x^0^SYMRB`vFqS~gd-Y%VI>>?paY?0iww<%+1w1u(iHATot}hT{~z>jGw%
z1k5&+9bmjD;B-a6>4JdMU19wVEL*sDIBwy)C~SX4*#3aZC1JM<!frP-bS{Xfe&A&g
z*8n@{s}O^X;sU{?&MR2fa9>n0xuRlnQQGv1wCM*1CQ#5YaoTox-<45akg_CugZ33Q
zyDMt07iHY8$hduAVB`gb9k1;~jtPttOmE03&B<P&I4AFlobd+7D{^L6<ZLFePT`)w
z{ec;zk{M<mq~QiH^%)o#K;<u}r2lLIZn#lWYm~s7NDN5*z!G@r%D{lDZa^PSqnDm^
zv|c6hz*G$bF?CZdV+|u{;19W+01xw|J=<RdYGi40q8AL{z8z~~QBh(gxD>d>UYeJg
zmS2<$?!pz>f*M6k;O2jkJt(_@YGqizjSHL$<3XK99IbtXf_{+Z7Dyuq+Wp6uQz1hM
z9&^*?=grDn5xT+UqMYRwIm^9f`|Wnwolw0H7<$n<?22{R)rh$35ownq(k`UuT*xcB
z7*TvBqWFSr$wiUUD<Y-PVF!L;M62lrzfcDkX2Js{LU6(ZwHH7mu)z!~8U0o=XflHR
zUJR0l_EuSwbMlK*ag<A7xw#;}f|6eY!vhYU4i<=Yppb*sD2QQDA8<#o1SXEB^M_i;
zfa(|I!BJ4mr!xjK6orB#>=t`*YHA92P%f_^9x^DV35mvAEFh_3kZYjvSQO2`z<{g$
znwpxj1mqoMaMazESGz86a7o@^gUCgBvn%pu7dd1=14T1fZU~5Uly&ezyb1~}n3q5m
zG&p1nz*!pFgn`L2WWm+CFvQA$n`|Jb!R29$8YV=)7Bwgs8G2+(IAQ7-7!cKR4PyzY
zK!Hjj`cG>ZQLB7#gA%HW0jY0ARF-2#u^aC88s-{wTbOH@QTlVB6au!D7<+1v`f|jm
zPhpzF9L%7}?5D|ii!}u_0;<V;i#tE5IJKxO5j3g^?lRrtNl7ivEP{^OXmUY1P@tZ9
zkqfBB;0Yo?MMO~`hz0H}fjUsPSd#M#D#1krba0@^3#7msM1ac)P_)Cx1;E7!u5n9f
z?`$O~m4T941H%nLVMy&9bwyMSR4J>2i4NBrJVO1xUA{9^=V)K#QM<yUb^(m;3JOn=
zn4z{-XpPhbL6eJuCRYSaI@oUT3H4<4<Xz`eyu_z?kx%&wpK=HDT|R-H%o)rx!Y=Zu
zT;WscVE(|yAR`Z|hok1iU6(buBx`U{*650?(G3}eIiB<VX8B!|(YYd{^MRd3fa?PT
zgy4h}Aet<Getv$C9LED1p9KxG$H(8|ijU9DPbtkwjgP;@6CYn#nwSHTDN+Rm54f{w
z1!6gZ2!9X(>LM0}gIJ)v2oA2IFc23sgajU1DFStEzyzpgTMX)xG%&#62Q~>-`40@J
zgasd~)CUGQp~A$f{(%8bC<uVKa6(I-)%F7eobY4hW)=Rx04HRaSb0I3p#&cjs~Cuj
zOmOkAa(!UHB)}yC*x{P&w?rXrKJYNN9%$StK07ln_m&8<aAsZ^L{tJ<G%Y_TC%-&C
zxwNP(^_C#AOle7GPVp_)ywco)$|7)D1qT&s@crVjfy9YjQ6>We1E|m`u4iCi_`uA_
z$oPOk`2q~xU{JaMLpK=IF2K+Y2Kfsx^ns0mQTPIb5CnY?NoHc?`oMsl{0J8R0wyul
HfCC2rDB=i&

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0ca4ef1695f5543b39961ae380c249e8d247006a
GIT binary patch
literal 1424
zcmd1j<>g{vU|@)xtCkwT%E0g##6iYP3=9ko3=9m#Q49<WDGVu$ISf&ZV45kHIf{i5
zBF37_#>BwLkjoy$0TyA(;mqZV;sUc7bGUPPqIek@QkYX%S{S1EQdm>iS{S1EQ`l2D
zS{R}PQW%37G&x^_Z1vM*yv6HQnwRXDmzkSbl9`{E%mNaJVvtkW85kIxK@OV0z`#(#
zuz)dzF@-6Gc_DKRa|(+zg9t+{OFBae>mtTl))J-^wiNbeCKrZS!&<f)rW7_wh8o5i
zwiLEBCP{`G77+$<hFbO-rW6jSI0uqAM-5X73sjs1Nu0BWA&a?&y@sQPvxc>XO_Cvn
zvzLXDp-?1+A(%mv%kMeVtqf4c2m9x<W`e*@`<I}=)MUQJm~@LV{T5?U5hDWw!%Jom
z0b;FWyv3TFpOast$#RP+uizG2Vo_pVdg?7!2*py6lV76Ab&Dyn;ud#)QD$mh2{?pq
zF_*-b-Qr6uOOMaV%*&3?NzE(CxW!srl3GxFi#a(z=N4n;EyjvljFq<-t5z};$uKZ5
z{7TZ#$j?pHPtPwd$WKWu0`rUz+?>+96#ast#EQ&f{nDcN;>_HFoYeT_{JfH){2cw<
z#JtRc(wxMS{G#}>%qsn&{G|Mn_~Me%v^4#cqQvsdymY;S%3J&(wm-t59H3YeU|?Vn
zW94AxW0ZhGmOn*u3=9k?5e-tx28-wfw1}={fkh2Cf*{ezTEmpW3W_c;2@!)wIXL1W
zV(c{xSxlgat6{HUPGJW{9XR4R{E9#kQ3OhEMXU@A4B*Hq;s&umE-B(+U|`T>F5&|T
z@Pi0JkN`)K5Ca3lEf!G77m0wFT;M1KM@5ksNS!2zkOC3XAOaNp;5aCf1uF*yQdnwo
zNq&(8D11OkoR3L_5d_5;ixkmA2IMr58gLdcVqjpXVaQ^rVN79^WJqVMVN79y(#&9*
zsfG!ZEkHT0mm`L;mZgRzm_d`>56yGwkTfgikbd?I2)wb^WWB|on^=^cT9lTUoC@|5
zOdvHS9V);EQE`h4LKkONrQTvq%qhr7)MP9I2M;SGco++A@qo>U&&(^%Oi3+L0ePLZ
zAU`v&1QHhDPyq)BCn!JyKr%|8@Bqb!9wQ55krpVp1#hwDmF5;yf}`P<U~XawC{yHQ
zCg~Maf>^iMpfqzqPRT9l+=86=lKlLf?97s4y<Ct2;}pHbiqvA`lw0gj{w+4B08@eS
zEfFLW6JbSA5h&ZDSO)e>@hvte&61p-Qv^=L;P?VN70Fu&zj4^)=BJeAq}qY9dod_0
Va4_&N3Ni99@-T8Ra|rNo0RXr_S(g9+

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..60bcda8221c70f3842b39da90028011be8e56d07
GIT binary patch
literal 2272
zcmZ3^%ge>Uz`zhWS1mPwm4V?ghy%k+P{!vl1_p-d3@HpLj5!QZj9{86mpO`s5hBK#
z%a+W<$dJn(#Q_#!%HhoAisAyZ8FRREd7^k38B&;A7^3)6SXvmO_)}P07@`DH7=sx!
z*<OMy@zZ3y#p_m@m+Y9AnVVRWnV*-;0uqN}4h9AWW(Ed^&w7j?hm<fv#TghFvf%6#
z#uTO$=4H$b468wE!AfeFQ&_6F85pWq85n9=(iu`%7cthdmau{)!9)sM3i~o928Puz
z&oa0$#3t9W)i9;7NrF{?h#JNkwiLEBCL|FSkeO8s3=FmGHB2cSNHQR`95~c+)G(#6
zps8iSp_a3TVFBDHAXmU}4SNko4QCB&4U!Aja4utEU|0<gHAaS>loW<w22C!%=P-Rx
z#s~Z7vu1+8PWzW2Uu!boVobWln0||~s0hS=39|GhC|p-E-eOJ8&&e;+WVywZS8$6h
zu_!SwJ@pnVgkmYk$uH65y2X@Oaf>^@C^I#$1RQs_m`mczZt*3SrN`%F=4Hp{q~?`m
z++r;*Ni8V8#hjd<bBi(a7GuRN#>!iaRmGt6rvL}PlJqn3b5r%x^NS1eQxc27JR<}*
zr!+4`zo00wBC}Y(v?#tfGq)foH9k2%ucRnHM?W_)FSDRDC$S{ID84MSO1~&SDZeDX
zxTG{KO+Te5u{<*`U9X_>7C(sXk8p62JOcv*D3KK(VqjosVED?#z{}s`IU#w5@dD<H
zJStasR95iq5WJ{ib&<!q!Sw?h1GiL%eusX8^Icw{35F8_JEMA{J}|Jbs)LCR#v45R
z6Bv8kCPXg~Twu6B_=>3N3f7CFx)*tLDUjpf>Sym_pTIc5a6;mY;su6t$~xFDa;RP5
zP`dy|A2?wS_za41lpMqc%RzU*S(*48RLg>%Vo}mBA{VgMFr}~}r)(4+LLElVLdj<c
zb?h|^3qYw6<P{VQO7}JFHOy<+(NjD)y>s{#fwFfI$j3$O3=9n5G+o3CVu7N#h>wAR
zL6f;i03;v?B7{K#97Q4w3=FqeKnb=;48-IDr+jb<E|LJLlLiqoAVQXbfuR^=7%bTq
z$$@o%l2ur0a!GzskqQF?14vhKIXFc<<>5n3PYtd&IJi3;d)Ow3Oh}v}KErgP{6!9h
z21k-aS=id$o7_7bI|?T#&ai55zsRC?g+=WG3>7IbFfgDb15jQA<?hdU;ABw4umB}!
zVVoMq6h=f;rZd(srXUr4U_LX7JW~x5sHjb0UBkAFgMnc+C~Uxn#V~>j-Wrx*22FN9
zw3tW-t6&gwNI!c91m4(dvfkp)O)SbzElNvFP6dZQOdvHS9V);EQE`h4LKkONrQTvq
z%qhr7)MP9I$0aKyE*T4M@qo>U&&(^%Oi3-$07VjOL4IalNiismfMX95b)2B63joO!
z=`k=cD1jo_4;*thc=`LoyTT_V&S0F9G9he6;sVAwDKo+rB(7jwlCmIdN6dxj^a}-L
z9~c;1nB18@FfjNt1u=Dme`H|b3ugKPCL7%vTyJm-H28ocL{sn<YhGz?K_xf|-V)4B
zECH3HIhjd%1(hJyEjB34T#!?8OFFk8C%z;<KPNl0q*yN(q`){uFR>!E*f`}DJCuKm
z4JyD?V0=pi$;3oheOd&Hb`;CNK~sE-4N9{l=jRlG3qo+(1qZ`Q2C$34LG+8mCO1E&
zG$+-r$cBM|0aQH{&tza=_`uA_$asT6=mHGgV35Cnif%AyT)=^DFz8)CMK>4}E})_h
cY`l!>9~j_-0vm`6Cq$SS1wSxg5@4qT01#pxO8@`>

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..596473b77c62ef13d20202dd981aadf8f95a37f7
GIT binary patch
literal 1220
zcmd1j<>g{vU|@)xtCs4_%E0g##6iYP3=9ko3=9m#K@1EGDGVu$ISf&ZV45kHIf{i5
zBF37_#>BwLkjoy$0TyA(;mqZV;sUc7bGUPPqIek@QkYX%S{S1EQdm>iS{S1EQy7C8
zG}&K*Z1dA(yv6HQnwRXDmzkSbl9`{E%nA~RVr~Wo1`yvF<d7K*3=Aa<3m8)vQ<zei
z7c$qdq_9Xch%nT$rZc3lE@G@@D`84uOJQ$ja$$%yt!1xaPGOT|s9~&OPhm@Al4Pi1
z6=4u(sO6|(PT_!xb0CRx)-b2AK*d>*#JOsiYS<+i7BJUv)Nn3jVq}2wS!%czGBPq0
zili_EGiY-9J%_rP0m}Gb|9sX=5ZGz|5)_!4%(obmZZW3cVk|0RWME);$qXVuEKSy1
zOnC*j*b<8p^U_mqu|g=0;?$D(l*E$6TY?~#aY}qaQGP*cQAuWMu_pH|ro@U{-1$YB
zsd**faJwal#EH+z%u6l4#at3!c8f2uEFHwpj?YQWE6KRUT3nJ^P<)FyIX~wXW9BW!
zid&48w-~EdG8D-$Ffjbe*3Zb#P1R4&FD}SWNh|{Mj1b(M(!3P?f}+HV%wql0qWI#>
z+=86c_~iV&lA`<^{oKU7%!1OK#FG4?__E9@{i6J&{F3<MlG3y^{gk4_^31&S__X|@
z_{6--T)l$ITf!hge}pGMsel6%*Fp>o3}UPt%zTUzU?{*Wz{v8mNREMl0VRQOA}5d(
zv;<Pi3X4*3#6qH<t%f;;6%@^25+VjqD&PbI5#y*~TEJApR>Ka7a!~Yxq8l99?0!X{
z$SY!DU|=W$rB85#7J*W05gP*ogC<K64@im^MDT+M0gxzXkst#D!!4Ga%)IPdoFEbu
zFR4W$AW^R5{G9wEkU(~k7)XmGh>!vi(jWp9Ip7#7k_9Q}1BF~zYH~?_5h&0=>WV>j
z@-c}pf*=nQA7hac$X0<{ta+um1(jeY-4e`AEGfvzFUiSF(krM0v2L+JY372Ql3UWb
z1v&90`T057nI*+~xgZ6`DSC+&sl~=Ax7eZlTWnAPrUK(zB1k5J;uV}4i$EzJ#WJw>
zif^$&X_n;toFZ_1f?Wym8JZtCY;yBcN^?@}K$)bNkAZ=KgMo)ph>?ephmnJsM+gAL
CqcKYW

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b39611c8807a0497f50ad6faa2f76392b6793fb9
GIT binary patch
literal 2032
zcmZ3^%ge>Uz`zhWS1r|<m4V?ghy%k+P{!u~1_p-d3@HpLj5!QZj9{86mpO`s5hBK#
z%a+W<$dJn(#Q_#!%HhoAisAyZ8FRREd7^k38B&;A7^3)6SXvmO_){2z88lg6f{gdm
zWW2@eR+^XWn3tKGSdy8am&^(hhhh+$nSp`fa~vbc2_=kBaRvs4EI2!bF@-6Gc^NYU
z!)lOPu#y^<6qYJ(28JqD28LSJbcPhxMU1s<B_Q*_@(c_qY$@!^m>3vV!@SAh!Vp_s
z%U;8r!X^n-!@y9(Si_#emd1o6!U{68ih+TlmZOF_g#$?jq?QARTFx5g6c#kKEI8D1
z)iBktBh)T{`wH$X1_p*2jv7u>_cJn}>0?1w4`bJGEn{S0SPc(7Muwi66oz01O-{e(
zFf~xd2m9x<W`e*@`<I|l(qz8Hm~@LV{T5?U5s3d1<gk|@lQdaxG36E9VoNMa%u7$b
z#R{P~ic?GCQxZ!OZwZ1}#wqayMfnA(MJ1W3#hTo=m=Y^)apxChrskD^WB!&P5+^<<
zGcUFH7IR5_*)6`rvUCtXJ3c2huO#CZYjH_xLGdl-<oujljG4C>D{e7X-eRmO=4N1E
zP(XlR+4>pzxvBc;`NakKDTzg3o)LnZQ<|5eUr>}-ky)%?S`=TLnOl&P8lRk>S5lOp
zqo13YmswDnlUR~p6knEErC*evlwT5GTvD2rrk_%jSe}`e9-o$96rY%vnX6Y&c}o~1
z=#TIXC_xv=GcYi4FfcF_-(p~3Xkhrt#lXwo<2fODhVcUCi##e<cvM#K?GU`EVRezm
zy214W8w0mghkl2CgY#Wpp$Uc)0z0F6qCPONvl@Yk4#pci{1X^^+$KaX5L{rmK=_KN
z>I&A2qPiD(bjg?dz{bESHAC?Vhs<>j)k_?z3ldidUgXfe!l8YEL;D7YBveZ85{KT3
z!iyY+S2zqWfYAqjn8QAU5)Sr^%Rpkrtz|{eC@9$mk@?tam{V90nWTmhg@;gwk)ctt
zH$ojp4buWpq6T>d1=q0EpcNFLjD{;yqGv8})?)W70+l>PYzzzxMeHB~WK$6@hy_X>
znk+?pAT~dU5CjoI3=9mKoJGP63=Fqeax(L>Z*hW1P%cO<5(9~HCFkel7l8z_izGl=
zq(OuXh>!)PI8NluP$UP^!UsyCVX4U_`9<K=smj2>AO%v}4o;m<dH7J%W`pYu4(<-e
z9<~W06B4J0&oG@Rf009>!I2cv2Q0kTS;Q`}h)pP*p?Hx+<_e3<1s0hbAW`{CEb<E&
zFS00IVNtpOLq!T8p9|b#%`43<s00VgEy3Kxl7gK4lAO#Wy@E;*>lPc7W-iDnxh0)j
zkP}~$pP!SRSyHT*3sPX5qL)~aT5Ozhiyg|p#Re5%Dlon!f@C5n1B1(-B2aOLVi`Dy
zi*K<(X_n;toFZ_ZE&>JJN(QiF!D0K0!zMRBr8FniuE>~y0hH(&in|yX7(OsFGBVy^
z5V`<EHyGqEprRWL8W(V&8w`3EP|*zrg$t<Y1DgP&(FX=Np~Ata{DA?J0J{|cA<^Kl

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..965a906400872fdd6bccecc36266c18097cc08c4
GIT binary patch
literal 2363
zcmd1j<>g{vU|@)xtCq^h$H4Fy#6iYP3=9ko3=9m#1q=)fDGX5zDU2yhIgB9?nh8uZ
z=P*aHq%fv1=debxf@!uWHZaW|#Q~-{qc~GoQdnCUqPSAnQrKG<qPSByQaD=}qIgoc
zQn*_fqIgqyQg~Y!qWDtyQutdKqWDvogBdghsw9&0^Gb^HbK*-<i%S%soSf7my_X<Y
zYBJtp)bdMa1hJtQ#Aan+U~mRGT7ZFpp@d-pV+q3orW%HYj0+iq88jLFG?{NP<rUmw
zODsw*&dj^TT%4Jw$#{!3F)uwQbtOX)I|Bp5uXO#4{M=Oi^!(z2{FKBZFwY3V%_+@G
z(Jv@UtjH|ZFD;5M&de>yNsWiPPd_&?FSDRDC$S{ID84MSO1~&SDZeDXxTG{KO+PI^
zCnvu=KDo50ELE?d@|JW-X;Gdd$m)>%fSl4|-^{$y;(*Mc#Ju!WVUQ<4Q6Rur#L2+G
zkjxDAC5QsC*<e1^0sAzIv4$aqQ3T{?rV{3bj3Nv*AT|qxox+sPT*8{d+{+ZqpvmI*
z^8f$;|BFE3wUY4`Yf4FFLF!7jB9N)KSQCqi5-W>9;dqN5?9%vx#G=I9)RNSqTg=J%
z#kbhga`F>PjEz<@-(o9DOvx-QzQvZ2nwg$aa!W9|w5TXGuLPk59HJ0{gMoqJmUw!;
zXP%>TW>HRRSZ1P2YEfocVo7FMsv^w0I*dhPc)hCy_bw=EA>KpwY7JvLa|uHhYYHRC
zvn!eWintjV7&IZ?E8=5dU?>8mjUs*!3*^lrK@dv_M1Z0n$+0phjt$5!&Me8y&vPuw
zFU?DdQiHizi?K)o<YE?3%s?@S4e~EIDeA#o%mj8PEP5EhJZJ=oGa$RVNF3xEUXZ(4
zia;r$NCXsk%oXtlMWP@nG>0nTai~vbT1je3gf_^zpbRg<$iv9;pM{a_e~}c(*`PEA
zbu|Mh1%NQv*;Zg@*D#haW-&=HG&9yR)i9+nN;0G{Wiu2>*D$3pBe7Xv>=f2uh7`77
z22FN9Z~{yJv-0C5-=qNhm!RbK5@cJ5n<n!u=ES7pTZ~1wm<!@lHJNX*g7e8O#*$l%
zsgM`|dl3|dx42S^it>xV#ef0G7a&1CMioXDMi!<bWso00M#KC73N?7%kYEIr0$Ggd
z3@MC@KqM2GWCoKgix|^cN*J=3QdoPLY8h)7K?PJRQwn<;lO#he6P(YM!rsaR76T<L
z#uSbch6T)^G6kI5SU{{4HgNPoOk$~Fu3-^isAVl-$O4r=U==VuDV#_u*-97|u-34p
zaHTOVWUOVYVXR>jV5nsWi!!CKfkl~W*lU<17-~6ExNA9UI7=87u+?y+a7i-MaHOzF
zGAv{gVW{OQVXxt8W-MXI;t*%3;gVpe<*MOI;Q_fXi$jE=hEoJ0%3Z@<!z#j1%TvP>
z%%I6zHA^8SKQk{~AyFYMvm!M`Avv=sIVV-2JhLQ20aU&zB!bhrLIs4El$xAaTAa$I
zprD|TsgR$SQ>l=bnwp}JpO*^LnwXcOP?C{ZtdLo(keXOr309j}ly0R^te~q<l98$a
zEg}@)rHVpk9#{e@P?VXhkXopx$?_Z)qeUtV3=B}-hy7rlCJUG<QUzrOkTjSJ*Ym+1
z%tsKKEa?xcMAo@Kd<4-C<JlL1vN5=X0H@ZMpwwRk$|exK(hLj?>9b~nz)kxi8Bm7g
z1ecSLf&^T0fXfh2k#dVIqqrn7Ir|oSVo`EQVxAGW;J(FFkg3Ugiw6?6kdhI?i_gf<
zjn7Rixy6>8ng=dkgA9^yi9r=+B_`)5WhUmuL-@BuAap!jJSnj_^_E~ker8@te12Yh
zGE5;qk`P!8C#aOpiZ4q|2A3tb7>jQ)r4|<{fx-@)%fJLESKShW6(q^v^4SaO1`ANO
z1JyHJTx^UW$n=+mneDFtiwL6{GY6v*2OlE`BOfydBMXxRlMs^_BL@=;W04ssxCCyo
z$H%ASC&$O%5(VdRNL{FxmS0pJpPiYPdrJgaI5V#dBFdUqnp;o_4(nUY#g)akxQa`Y
z3X1ZRQ;Uml@f9a$7F6ox7nEe?W>%%%Vg>U-)lptbel9rJ!L<muU<Ma+;Bo@&BBas<
r;cpI`-29Z%oK!ndNm2}oXBGwyCJsgpCJt5(MlfXM;NxHgK^_hOUVcG$

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8bf02d5aa7da680701e2164359fa19bae13fb63f
GIT binary patch
literal 4189
zcmZ3^%ge>Uz`zhWS1py1kAdMahy%k+P{!vp1_p-d3@Hpz3@MB$OgW4p5Sj^0Gv_cz
zv7|7jFz2vFv4Ux~C^j(79>oEsIiomJSXvmOxKdbK7^1jS*jgB(cv9F~7@~MnI9eE@
z_)<7q7^3)7n1dNKxvC_R^YcoI@^j)#Qj1Fzpq!l4BE6R&=V&tCV$||WW(2XJn3aKn
zfti7U;j<sunI%YaS#UOrD9mFFH4Mub85mZ>RiW~N88jLFG?{NP<rUmwODsw*&dj^T
zT%4Jw$#{!3F)uwQwHTyP;a9qTMt*LpetLd!L4HbN5twI$;O3O(rRWzFC01k>>z5YA
z7iZ=c<fO(!J*J<Vn3q{lnv+<PUld=KS*2f;pOjw`UtCg}mZqPUpOcec9-mxVRF<k&
zP<czbq_il{5oB{nen3uXv2SKxX>mYiP-0$sY7r<Ng&7zailrDB7#bMva`X0BP7Rt8
zHp6vd?3CDx+{#zDl`pU;e+GqGGAO)23=n3Ah2I)H;a9_u!dS)1z<@m*OV~gLK`?3<
zg7nm&>SBSYV_?9qD}^bYxdh}XsPZ+;%a|A#R>R{im_d`p@8$ph|Nj?(#8xuiVofQj
zEJ$6+Rs>2Bw^$R4iV`b}K)$=h4-Ufkg2bZ4+|-iPqFc<#`Ng-`({l0?ON@<HGT&k=
zN=(TtExyH;k(!yFQF2Q#xwNP#HLnDrr5F@s3JMBEoD2*Mx5U%)J@Xu$GmCOk!!i?H
zQj0Rn5=%16Qi~)R7#I{mvET%c1&aw`Gm<7oFEv_XwpRb5vegx3tBaymJKT;29SJ+@
zf6+DOifhb8o0to6@fSoAu4d$4;4ZkxU2uiF-~vlQ5jO(^1NPY3fH$`27f&^e>C7e2
zI08{wpwI^~7}hYN$Jk0Hzal;c1_n(?92E&NFfbH>5^a$Xhy{wPA`uWv6cmIgAufXw
z;sN=^nI)O|d5%T-rFkh)MKTNw3~C@fj>y42H)&4V()f#VrdQ-lFN&IOC_Bn@gzaek
zMThV!4&fKg!!JZaf;{U=M$rZC;)~qHSGbEWuoOc=9iC1Z7#Kj24sy=tZTLc+2`#0A
z(j_<~@Tc?|Ml^kJoAK+bVqjn(KJ1I6K>;fO3VoI$F%VlEl#-b%;th%<KtTtxRRNyD
zmGFeOPi9(4YDz?rJOcxRHb`qIQm6;dP(w-TQdT?MZU{-tHJW3#RR5y1(G_W<i$X>p
znHg=3KQJ(gn%uRv-DCBEnUTkx@goC>`~oIFFfg*ZGeTkl9*ztQ3?Ns6W1<TqCTbW<
zkW^;D*;QN&Xytq@Qw>uJBPg$eqlY1dDI3fvlB!`!Va6iPSHqOTf<>Gug*BKVg)Nvt
zlid$o45j~B`SFr(Qh@zSP~Gqn<c1J8P3Bw7iAlw`7>jN(7sRJ(GT&ka7xT9mOKvfy
zqLwUNsYONkMc~S>NRxqq!2lG#(87d+yTfrR;}q6T&rZG`z6Qq|>>M3}EhQbPO*IWQ
zGZ<$m&S9BRI*Vrp&kDg6hHFGNu&$C>A#;OYxTE9&zeq<ZBrrkl0(+K$fdQ1^z=8RW
z2~@+Q6dG`K=?p21ix|@xQkcLbGniyq#F);4lI=lW1)IBu6*b$}GS)Dru%)oKGNrJm
zF(Cr6mI*EnmPL_c#_pCB4is^a+rf6CSLC2-2Sp_)*<e+f!iJWGaQlO$hPj5NiiLrp
zmKDWpcu1j$;<OP(6;2<da1!k<HWU-##cvH;3RfBvYIR)8R>N4s2KECx4qZ$sY#6$j
zYS?R-K-s94BZa$`vxXBzFTC=v;Yi^^#BU8p3LAolnkGQ;Q_EGt4~`NrQNxAmQk1mC
z0an7mzyK=pYPi5I;i}<E;Q^V0LnSK%Lk%ZL9;%+ZhP#Fp#INP4;R$BY<gJ>ekdmL7
zm#&bgkd|4Inxc@LS(KcUs!*O;lA!=<ttljeYZrwI2rnr$IkB`jl?zlxW-8?8<y0!<
zrKYAR<maWrv?k`ID3oMm7As^HE2JhCSAx|h7NuJ$6f5W|lw_nTK-(1x@TQ1DW*%4q
zDo~V}tdLr$r^)gh=H((#2@B<Y*bnAuvVf@~9Z)F_k_L0(dOp~L`3ORjCH-NQ$U66j
zk0APCJo_SDkO`m~8C-b11f|9zUXU2rR8T`Eeb!76xM^Rc1gbhX!Hp|O#SN~hK}86-
z3ctmcQCyOkoPCQuu_(DDG0zCxBE7{_kg3Ugiw6?6kmeMG7oU-z8=sq4a*HiFH4oe-
z3o=N)B?eWPm6)8Ll$n?p58>Yufza`A@ubA!)LViD`I&ho@%ef2$uNcdNJ3yWoS>#%
zR(x4%GPu^e#aMicDYdvr15~bpyb6j=IEJ+D#9(z_GPrr?1$9f2Edv9C1xVj4Mo<&`
zs|bUL*c6KkvPKYeL&op|7~PODxBx~sWQ;F>(Fb;BS<|m<47T=rygslqi+C`8WB`$0
zz+{8Z2Mz{4sTqnh9A_xcD4d~og;%k`?FN_Fgu)qu6N+az&Pbf$JfY$WmqLT{S2hMY
zg*g!miq<-<aob>XQODtmj>AQDhoeeI)K17;bPBuT6m}sZ@`75_)ufawNjVo}b1%x~
zUXjhcz#?@+TJD0P`9*2-3oH^}1sEiy=Wr}lTB5c>=Ax3>6(zI1OncaN)LgXixnkjS
zBKd-1(AB7zD^V#IBvLO*q+XFoy}%-RS3+tI=LKbx4T%?&EG|k|UXif8z#@7>OzDEE
z*+nt42LG>Y415ATku%s9Fixnx$g6yXSGmFMhJgG6wTl884W18p1sdEQaPm)8nxZu$
zazWvom=%^A0@v7F6tcJ?WO0$xqQUVghw=i)iyUfKIMmh(tr5M*p|?TsB8O3f;|)%(
zj=)aQsX|jkJ7s5xUF4Kspm>o}xxwiH2iFB&#fuz@4URXsc^W*x^_-@_E%x~Ml>FrQ
z_*<gj+7{BO(o4%PDv!_3%*(wcf-IbwR|XMf%`43<s01hTTg=6k#kaVMOOpzU@{?1G
zi*NB2CubH^>g5-dWaeg8rQTu%^FiH*yp;T0aEb?a0l*DXaQg^ciGx$lN(QjQz)9>E
zhfQvNN@-52U6BU^0|TfADL%o#!0>^Yk&*ERgWLrMxd#l&7hvcHgVF^UdcYub0fs(s
zF)(sBaDm_r2Ehw3bb~?k0u0??;B5fI4_u6lDjyi&gbX8NAtQ(nC%G6IOTZ#<l8cGa
co$&($63NBNDEff`P6%i-@_t~zB)~oe0EDEavj6}9

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..83515247013d576e7357d50aa88d446cd98cef1c
GIT binary patch
literal 11436
zcmd1j<>g{vU|`5us+JlX#=!6x#6iZ)3=9ko3=9m#J`4;DDGVu$ISf&ZDGVu0IZPoC
zK64Zcn9mC4Gv%;BY4#8XP6l^|6y_9`7KRj-W~L~P6xI~B7KSL!6vki%P4<@{d;ByR
zZ!u~mgP6z|Zfzd}14Am<$|$B3r4*(V<`%{%<`k9`))s~+mK3%W_7;XH))bBu&K8C!
zwiK=u?iPk9_7t8J-WG-^jugHWez1iCDS|BwQCulPDZ(ubQQRpaDWWY5Q9LPPDdH^*
zQM@S<DUvM=QG6*<Dbg(rQT!<~DY7jLQ35G)De^50QGzK7DT*x&Q9@vcE8pS_%1_EK
zi7!el_e*9%ayA<S0|O{Hy+HxCijje#N~DG<g;A2Bh9RDzhAD*!%wnuzN?``Gm{M43
z7~+}38S>;<7)n?wjTjj~u!OaSshKf_C7Y#4tAs6uwV9ETp@cn!1w`kurLckYq_Ed8
z#4|-Q<S~{rq;S+Q#B+efInzOQan&$nF{Q9)GZlH2aHnvla5Xd4GMBKWaDz+$%kgA0
z6-AVAr|`n%_~3H<*-S-gCEO_ja5+J^oKQAXQAr7RiZEPG1Vye1hn#3OQ_+MH?i4XJ
zeas7Z7BVd0O%Y$nSj$qwvVd<P10zEXiwi@nN-b**>jK^siG_@{tThZ-TnqS11WE*J
zSX0;}8JZcvEEYIR63&tWv1%ByIGb4)GBPq0&Mg7!Zf2}uh!?71ULXvz0UTB$H4F<x
z!90dw22E+dTWq=cCHY0gx7dqQ^NK+Xt|D+e=OyN*zGP%zV5nNGkeQ}XoSCbTS*%c!
zuaK0gkdc_8qfnV&s*qb+T%u5rSX`{YrGN`S%(sos$t*63&rgeo@MH1lP)N*6QGhxJ
zp${q$tB{$e0CHT4LSj*Ru|jcWUP)p_5y+*t*z)q@i!*a?v80wI=2Y?N=_eND7VDQ5
znWo04WZYsbs<O;f&s8Wb&df`<QYc9+F3HSGk59>nFGws(%uOvxEh<*fD9^}D&QM4!
zN>vCAGBik40Lv9?-eLuFt2CiT8XADiMA8|bR+OI`Uy_j;Us@DjoLQ1urI!h}tF(x)
z`dgeyiN&e$IhlFcRWe{(vP-iQ<8zYA<1;hM6XEVGu2Mqs1r9a0II@iL%!-W-464Mz
zX2G~f#>gTWgDHJWD9fnW9ApC6N%0`Z!Oen7z=E#$79-a$M(dZLV&NsI6w?$dVqsul
zC}IT>paS9+Gbmm-yi+S(i;D7#ZZYSi=G|f~D9X$$xy6!Nky&z!B{x4M^%iS#PGWAs
zEw-%u%)FA~TP*nnsd=|pic%9(Zm|{@<YboIVlK!pxW$%OP>`CJa*Ndqr0y1LT26jq
z$t{+=l>9W15&6Zbw>YvAlk<}@6Z3BIBq!!1dqH`cY`0j7Q*+X8F(p>qV#_WErJ-BQ
zX_+~<*dQ@|i?LuOLy-;x1H-Qr{fzwFRQ>e);)48?#3C@y2*J%M%}dcQC`zoz1f?-h
z!Yjy0jZe<cD=Es)(a%lH%Pc6(Ni4}PiZ9Eo(ud@#;*!#|H2t*vqVo9c%)DH^g34PQ
z@$s2?nI-Y@R-ghjjDdlHiIEEmc^G*ZIT*Q^*_hZESs0nVvvIRAvN3Tnaj|hQfz)t-
z)uFH<B8+@Y93Y*HHjF$>987GCJd9OB@B&;9W@R!cvS4L7F9QPu2Ll6xGsqn&j0_Al
zpyGp}hN*@jo)KKcGNmxqGM6ydFl4bbGrBOu&a7prVaQ^wVUc9WVoPC6VQOVcVN7F^
zWB?alsN&3EaW)L~EMReV3~?r~I7bab7Q+Je5{?C&=?n`Q(diV{g)A|QwVVW0v4K^A
zDhPaLv4d4{<2S2@A&U{Lj;Dr!g&~WnhO>sNhP#F*oh6;ooPmiUo~Z^DJ`lfyY=p45
z#Tm>QYMBcKz_KjTpbCp81ylrc_!WW5t|B2&R$&Jbf*=A^quydnzr~oO$y=n(z`$^e
zD>*+WzX+TeZ?WcN=A{<jVl7I{OHVCQ0%?!}sbTZT&-Df6go2#>l3Pr91-Doei;5B}
zZ!xA6flcH~EK3I)5ucNqSCXO0Tcifk46>le5X7<s8N--$i?t#?BeCQbYbAuL0#QZU
zAW1fm-SHK-Aao^^u7c79;H(27bV16wL5bU?D6u><FTDbkvJ@B?7}(g@7(pqTk5K{&
z)tJ;6)fhDxC79$G1(>Qt5or}ke=;bfV96Dfbm7TWiIIV!gkb?=IztNMA`r<0CYiw`
z%Ob{fmW7PzjJ1p<Of?Ky%r%TDtdb0%c2p}9gajqF5{3mVDI5zKK}j)%Q-q<Jg^{6z
ztpt=bIhq;42|l)#sfKX@Cq#b@Ll#F3lO#hHmjDB()-3@g9UNxYFl6z7)TS_`u(pC)
zM93~+2iXp?wV9QXp@bLY!W7mT#uP5N8zCw%^hhv>Fu+Y_m1Mx8harm(WNM)yII(g2
ztz;<zCo)h-LJ}T`wUPzGMwNnzf>a=Mp@_nzUjG08|3AbKP)c6O2uaYOwEXhl|Ns9r
z`HRdz>B}6HCiqi|63hLIGE?(P5=%1k^B@VFwJNnJzqkmL0*j16YDGZ=C|wuHgIFdY
zt-@$p9rH4Ci;O@rd?1;y)Z~)<BCsGRIj>{}r|(;ACHYQ?#i>Q0A{LT1K?QsfIDztl
z^aYnBmZXBTO#vlQJ_ZH`6-EO_Ax0@iH^vA?mcLcvhy;nE9wkYFYB~@Gm!Jn2KuHo*
zDmF9KGS)CIU|h(sfGLG}AtN{uF@qXmB@8Kypk6{PV-3>+7LZ5^Yb|pLYY7{uF$_*}
z#<eUpOrR2W0aFUwLPikV1{CS+ATdw^1kow%HVh@KDI8!OlC`Wg%q5&4m68lKte~bL
zsO+g>P2tRDF7mHoPGJM_3$1FHQ<x#FLd6=!1za`Epk!RbxB%=naDw6TyCndrxxlp}
zxQzrZNs9PEVa`~7i=jvW6gi-j^AcR=gQ8!PyGR-&Dgz>9L4-1hkOL8@QG1KEBqOyX
z@fLG&NzpCV@}kU=R82N;q!np`RBM5(;Y&(P&W<n1k1sAM%FIi@#h8gTCi#jJ%Tk^5
z^U^}{-7<4h=YgUUR5o%k@`39@4n`J60Y)K45k?+H5wHjs6Vq=lHXg<*2}Jaw8iEqP
zA`A=+AdiCMmyH<|zgeKdhH(*N38?eI)XM}eWEjAr%qXHvB@j_iVZ@Na3NDz~z$81U
zc!KG#WrnKbK<EZ_eM-PJEjWG^YFWUw2pcHov)Ba~YME;o7jV=tgE~E}Oex%;;shL%
zOh}?!FwsJ*8m1H;Nl+aPropB|+{TNra{(u)8@Z6FmMxv3mOY)JmIKKJ7PTxT3=6nw
z7(p(mVTGhL35JDCwVX8^3pi>x!M1ZQ;7s94;RlO?Rn~IVurJ_%=mM+Yf~(*vVOYQo
zs!P+D7BbdymoO~gsbQ<(2J_eot83U&I8*pqnNs*u7+S$CgBpf-P(jHH^)myKIA1zL
zEl)Z_EiW`AvH6NGg|n7Fg|k)w$yZZqS&(=MwSqM)HG<iU6Bvu+!M+9OxEl5vK}m*%
zOmLU;Bgt`~$q9gcB~Zf;_J<JIS9~=>U>;xLTd4bxVj9g<4siIQ+lXdr3QvuY2tzG~
z={0=lVOGlr7UODVN`Z%0t#A#`0*)F!aD4DC;HVJ>v7j!)8ghltYItfG;`wWMYZ&4M
zz@>zMpC&V;?1$F}D;aOGrj%3`q@u`xr9ru;$OV+ST|tB!0|SF5^K)3WUK9%w1QTG@
zpyH$mRE-q5gA}qM*#nkG5TKm-5>)(x>ncq_qzZ=}+(9rly2V@&p9-m5Km|h)xN-s2
zE8rUD7JE@@aYkYRs2ft0UtD~Pt*p2tF**AdTLzdeQUMi4>{*G)@g+r>w^(ws5|eLn
zfJhKuQv_UW-D0VV&n&*hQV^e6T;vb(3%Jrr0BL0gX<*5U&o92kk`-T2e2Y0Nz5qnz
zgNV$cG?4OikV-bNsaYU9;`6~|K~W?~f+aUAKJykAL>go*T6I>W%D}*|8&vXvY!qV@
zW7J?2W8`BJU=(AN0GD|(j4aG7jC@QIj9lQ}1s@|1BOfDJo|%P*g;9oyg%Kpr$Hc`b
z!zcrmlVJwY9E<{tBFrMpT#Q^Sa*S+@T#Qv>h;kFTmxWSlazY1)K=EG80P0sTWPxfh
zhAc(_22jlisv|)?2T=b3R7!!GX$zQZ7(q2<3DW|W8c><QT*Xnsl*PJ$4J^(Q&XA|b
z!T=J<Vy|>zWT<6jWT+A@;YeWy4b7B*@+V_Ds42%K!myAroPmuYk|BkmmbH#G7)qBj
zmNTR?)Utt_ZCv1zRi_rz*yE{T2MttZf!zn{4S<_i?2=%<I72N*4F`scIcnGz@TRcT
zFpDshFfCxN;izE&H3~s?GcpwVfZS8Vwve%wt%fa_L6a4!&0nPu<nQel>Y)&-5bWva
z9_r&5<Qd``qKE)Ro}h%!2T6U5A>d?vi?yUEF*)@Xb54FbxaFOglYWaiGq3Cxw@+rC
zV@^7_n_VOZN>HFC7o?8>u6n_V4Ah{~<b)(xM&DbE*|%7;<3VPz`o@DyVNAWnUzA#0
zngbppNzPAAD~6;$9^d$s)S~#L%J{<gOtiFjOE@_(C)qb7-_^Y+F$L7Ri_iAuXJlX~
z24x&RCJ|8Il$nVgR0ne~$}w>?F#Tm==VMf16=9TP<YHuDs**)bPiSVLBr8w@1k|Si
zC#x_LlT{59C}E{AEo2m705!O>*cPyZnnMejKqGQBi~<ax2&`o-Va;M&z)`~riaJn5
zSOV&5LK2I5EvR$KUBiKqSioYS#4XNH%UQ#Tkytou*cb5Bu+(ruYylOxh2|wp3wUeT
z7c$mzr8CrWBiUnB%TdC#fUksU0e=l^4Hw8hPz3_w!~6iFB^YX0L2@8m%L8>g&qBsp
z-Wp!0D_CF^Wi4L~_X3_2R&a9Vt6>AV7Ze&`*JOiT!@UqRWX*<@SV3hI0|Uc@{V)H5
zn){$)SyKR<FpEk-Ne$e@F9VfR%&C>Bw^)mlGYcw<K;6Du;%P;R$)Lu5VorQ+Vo6bE
zMSMYid1}!umYn?bT%<A?o<57f2^Ta<UX%jL0sP?PT2u|Ps0L&QYjI|JZsIL2Fcn{1
zSX6S0H3Zz6<jl!WkB2Y?!4x>rmKT+PQ!S_<Eouar!01}k0HXPG6HDUL@{8iric*s^
zQcG@eLm41_w|GG*9^6a?YX^57zy!GefF;p;nt~ENNC_m-^D*)<@i6i*fr?cXMiE94
zMlL26Rz5}%Mj1x7|2#|}nu|c<_e4qjpn)||xe8AFGZ+}a<teDF0!1VCGBur{mZ^jZ
zRz8+6XR#~*4H7dfWCWL(pphANa4D!*3mU=Ts9~05KvpHd0P2Y>;7nmDVP3!mmf;4K
z#h^q}!VF5#AQm|JsFpA<;02Z1Xl30kmV(T@GDt-ZDbFCu23$;m(+()96ioufFu1G&
zSJLnVaf<~U$IL0IB}Je_22LQFtVqcM6q%56iqR3ha1sC)PM}da-^2<~0Tm94BvA7L
z6cH?p9E?JYpz#O|7A~eLDYSA3NgGN$pbSEM1NZo-GYHYl%*X&9#AgG?i7&X%$c{Gf
zzzM2j7#Z*lYCv>A202o|!w6uLK_gSl;6VmXq6QflQ_$5hWO0KAND8&VEkG{6B2ZF9
zYu15d8q#0`g>#VyC}P3284tJ?14m$y7f2L50tK2vLMjPBl@y|hc8d$t@rHEpA!Aj3
zAdR4028n8LZsY_h0{7;#K+y~7$!jq3F-kESG5Uad^1_Iyg{wu0Q&2A!)QkjqsCX7M
zP9b4j!jQ$1!q^LHkAT`1tSL;{Oht0g)-t5O2bKrLRt=LlLoFkC0ErPge53@)St%^p
zOhs0p=^9WTTfkMqUBd`sHG|CLSjfc4P^f~Cg~)*hn_x1a=_5u4aJaMj6-9$W0W@~2
z$(F8F5|=!EMc@AP<u5$+{ww~pe-35rFPh1~z>uETCjH^3BIf}}BOBCU*5pOR#4SG1
zKs$Iy9UOv?;rn=y`JnKC#0+>)MiL|jYCPW(D#`~9oy6yr=B5^9CMV|PRNmqO58{K%
zU2t<$lLeebZ?S>P(BfN6g@)iv3W+vw1cKvEytE)Cu_P7jfMUnI6xf)3F(@WM2}q2Q
ziy72$7hvXMlw*`);$f^(M8qZz-6)flp!^E*3pj2;W9=mjC5$zQ_yG+MGNmwPGZo2z
zhT%X%b_-ZaSZf$TEO7iVfnxzwS~7#g7O+6%Kog7*8Bn~eWb%V_LfEhcTG4D!@XcXh
zV1SNK70m?+fU6~NXn+ZDkZ=~KmO#Rz1r!LN_KpA}#PckFs*sX0Tph}gJ}4=}$4CvJ
zZ6vVwBx@K;7_*p47_*p>F=(ziovnm1izS7n7m|QMgHf;mN&qKd_7v7^rlL?#k_5Bt
z5Q!DsnC1Z`<}6-F!WCy|W&~FX^5AAPvn0a;K1@|LOwtS>l{E}m?BHMospbHio{caa
zW&*-&bdw==BB=wLi^bIL8iob@-~l&=U{K{LkiwSDR5Y!IVSyk_6cmw~?0!X{yamoM
z>GyLLLVqj%w1?z5Q1&STRl=G=MW8f=l#!SUGK;|Z23(6FvWze&o<Wgvi@P|rBp|;y
z6VxIq0wo1VK^F>A08SqBK`aRn0qW5~vk*9;+~Q5GC@D&ePf5%z$jnQ}o|QmTOo>G)
z-kEv1VVQ|An~HlusR%UW&&49d$ic|;pM{a*zW@st6BnZzqaG6%vl62g6BiQ?BM7tn
zN6ub2OvIhNK;v&H*~_gKk-dyS*$bQ!K{3q&&Pw2v24X=|3@DSagZK+LO4uOT43PzE
z7#47XVyY0NiWzK@O%1~WF1Q#}Cqy4Cdm?m!Vz+1ks9nj8o=4%aRI~&X{)pHCM+_)B
ziomh65F`uEt6&10VP%kl$q!N{7W?PrRJx=lm8P!&g+8dUBfty_aVbU_P+yvvi%Eis
zi;?9+l^UWj!)FZcAO|&zL1W6`79FTIVFI@vl)wSc46d!P1~(`&z}XJey@5ssJm8oi
zG9?Vqwj&x{!?1uI6zHG;)nxIjQnMurfXm0CU{C~bK>DfRyaI0V!K)WUNeQk<im-*a
zEWV&JIs^)GP~E}7$j1UMIGH(^K~tkVj8*Ey1-B7Oc!P31D7S-akO|P1cP&#1Xle;o
zObXO8*D!$!!39t{3sg%mOEQRqCyEL=L0vQ4Q&lCbS?npSy-c+%U{jf(tq0Cp)*6-?
zCeVm}76+&`UT9JSUJTUA1ZmkpM;AD-x5c7CsTDk01_};MPDp_R4s=ifb&CyBlNW)D
zAaG+G-2Va>V<w<zVh7jakRpg3GC^+yDSS|exiG5r)Kpmc<Cs>GTI7~rR1PYaZiAu*
zG<m?rD8MMksKF$_B)}-Z$bwR-<Isyz%YmW;6fNK=DS|fGK>ghm=7r3)%r(p|46(|!
zEG0}O%<yRy5%3f>3n(zb-5@Y4iwzRIH7p{qs1gJv?;6Gw7Dy(jVOYQcO6*WkR*)z-
z;lp~#Aa}e3%@f>WOUf)REKMyk0EHbVxN^9~k&~HMT#%TYid5u)no&h7L6HLvc~EB#
zoY-O0AmElUsDW^cwKyXmv?u~ZK>{9J?tlq!;Ukz-R0^5Kf`#!5P#A+6!Jrus0Y)|O
zOo#wul{6yhW9ZRj^wSguElU8cvWbts#T6f)o1apelNuj?izhz5urx6TB2%OXa<vnP
z2mldLAOc)GHi1|ZLBuo=0rnHP$Xo>CE&~zELBt9Wu^L2d1QB4LAPA73ia;$k3($Bb
zsAS+^;9}yi<B;Lt;*jMC;ZWmH;t=NG<PhZG<dEkO<B;Ux1*=a6b&p{K%AnK^iWP9`
zaAINr4f?S{>ZL4L*5a>at6>FaRyIk76vi6RyehLa19bR~vxad23up`o)FsPeOJ_(y
zpIwGb=&*n@E*p3fh&_cpoe_r9nQJ*<Y!IE!T+0cfVK|++mJ7xP(do>fDG*S@6z;<9
zwQM!)3z%!zQ$XWQj9E-IY?2J1VgS?vf{3QDr*MLb0uTuvM&+sDfr)|mD5^L?BH|3S
zyl^$(2_md&_~2^5i#xEY;ZNbHVNc<fWPsWTHXl<Zc*Yt-oj60SKnX(%C%Ce5tQD*Q
zPoykhuHgkws4y}z)Nq1W3z%#8Ks4CTTp-p0<{Eww4Hn@Bu@*4b2!Lp?2xAr}D4al@
z*9BZP+$o@eU2f34Iw;@PAj@*saD&z?f^-$0FN~^TNa4z60!{tYFoK3QQg}c*Ksg<R
zHF^CYQxc#wh2G~yl(V1-3&=D$DE)zlalvz^n!H8uVi!^hgL=BRxREL{aB%_Y9>WV@
z$TT-cacW6?N@7XkEkO{=I3>QID8C@Js3bGBxCoTdG=*-lrh(S8f$f46*W7uam3Ls#
zV#s7WFQhLPU!0SfoSFieP6ZX?w>ZHIKR`Cz;sh`LfH1)eK)_7a0x-p$k2HU84a&VX
zAi@?zfYym%Eui=iUIb0He+QLLpsI$2QHUAT7Sdt_)c_z2ntGRDQ~^T=Mjl2XD3oKD
zVr2Tl#V5eX^1lc)#jeS9i#4w_x1bUnWVe(cv(?~rR(j>BN%<wk@rebQ@foQ(1*t{x
zX{C9^w|KzAl6pCrd5JmcMXjI#KpA9h;JPh7u{b^>KNmDsc}o~s7Bpx8nuJEt0u=<0
zAtr*BB!G%7u=|i|LvYCkD%5Uq*g#5DJ5YUG3~J?bFmSMPFhZIpOiX-Q4D}$$;{*VL
CctSV;

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ec97b7a1cca924c836514e165d58713cb9915c19
GIT binary patch
literal 27512
zcmZ3^%ge>Uz`&5RR4p|&jDg`Xhy%kcP{!v71_p-d3@HpLj5!QZj42E$OgT&;5I%Dh
z3z*Le<}>B6L233722KVh26u)O<`#w&mSs!~46B)-<}pNZq_DOyL~*7t1~X`~y#xvS
zX)@ko)Jg_1VHl?Ta{<`;RIt@iOesPsOf8I2%pilKSW;M87@}Cg#<8WawJ=1nr?9tx
zEZ_iJz}dnO#g)R<!Vtxs!rj6U#goF*!Vtxq!rQ_S#h1d@!Vtxu!r#IWC6FS}!Vo2x
zBG|$ZB?NYs@GZ`u{G|Mn_@cyezhszOKyKq<U|`^8U|{$h#R>`*PKFw$6h=vq3>eoi
z#Di%NQNxtNgd)QT5(VKJrW9rr8MqB8EHw=A%pf%&oEM<XP{ILXL9hx#kuxJh3CMnk
z07DJaGDZf5)o_zjShB$pS0rD;3su6vz>va<s)~`Jgda(k1zk3eFNF=+9VzTJ4DoO`
zMl$5FlrvN?M>3Q%rf}3S#KT<(*2M`kIh~<~As(E-Kq55^S#T3l*t5axBKZ<=kQ@Z3
zaHepfCY4%d6kl+oyA#Ds9;{~a5-^hwubKQ<%@iPDrXXH3g|M0_Ou$SLx|=DA)tzD_
zyOVhVD9=Em7>QiQz`(E?&R>9(TH%5WDdMQUu4SoVS-^*oN3c*$V`Qjdabbv^T+3R+
zio;|HG?Q6t7_#7LbpcZDK-f{Dh{RS#uxnUT*buoH)!Y(Pbu2`xlO$4|6fSi&3|a7e
zxQvy7VKpnvy{PVEWaycKQl21%0;<XyhIk>E88yrcgkfxC8a)=k<$(xHMGeCOQ5YMU
z#?ZwO%%CalcZ)4Iza+n?_!fI{YF;sj!Bqq<lk*aDQ(uBgysE_tnQ02enYjv?#R?_)
z3Q4I78Hp)63YGb#3c01lB?<+J#l;F-3b+8oeB0=p%;J*x{IqxoKNgP;g~YrR1*mfn
z`k(@_3YmEdAjhRBBo?I?D->7el_XXafl9htY<c<d#hJOcSW?RpbE^3C^b?D6i}g#3
zOjF}iGHx*zRas`L=PHyIXXd3_DU_rZmt^Lp$ERe(7bF%X=BAdU78NUKlxJinXDB2V
zr7DC585*Q2faQucZ?S^8Rhm#E4GlnMBI%4zE6UG}FUd%aFD;5M&MZl-(#wR~Ra!(?
z{VmR<#NyQWoXouJDjBdX*`?Wu@i|H5@tK+BiEwuoS1BR+0*9Jg99c$rX2r$^236u<
zvtV2#V`Pzx!IZuwlx0+G4l)7kq<E0y;ATN3U_n>>i;?RWqxDNrB)kOGtD0g(><kPH
zMW8yX2$V%`F@xfT!#lOowWuh+=oWKMYThl@f}+g4l3OgP6`3WsSaS1IQg5*)=OpG9
z++xeh&&(?+zQvMXkeYXkr6@Hq<rZskK~84LE#`v!f?I5f1qG>jDYsa?K<aL>rsd=(
zmfT{=OUX|I8IfO{dW$0~F*!deGcoTLPjX^TvKN%6$##pSI5j8j7E@xyEw=1(P#U_$
zoR*n$iwzRfw-^hG*%%lYAj$bxihf3ZZmNEIesMv5N@5Y1XN2J9l;)-A7ZfE{WP;Ke
zDDf5Kq{b)b=am%Y=ji7q=4BR?<|LNn7sZ!lR_Q}>R&hybTAF@Zeo=XRc4l6#UP0w8
zj`;Y@yv&mL_$ncIi%1V9X2rn3P&|u?fuVun0}~&o;0-B-1rk@Jv^rdRyl+S=F3`Lp
zt<&Ms<8wn!WkJjpIin7j9{(HS5*;o*9vvP(?(&O&VBq8wL=boRg(jGFR`*oT2<)ow
zsJ|;9`hkH-+?Vkq1CyX1;}<ab0Ze{nV-OUb5IB*23i|~13G5G~WhSsp;kcosHGyRc
z{|z324wnzYoScF`e*E|##vm%)!P3KdLsYDTrHAW*m_!Fl5BCFZ{vP`o8JD=#E^w=T
z<zWyOzaXx@LinPP?iC^34(F#lf)|8*FY@?Z;qkk`<97qBI{gy2>IH7qyFy|Y#GMW}
zUlj7VBIMEGd>5qF=^~Hw6&~jcJkAic*_XIAFK}yu)tYW7z9?jUMaUXzuGvK%^D8{&
z7kJDeYO^kJYh2*g0IN0FpnOrt?23>X)Lg@hJVsY|j4tpPk*xNDoY5t2qYK<d1Pp#4
zC^B7QlEg%*DN@%36)y=YE>O8BsD4FIy@Txr2VXx|7gr}w4^Icr4MFh_3{0G9j1aQJ
z@rHolboNQ?GngmxOyTKpyul;T<9(e+?h=pO+|s3?D-2fzt}$5{y(D_8>JG*oihEc#
zYH!irTY4byK;e;)oz;7)&$?a+3ce5$dL=0Ar1u%`3z4x`B9kx5r(BUwxyX}xg(vj_
z7(EpcpTRPR`=W@-6%iFsOg`Y??&s{{?Bwp@?%=*5A=$yw!+V2+=Q@YvB@W4p9MV@f
zq%Xiwu>b=DLzA8%!$wCgM?2<&wv3K;%*mjV09FNoN&*o7^8^-9i@t^twGCgx5D%(r
zz^cJ5T2N5}=A|&!GM9h~6EKf~p@ty~-1Y`>QR_|@h6zluoV6@93|a7cxCW^$kOj)t
zAZ-jOj44d5Oeu_MNIi#IP}_t!eatBO*od}|1w|h_(fXKB^l{WMWPyDFA{HQp42TcG
zC4vwdL?T)!=?tjhMNL&Htjky!7*@k$CWeuLp_X${x|9uzOSuN6OWCowlzT9`w1y!I
zo_n#lnWu)Kf)UX#t>LWUs^PBTNoPrCtm0*0NM=IxdTW?*<_dVf0jCOhkG+b4fdOQ4
zE%OAX9&WIi@RV7_4Jt$8;k~64P&=5z58Qw(0=1lrKo%8&+S5g#?$RyB^jnNcn!H6?
z3=9mnxRUd8@{8g@jkH^=IhlE>#kW|C67$kii&R1F9a)eXHjn&VUr=+TASb`%7E@lq
zE!M=MqQuHuj42Qk`4Y?0!A8XAq~?`mXz~_mf;59HC^7-DY(T~^X5C_~h|fqYxy4!u
zp{hVskse5r4P<wG#VrV338kx`bU`u5<&YK(lq}K*Y2ya9GF*xh%QN%Rt3(k^4J6?T
zP;28jBe=D}!yqi$!P3KiLkd(Q@ZS&>?_lZSx*;jm!FN|sY(mMD%8P=ER|FM5Fz|3X
zF@6M-9ga6dB&K*TV4UK2MMQIj;tIzV$}19As9q5<=x`-dN<d@^_YB7=d{+ci7bLD=
zT#&p%aY5=80i6!V2VzoF(x>N4%2{f(#A1chLYpNvTbZ_S>=4?>vxR4G(4L3`Njqcq
z#GEj`DCU1f%)isU!|leP(qeMP{-Rj)6|rcL(;n~$ba?f6&2U@bw!&?L+XZRs3sN>0
zd2B#}H+ZCGq|C`)p*Sb+ik#jCkuB0Y0=LLrF?75r=XjCFvBR&^?}41oijp;z7v(HE
zSm3quXHX@O49}hn3=EtM3=H6^_#P9eDlS2iLuBK0h7`s{jOh$1Okk23OtLIuOh+5W
zN@uKPEP+>>psbYzG8<$mLk(jJE28V1!j{6`$^;>iD>M}Q;n_5W1GTb3&h#mqRjdpQ
zs09rp1M(mi^1xdSLl&s<0d^6pnW#J$hS+DdOf`%PK;Z*cjZC2W1=-9Rq(%f{aHEQa
z0j;`aWI!H<t6?BDoN5@d;Dr{lohb|{tgT3HKy@i3yi?ebLmW9QQHL!U8A_xPzD9Oq
z3Tq8x3KvFfqMC~>zEM<EaWOD};tr=@kkShg_Az9^hpEv0(eoT!uygyZWPudupavzR
zkO#3=vOw72vK}M==E6k50&po5QMlC0|NsC02kQdY8Y>yW6u91a`S1V#|C;<omY}j6
z)O)+dpHh@q?q8Idnpcuol9`_esZ3a_Qj7A7i$JA)kr_y>1c(6DAw^0cmN}@%7Dm(R
zn3tJbWD1hu1IdJ?CYR(FfdxTT$4X{!U2==9B;P5qIJL+YRGNb_q=JG1qzd2#=?X4M
zEJ+1vsuD+30w@wwKpJO)tAV>hB2zSGc!J=9N)X(j34%K!LGXgKJEVtpLC~{<?JftX
z6T!YfYz5O2sTGbZ5?45HVBDa%fq8}d6-AS+Nn0{@lx)n~l6N5Gid*D`nAnSMu@^bw
zu5iR%;E1~`C^ofthRGc3i{k26#MLhfYFrW2_`tx->B$5kL46;ge%~(N8EOlJ=4dS_
zSyH{ha7o=2b(@QzQquMckL?8>+q*pS3s{!0uMpe7v_@)!<A%fy&Y*tL4(1K+S2S&Q
zr0mH$A-E^^g!74@6V6xc0xkqaT!@Um5)^YGHts@v!iCs`3yDb=Vw0`}C0|I*ypmdR
zk*D$sPvr%k%DX&LGfd`~FK}9+wn1o()`q|hg&Tr*2yO_yqGi1!?LZL7;1h-?gii#X
z5WQmOe^JZ-qDtU};P5NKu@~avuLLJtNKC#GoN^&8=R$7&m9&Bjg+&*NOD+_aTqrHO
zP*`>)t^7ho%@+oSN=6T+j|>dFo=jiB<OgJu9T6NwZVU_zC<QyHF93?`&p8+cJE+Hl
zHtNq<!?*yc=tpFG)RDpkpcIdggtAhYQ43FS!3nY(DvzL0O=Dz0F78tp(Wk9y8Ecpp
zz$*cSDGUtg=A^LJGMB(xd<-bXKl*$Ps#{$cVryzyYM7AwhB$o5hN=^!2b5cD7_vZN
z4R#)~DTs;&q!OvHWC!U&b}f4K=E4wbSj$?&T!J*>i)uy<QqhR&Do9JWhBbvV8yo{g
zY&Fa&Y!J~Nof_s8W+>aJr?!T10jOL9yBwLQL5ojDh8o5NVAGIQf!G-F0BS#T`P~wL
z3~qr3(ZEBg;I?iNXf*p4WBDzHB9PC(Me$2e{DMnMP3|H&P(ddTA{0P`8i)YR0H76!
zw^&OuQcDtVF&CE<-C`{-$}CCMWCIs<MLHlYy5IsaDKR-az9c`sxTGjEFZ~u{CZzlW
z_4^^sU`P?jSDaXu>YSgK7LxClnUh*2fhYq}W#)kjK_y5b$R~6`Sam`1MLz8-eA*q%
zH}DPTAPt<{Mc0Qi)*~t1!3Qb#%qCV(slF(vd__?C0|N)AHkjyeydfbsqi_!AeEwPd
zE0`}z=v<M|>2RB1*y(vgT7Hi8g2Fj=SEThpB0ZiH5<7kFib~BeoER}B;sXOKuQr&N
zz<5JIZmIHuv=xC%GB+?T$+@Uxd_~FlqJZ%Pz8k_KGZ-hCO)$G5qO>6SqKM`MzX^U1
zM3fe!Tolm)u|IHwjNxJ6<N*x@P0+i@p?HNu@dAh94dlV04(<+aJf+be4nob$5>Uc|
zVnjc24HIgK1#W60l~^c^Rpv#EM4Q5dTn3?-0&3tgq_Bb;yKG>R9n|C{&g5EVoaS;6
z>pJB5;1Z;UC`ywJWiHx<AvUy@1>DC4rE_p7qLfMSk_yx$t!1uZTmWwlgB3%G8fMVU
za4S;^H*!-EwWMKWs9_?Yn+u=rp0FCG6dpvmSj&paNAW$bnC2xmEU`D_Q}|N&QPWi|
zTRKB6dpbic2hkzEt(FBPuHj{B4I^^c*RZ0N;h^R(n%$f=95_Oh6C><7arlS}!$+K`
z?x^LeVaH($ihD6^<RaQeE|l<q*UO->o;0-86SdqZY7w0pwi<2>wQN1xYuHjaQ}|k$
zQutFCT2aPAYZ&4|10CQLT*HtBXG2pm0~Yo04nsOaEl)Z_EiZ~&;At+MA%*(sj4y?=
zmOq8FR)E-aHi0qrVl4{^s%>foYglRov%&r-nvUW(L>gsBNu%sFf`}Z6nmUP&eSV^B
z;-G>}0w^{i(wRUFKStUSLQ#uIXM8n67;5=?7NDpGhaf0nr8A@uU3ya-UK~`98Ir;)
zg{MXc)IF~yDdZ+F_Sn_%;mV`6d|)}QR;Cn;{8%eo!-Jz-!dx-Li$kR_dZ~po9%+!H
zCouM0uHmU+h=;Fbso||*hzCuFff5e`co0IsPm>ukQUK~pf!n|<8E>(slvEa^qR4=y
zJwWYQPY~e+BD@(G7&MumgG~eRQ{V(xHF(4tJXq}mQpkp64_F>Sfb#!KP!j>{08K%p
z;R$x|{JF8wE#`vwR7lGkKAc?y8ifFlN8DmBN-fSvEC9{Y7v&ch-(o8(E=f$zzQvXS
zri;`;jd1p?#N_yrqRd+?xmk(Hw>Ur~h_5LEZm!>Asfy1mzQs}ypIKZK0`d!Jh@vPR
zq?H+@fh8+GzxWnQR(wJ6E#|EF0uYf8A~K6|LCW($D%rrMW`XR8&j*tQMW7{tU}okm
zE{HV9+G0@HKpOw3B)BV3qyd^K5JR;8k(UbW2DSc;SwJJ<cLju|aLf<~!L>|lxGo5Q
z+Rs-6j4$w;bg<x;6cnB!F~brB7i5Cq21yXy;R%8lq}<LXoyoc&7<o}J@`_+&2isj?
z(J2-)yg+bm(weLb!j>0>Ew2b$cChqtJVhOHXXXTtj%ai^-Vl{W3=JD!6gBQ}yTK#S
z@7?7+gKb9P0>K%@3mj*Z&EdPqqke@){Q{3Vep!$LKA{OlJsBO$H-yD!Fio)lS$~&L
zpeJKOEeNi4TH|$r&-@~v`4v9%>wNZ?`0Nib9#Fi<=W>P5rGxnc8-svQ2g?n9!44KQ
zM{|H2&B3YB;RqR3GFiYl$9#eF3Y8UwYt$F|UKBFAB4pIz409m!MIM<eJTez}WNrwF
zPS>8KJtJ`e<3%B*D?&;g&Nqa_CKOIlN2q3*5jcnaB9Foq9)$}$3RrZ2?9`kQyg+$|
z*F_=qD?;iW&RB#xoNq`-&v2ZPIKz1X;{v7yOf%f(aD!&QKd>=K$V{nS5V)XlLGTJe
z5L^&?MO?dsrHAK(1jNt6q8%Jh!EvrTLwAPm+M+d87X+;?3R+zew7M?nbV<<ZfZ>6_
zi-PV~1l>E>Ztx3$!bt!loTNB?7(X(A$PUM+qM(5%%Zs9_S434mF!1o2e*_T|7~$Qw
zyVA0AEEc$|5M1KEB4mT&n(&L#rdOm*uS?rrlD6HEc!2Suw9^%7rwJ}3DYRUWvchFS
z%tdLVE7C?2T<*%s&xu^Ywt;0q-9=fmE3#%2+$OkvWn++$n`5!S3k0_&ZOOVI?RZhz
z@rtzLb!pE_(w-+2PdHwb_PZkON15d}q-7Vd%(TSniivI?*cilQP-ARLTn9*4N?`%x
z0>uT)3mn0ab&hO@7c6nHUF4C!!XtlyNB#o`1FvYmf0zFZFL)lgD=I!EVn!|qZZ+Cs
zbwSkbqNv>!QM>D+ZkI&e4iugcyeR5@Mbx{)t<&v>2xzqn=M4!3P%`P@?cl}i4ud*f
zpaGT7+rWc!$kVvUY(#ey++Aa=VFV4`ffg%(mMkE5u2AP)kY^K6E5uqR<oOxI;tBLY
zvRY<Nh7!=w47932ETUKdsx_dZ2&#q=%^sFKUu7I7Aco6S7>dklS&^3>pbsT7GN8B@
z-U&`&OlL%1J_>56pt>NLDUu<Dp_a9dH5f{lGs2cor!&;Dq0PU5Dnzh%P)%`Rh%K!J
zEzUqR7}*gGWZZFuEQ2z?futKXc0tqBwH!4ZIAfTjh7HHUT9z7SkpEFa3Y5CQ{;A=p
zVF9HcRDXfgqlz#x^pv5P01xXLw1LlBwi>oz22EC^Y4R$CAb)SaP!EMrg<wxV_fQ|l
zAkPrj5Jdzi@&mO<_#n*!#t?9O;}&a4QDSoHE#{p3bns+#Vov%k=FGgZTiiaGd5$^h
z;N^iupztmN&744{)xnb^;MyNFFQUl_X-P2p-eSzY#hM)tGK<wW9%Kq*>Mj1F)Z)?{
z@Y2rY{M58!NW*}~H$Ek`C_bq&zA!$s801lSa{$~DxFrm#`h7$4UEPZkQ$Q0n@!7sr
zvZyTsG--ZD28QA|@bKUR9)TX;87^~t=lEab(e7Zp!Nc2;IDxSvd4l1DzzN1389m+|
zjCXnXdORisc6x(2ptW_vQ`jaHPUM`z*}?jOkCjvRu87za&*{FCd>1G}*I~JKxc<1y
zFW6Igg<l0U3!;l4zzgC`F7lXO;W53yWBL@_atNH^yg+4%=1l*K!a7%kbuRFrqaUEf
zdhE=cx*(d5L0YzhA8jh1oil*(0|SWcaQwi<AfqtHazWr6n-wZ+bl2!#l(yXAvd8y?
z%NfrLVc}<d&iGxliMl8q)!}nPT73oM3dI%7D*{&(t_WViaYfp&!{-K%^mQJkOFT*o
zj8?ENv;xn~%bZ}nXz6>=!0(EI-$iA=i#+~Uc>FJb(OoH-IT{P>FG?9-kuvV!f50zy
zfkUnsG)8=oN8V9~;h>I)lNs|N25l!p=0n<yAhsc^lNmG0Gzb@Lep?T+)C_646mlL#
z&5N~EDQRnPl(i{nLm;5MSj$`jaw#~+GB6+(r!9aF*g>Vh6lfS9wQ~+yreDJd%IA>W
zUdxKZJW$O8HUpWcLEEeX83aR_e!#g%-Gw1Gp%%0Z4blDP0OeSS`MARvS*C^|3#)F_
zSOJ%ioHd*{OGwTdcI+D;YFKJG5n+Q-?j@p>d+>py8g^8bwOr{8wcJF9%9&aY6m#K&
zO(>#>t|Ds<R}E6gp$|%6*M%YqudmTl7;&mV1ClkY==P%XYk5dYSv+WA$Xmk;jXxIj
z0l8Yf8gA_Im%@tT14P}%SHp&yMo?`7<t$VYMuwiVC}tsI8*Qr-Xr~-f4F~FzGB7Ya
z*#Gh`XsRF7k<=6b*Jeev3=9n5`mCr9)E#9`txUbeTAZ9&P+8Of5)e--N=yb#^e5)T
z=O&gEWmd!&<d>%w-D1hfPtQf_VZ$pmNNopN*jbbfsy6w-bzD(1$f6dIk*vj;>A8uw
zxWH6=abZ!(E!Gh5oET?LetJBFDF~*(HC=g838<<A*M3EvAQKo}i#k9we{Nz)d|G}{
zd|FXzaz<*&Ep8|Sr0*6lsG0;%wt=)4gAyyeri3tX)|a06>q}2lP<{ED%=+?%fZ$Z8
zDQ+_iCwfltoXFe3`czbEO3Zap%S)n`J5&#dToiS?BI?$`bwfaEM&Lxg1yUCUv^!Xz
zibziJxh|r5Nknx)>O~QqD<V1_oHu0TE^tVq^-P&LeHlTOt1n}RBWQAsqDoKpI*-aF
z9+d@A8+0zJ+8vO(sOo)@$L9)<&jm1g3Tg;k5LJV8HaZwTaAB*6Svma}KQMsE4#&H4
z3Ue~o3aydc;JU+bN8lcl4Yn6G9j|CQf(9Qr=Nip1Tc9}8a*pNFq9tW35*Jo3sk|s{
zutD{Ll*L5>%PRtwAD9_6tv@m_%G-Pa5gq<drR3-6T$j?mB&EH=|Du%56)Bqz?;A3(
zIy`7aFt`qvG3@X~)ZvCpi<VSu;61>3g!`g__eFJ|E9yQMm3=Pq_+H`hy#Pi}`2~9_
zX7FF+*SNy3ae+evvm*p*dx5g^XB(<@gwh#MJ308f6eURA8ANwz0n#EiNJR!Bk*h4!
zVg;3ly8H}zeE_OD7lzmgwV*Yw@JhJ`X^$#(Tn4K4K<gWDj7GB{uck*VSi@mAypjfm
z1`MNj{ZRb~DuPk$Ky;+g77v0{qUvR2=$VM37g1QFHKEYDBez%zGV{ucz=NQWE(fHL
z2DM8dB{isEE}8=>Pr=;@@Bk;g_`SsfE>W3NQcJ)^I=DF2WJM~3K}9FHFuld-SPV)l
z;KC4677Bp-4xr6GzKIo}{zH`%TE_uNE*w+}f;NFSFo5=Q5bQPFkdo=(zstefk=DuE
z!`s1omxH%AsVB2PuPbjx$edWnf`p44I#)P!E^z3eHRf441HolTAgELm5ShX`Lvaev
z+@d*^^Xq2Stw>pudr{uzioDH50h<m-&?txuthK(DX$|{UrY)S?c{lOyNW5s^aK*sk
zqK4xY4abYhjz^u2c$^44=zYZdqO$)*o`5Sn0T;lCgxntm&iy#Y>Ic>0(`C#I468w9
z7B~Z-?F~n2x}&CL7lznf;B`^h8}ulPC2=k&1(lXyTS2)R)qK=}66D32SeJl;m){}}
zAYoO7vRo6x-=birfC*&#z>7mUL0Jtf#ej9uDT;rwsQ@)bK_x;BV+tX&kU9^`Q1+cL
zGW6^Q&$)2<6=9i30hbSuDH4!ZihM!E0eF~#2U19Y#~l1YqTto|0U#ElxB!hl6oJ=g
zf+j_7aitU`mO~a`Le}jEgJujs=>)#ok`tr^yxOu#7*Qm^1wk#@VrOuR2s98)<yDp!
zIb5%BxL)9JMJp7TIg7weogz@7z(dTEO6eIcbKDn%t#Dcr1zFDtSy;J&{fer^4z&Y9
zd$d6N%?<>g5Ihii#R}2Zxyu9EB~ZSAX+heGpe0!w1UDFN5Z(~DLG+56IW~Q2-WL^o
zPn2B<i#SttAu9SpOzedy(CWzxQSn#&6D}mCUP&yt$WwTQr|<$8ePBoU1+%3A%6FjH
z_$-E}tU>M?Ag5`RH9zof)*43CPB?0gtOX5Fg6alvHb`ND?(J8{S<0cWTEb}$s2r(b
z0(XZQaV~U68<0hHn+rp1F>1>rg$3$9E98Cb$n6pAYXwn;Q5lidquPjErlE!!BSVin
zX?E72*n`*Pk{Sl=T}rGg7g_y^l0capG-IpDmabJ2mppw%-~ROFFFf@AEB>^94rS~w
zTFSt{ke=5j{o$t~=Yb;dG%RTRMw1s&a@^tr?NkQuNe1V6$o}Uvkoll;9MbOtFY=ZF
ziGfOzTS7(opvBnnd8N6jMVZNoIXRWLxWK!h!F@UK45cOuxFK?j4csp)zQt5%2yTiL
zgVGDA9Rg{Fh?f?mB$lLt?JsuBOM&fqu2Mu4JvfvUgBm688NhR$pxLXrdMiZM$m|W;
z6Lmq#<D!(u6)BI4f}U4EYrH?OG4KfV*w29Nrv)j$Ams}|S3|?EgvMVGOt>hRa78eo
z!wEDunKUuF12j`4BtErx2FgN57EVvb4-6o(!|?{ca8DIvOY#lahM(&KhL;2kw`%PO
zJK(q{@}iOZ6(jc(B4?y73WQt{2)O`84^S0vP}?E6MRP~Uo~Vlg9#;fBE`SlPjjNDC
z12jnv&aw;)4DdojjS+p>VhN})fvUsb1O{g|CX|Cy7?Cm?+#KY69N3pVBQF}RVMJDs
zk=fwBz?#`euoKlDWM5#ITmq_WS2FoQ))cT|OW{S!L1}#j0|Nteb3)NdkN{}9z8K^v
zL|s{&S^`PC$aN)LumzMvyP!!Fv{hhWQql)bP;!MNT1e^vc><a$5PPfMFoRMLa`!O>
zmI4B67?D#FV!>+(JO&w%b_Y=|n!=RAoX&<~4yfP&yLk=Txlzz62C3VJT-^{#^%EFl
z&B2unC>MdvPho|o`$fowHe8g%Y5=9`LEaCA)QLtc14OMfKt4mwGpIa9hMokJc{?OC
zu^)#+iEC<@KpRmp-Gnl9fO4W2BSX&ytYJuE^^MDq#D^OrL(f-SreF^#)R@4f7s&<i
z(7_iT6Bv6ev4)6P4FmSWg|IaNY8bLWl|Ll4vO!ZT$ONQK^k7Lafz1R^8K}wbR|GN!
z+%QPLpQ{l1Tk)qoq-_CeCKQ1NK{SQHhoK-f7?=t&i@@yzGf<g`Xc~xtxS&jQi@P|r
zBp|;y6EvD!1S&-#9gt{{0&vL+I^L-Wv}+H!sQ@lxZ}Fy9loTb#rzGYUWagz~Z8Ct4
zAxSJs@y^W44a-b~SyYVNWWb@M7gQdf2lrh*aG*2^Ziq_HO`4OjmT3*^R;DeidztpI
zf@U^dXE-m=`oPSnp?BBRd`r{^W=45OM$nj~BO_=~63iBJWbAPJ%E5ryRN&z(0q-y_
zVeD{xfYekF7N24<qi~Y#1Y7v#;_D)Mmqhf|R_##PqkSN8kKPq4zYBrk7a}6B1V&x7
zin^#DeMLX|qDagYk(di$^Z>SF`MQYdB@xrDSqDUpNS_EiB6r0x^g;w^U)Yt1#0x2f
z7b6NUIu>3uE4pG<bfKi;N=e;Ck@_nl^%ubC0~>>g1P;fnC|Ogz!*EUA6@BLeX(s}Y
zWM0(wzbF!LMI_(?7@@gfL)wnOEtv;Yj%c4qJfe5SDeOXI+C`_di)LvTGIFkD6kQZ4
zz9Le5L8SPiNHOS)6^jXlUA7&zH+TfD^JrY+(YVN?b%jUk0t|t|t8#|hC4Qv~{7N?j
zM6L@cUJ_8eD4={rK=}d;ePCt~P`<#g1gX@Jsv<;_$ONMwg<O#lsvKf})nacF^&<DX
z;MJrF@<|`a4Ik_ywW!q;vU=2F9+k()(Bp$##31X#zTT8rd+;@zY8bGO&7v1BJ+rWe
zjA#u5_MRDr{#~T`jrfKi(LVSMDjbW}f~K^%(OZY`Lat~Ns5C<qZs2kav^28_T)3^n
z7*>%%DwF&m9j9Xdyqrpx)TGk%Dm6rR2cNn%paP5qT!4LL!&2f1fYvGsfYvI4@(-+Q
zg0H+$J0Q46^P;}X6@8bBBCc0NTrYsp2Q~&#Nyw21@B&8dgy0d)i)KDo%zRFyoXNT<
z5_v@=@`6a@4G{^*bk#)>^(!Lk7r+RmWO)EmVQ@*r;G&4p6%nHgV1%pu(7Gt%dqu?e
zf`~76gB4+(#FOi{VRVF2&_)^<8Bodrq%$Z{a~r%{1aC6K$4Eep7<i);B^ScSQ^55v
zlprA&!tKK7ynq^?B-n|OJ8+v^f;{4bWi|@FyMTd#RM{E^><dKD^FAowX|niLso4?*
zz#X`vNKiiIfGocPw@ARVB=ErsL{|>n)+oZvn6mgXoKclJaXHiI5GZS&fb{h^crWmo
zUF0yo!eM@a!yMc;Kt6ed6+DChBFP^@_y9WRYzp{@5y*%G_CbWNYz&-y9Z8*>J)9k!
zpcxrVCK=-lb>QrU+*7P&Le5So1C(eP4<maS)-u;HArHx6-^7cm1~CbW+D^ypoiH-=
z7$J{HAXY8mJ}(4i?Fgtz3(mr8SW&ZMEelRRqfPu__*tlywFb1E8)<_yVk0(a+N74H
zN4SOs+mr)zT_Xqf$pY}eAgCc!1ez?+<b<@m!TAEzQoqFp8M-S1x5eRyJwV#?pa}v^
zcJQzrq%F=4InvMw+zLnA`A`fB7SK2zd@MvfH5JwtcT6iuEpp2*DhIXEk=x}sl-vg8
zonzp<b3;gMigpM4UGT{YD8nU;oHidA7&&b~XDSF!aP0BA&Lew?M|MW~g2Xvlpz~$K
zb}(*{yeMaNk;nQ9kM#vGf~;#NdN0FMu(jqBYo^p(6jZq)r~<Ol08BvkEI=*OzbLBT
z;nv}HgHL2a;Do{n!7~IWg!bfiFyG)8>fnM@Oz@%>d4#SAoO6(iTF`t7IC+7OcuQeM
zoxrMPu3-jOK`h953VFI2eN8k*TC)Qm6NjiYkkTCXW+_S|7D+WC=hQGEZ|$#P0ZoBn
z&3X2?ivAjmV-y$}dO*YA@I?lYFoBTh6$Kts(efxGWIGcZbd40KTl*5UD(V(nQf6^s
zX=;%%sQTapcW-ZT<YeX*7bGS__Y;9K3?%>T0=4zPS!OYa1#0HOw(^7L+(4s)w^)la
z@<Hc+f+%o~K{}HYGT1GcR8$H%!vvl;q!G0XhU5!St^irxzyMk@!#%@sM&be{NEfJs
z^@fNfXzh<CnCNi1D=IbDct+Vw>p9jJMb)o}s()Z$<n?ESkf06q(DSIS3#eWaP+gF+
zBzuG6lDsQwR*-S(6CxLFeJ`r{UKH@VBH(ucj6j<X{JQ*R7|p1-$fI$EN8<t*Vb&Kn
zIC%Q`y7(p(%`m>mA%BHK{sIhvbAl$LpQbSAyim{)xbg9~xZ>k;^HWN5Qsd)q@x;d$
zmL}#vWQq(xq3a4FLP10VhyVq45oipxXcmaO07QUe0o=}B58`eC5!*n-4iK>iL>vYU
z-ZFsg2M2x;s7(eYEI>_jNpKswfdK+PuvoFmd|*H&Y8WM0Ek7{82@5t>(6KH^q=+D^
z$Oi^EVdcxns`!BciOgV9VwL~E04IDH<ykd9Fu(~HMqUsfPN=c5f~Gl<NFhFu!{CGx
zD=TRB0f`inW>xvX04MB(Sv5W|zzH1zkajp>#KkK7fdNiPv9l_CV8A3Gp#}=DWDo(#
zgGkd1E#MRG(-~@6kqa{9%CLqJF+_}7AffVH7-H3G*=ksER{2QhXQwdMKn`MJ1`k7m
zj?+RpRe+J9N4<s-`;-Lw(qYu~5Qu^e%i&>=vlUoyGzr*Hn*`~Mwd^VE>5MR(&Roj@
zV}s~)=2}h=4a4cowOlYZh)!ol-Dyr<m`-4fy;RFq!;T|V*;9}f@FK@l4N_>MHb+on
zjgg@S#f%j86wX#A2#LH}qn4+J2O^4gm=`wPoX9#sl}RlxTsO)I4Os2Q)XfLijdErx
zk-GU)IBM8axRKU}K>Ube4-sa;4i6(}7TCuqJD*d~PE&AUh<#csSi^z5BOON!^CGLk
zh&xVXSsW(tA*;bKfeTp{hY9@1YA{UTMwZ24f&j7_3=<d;r^F(sTjX^(*hiddxKog}
zH`Q<>9}$O|n?WZE5n&6`#1pKjWMHV_M!x+6-Ss`sCNTC~sbNUrf)3tG)G%US0Z_w|
z!h`NAP(K-jHF^EOhqqx}HiYO2fEE~ny91C8GidD)_@E<AUZl}2(8NAu%QQDqe;C{@
z0xv8?bP*sYxp5Symc*wdmL%R11hI@$;tPuM3sQ?pGE<9-K=rex&@I+9&>8h$yC5A3
z?mW<G`e4yw$a!+SkmUjK#W|VDsVU&IkH8&`Tb$r?L_s#(;sl>13SolJ69qF_3&0e2
zKGG3&cA%Qy9z-~R2+*;kkd_$eNGE7#1&IPT3~uou{0ll>uILH_1H*Sv)9VlT{PG(j
zVhz4u*%<hQry5TvoKi8-c8cvqUd1cCiXRx5SzSP@l2~0BI~YH(F>s1q=a9L?Au}Uv
zf#aOW6^biTS88s^zNqDRQP$}qhw~K<=L=wTgN3!-x5;;c?nM@bD=Z2ZSQNk;m1l5*
z-~vk!T#*TaJ0wByg1GZpqcc_)_`)vog<atbyTBXX;C4e$Xae(uzzNPX1ZOaJ)HZk$
zA;8Dq5j;V0g7AdI3Bet{4X$^2g)sdj%xd<LL73I7gYgCjUq5db?}W4&K~u687%s6`
z;kd;5in8fNG1H42W>+}OE^wIL5R(DR3NA2QAPm`Myg>Ylm}Up}4FULu<QcA41eCBW
zr@AO$-oc8Y4t2>E>lIbAivngOsN?0IU^YW(iWO+z^^D{NiZfEL2&*h8TamP+YJ=hi
z#|_FG5;v$`QMb4#Y;lnn)ckeY6Lo^|gyIS26OJcXuh{xs<n`-d1g$?SoG39x0(4NU
z8JGZdJVhrY_V`@qk-x+vzkqc?;u4OFJepT{G%xUI-ryluPH=)^j|X@wuj7owIW8A@
z6tD0oUf@yuz{kML*W(WA>P{%^@r5pPHC-XN#9~G02E#QG7kSLC@R(iTF}uOTi>yxj
z5|8!@*A0qmJm6dHF7TLsl;q(w`yj)>!8?Jmsj8vs1_x(9cNg~rry0x_Ib^PI$XtM-
z56lc4LJd{mW}qh5E!Mo!+=5DQQE*EMa%39#J^;P))TI29;`qdZ%=nDdoPyM%__Wfz
z;#)l6T_t)snR$sh=|z2@{TnjK+Q6d<@rlLp8Tq-OttPjGk!3*(4M8WNp=f~$g13Mq
zf=-hJwM)S{wdgn~r@Mg&&~d@PIBXz&3%jD*3=9mQLCfNW3=9k(m>C%vZ!id5fT0gu
z42*^s7z`lj27~Yg82Z4*z^HwJK?{OzFeqMtp&Ky43k+HxSY#PlK7=rqvoR`sV8Bj(
X1dD$GlTbBvyef>c9~dwRa9{%fjO3uy

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..352da3ed9b57880cf0b7df073839000cd4bea147
GIT binary patch
literal 6565
zcmd1j<>g{vU|@)xtCs3x$-wX!#6iYP3=9ko3=9m#&lngOQW#Pga~MKEG*b>^6jKUA
z3R4bqE=v?EBSefXmz{}$ks+5OiW4ltoWqsN9mNf1Gv)B)@<#DN+5A!bVBG>yf+-9s
zEIC5C!coFtIo2GZT#+adFq<t$G*>K2jFBOQJ%yu%Axb=jGli>#Axa{JJB6o(AxbiZ
zH-)c-AxbKRKSiL0Axb($Fh!_^Axb7iI7Os|AxbtyG)1h1AxbVqJVl~~Axb_)GDWI|
zAxa@dIz^_1AxbetHbt(5AxbGlK1HF0Axb%gDVRZ1@g*n(G#PI(YWZn0-r{vD%}aL7
z%gjwI$;{8wWW2>uoS##c8edodl1xd>D^86Eixd@WGTve>E-Wg^O=bic0mYy&WMN=n
za0Z2=9RmYH2}22E4MQ_yGouSbtXM5$4MPp1Btr^gHcOFE3YcfZ02O1(W+;+LVFK}N
z7z){H7=jrznf;zaOy+QaF%D=l-C`|D%u7$b#hjCxr^$4SBPBH_z9b{H<Q7|IUP@|q
z@k)jwUIqq+U&;Cz`MIh3>G{P4`6-D-V4e|zn^T&XqF+#ySdm$*Us@DjoS9pYlNz6#
zpI1_ppQE3fn3q{lnv+<PUld=KS*2f;pOjw`UtCg}mZqPXR~Db0nU||qP<e|tIVUl<
zAS^W}KRL6cQV8U6J_ZH`0Y)B14n`g(4o0R17RDle1_p*?Q0PFs!@$7Kz`(!?3#kGI
z28J5O8ip(e35Hsx62=8gHB2=OS<E1u#Uj8^!z944kg1j#EQ2NvQUk(B>R4)+YgnWi
zYFTSoAu=Lh6KdH?SZmmt8Ee^U*n$~WGWuz<-C{0?Prb#IS8$6tCBNhrdsbp{d`VH}
zEyfVAi#6GAF&5ooEV;#)dW$))H1`&BN^0IM*2J8GjKm^t1_nqtg9Ez=<mp?ynR#X2
znR&VKUWv)^A@Z=m;9`_w)L`Ub6kz0FW??Oo!4?>x00AePDQJO_#k2qv5G)HBYnf}9
zK|xW=lFm@eS_1MM8#rN!*0R;GE?@^Gs4O;Vh8l(}jv6*ehAhq!#w@NBCJ_c{1`&pZ
zjEoG0;w6k(+%>Evj9EM>%)LyttTikpj0<>c7_!(FGS-0Y5nzyDs9_agFlVS`DdDSO
zX=bctsbL9b&}8v@`Tzg_|LKq@{9(V6@fK@JNo7H*CV!D2C@?^ge~T?AGcPeG{T53>
zW?os52uPfhfq~%`OI}KT+AX%kf`Zh%lv`|B`I&ho#kW{N={E5eQ$gk}*2JQs#L8Rj
zX*v0cCB{a#m{TiLp|MaT1~Nt*L`Z<l;w&gmP05c3SpbTKTLNHH;?wer;&U={GfRqZ
zF=j(z1(XjUQ6mV8nt<Ze(v*D9yfQ;j^ne`5#mL9R!N|hN$Ed+5z$nMW!NkH;q>CO&
zpw!9+OR=DW5|LtS7_u0%m`a$lSQfCBuz`3WoW&%-(9F1yQGfvvaV6}C04)+=U|`VX
zfP@{$9YvxbAA&-$NCD(cW{?L#RziY-6)hNwv_Q&10f6pTF$}-PXC&sNd4W6(ssy+g
zl~_2KI5=3CiuAF06JCUQfQzs!P=;j4VqVBt3(A-f4>2rYS;$b!Qo~%rumGGA7cxRJ
zEjazKfYT3i340B5GdSG_Gr+tH_g|4B$o(LX-x5wv%t`hQ$#-=xN=(U2%`1t|_ASx`
zNh^T}Q0buw^*$&8Abfs{BfTgw#kVBi6+H~(pkWZ4nV0TZmY<oDn3tRy50CsrP+0IV
zFfi~jb1-sn$}xi@-3UE2K%o!9;GCI-k~32nK?PD4Q#wNm(;~)nh7@Kn$+Czsow<ZD
zi#dh0m#LPeh9Qe33zXm37O;cZAe_Z2z)-`S!Y0g6!y>{U&Hzdx;C#(m!coK8%m^yD
zf*CZ~{nCxW;Ih3YGqm{sU=L+5Fcg`9k`FVODl!HI6S4q^4H1=NU|@I&Dta|JAxRA!
z0qP(P8Xy7`0g#jjN_0hfATe-&vlXFbJJy`U+?2#yESV{(d02}-QD~UQrzGYUq^1NH
zmL?XZ7FWT7nv0QzMTt=ll)r?FEYO1*6d9nz3=ZlVXu_*u$YM%iOkwI}s%0u+$YM@m
zP6yQ!Od<?WQRWm-A_R%CNPr4OQ0>49m16_xsA0&0@!3-tYB^FEYB^IFYPm|-YM4Ov
zL=9IBdktF+M-68ROA2c*XDxRPcQAt{o8K+Al>E}9oYW#v02YC2`CBZh$@#@A8E>&c
z8UdQ@Mc}YRBvf#yg3Cg1sDkp*N*+Wo-eL^6#aMWY(Y*+iif=I{-C}gS#gvnDi>Wl}
z7Gnn1Fy(}XX<)!CP&k63M1WC@k%b9VY6&p1FoB>HGYfN(8+wR>90IRa6c`!6Vb;P>
z%ap>L!qNgNl|bPJs&l~=N;*R=TMBCmTMI)idkJF+xCVx{TzG0ZYFHPrq_Bf3ks7uI
zEGZlyR-sG@YZfSBv(~UJWUA#X;aC8|Ts15;EDM<yGS+gXGt_c}T0mLc;EIa7mZye$
z0Vq>2*YHR(WbuGaaVX));;rE>;mG1k;p}Cq<*s3`VW?qgW~v2MO#CTa*-S-QAe*uT
zYM5)dQ@Cq*YdBMQdRc4v^0-jNQ+Rt>YWdO`YWcxF;L8$(`#_+Ee*sI0Pz_&}V2ywz
zxMXHzD6B0JuHkQHtmUua4`$Hh^GjxiRBvDsl=DG2n1O-eGbppvFfL%IVF1^}Of}3B
zpc;~K0jR;k4EHi?4J)*)Tgh@uJT)~Szc>@rnsiJnNiA~AFDeJM55Vc>77M5WStQKB
zz_60-78|$@D@G(1NM;2m6xP(DqWmKC5{@H1wZzpmAisD!$ax$L3=CY19E?JYER0o(
z&_n|&PF+%qll5$J@{<#DitY4ZdQh6Jpu`PIs~~?CgYsbs!ve+{kWZPQe$iwCI~nX)
zO~xW0P$2=XTfqdl?ZaM@?_W@onY$O1OhM%h4^x!_PIo{RqqGS@nn1NI$QhqOIje*L
zR`t{{)-Wz)hB{^?%PrR8<jjJ~TO9cy8#Aj?ZwdHh=A|YUIp^mUmlP#t=9Pe(9xIuO
zKp6&{-Oz)ND;ci#2q*wSwhJ*=DdTi9LVXb^u!}%h3tTw71cl!%)?lCDzyM9=TWq<h
zB^miCx401+if^$c<(KBA6l*dSMT5dF22^&7K%yAbJSqZJ%S9?6DP<4=3WA~#P$PiN
z39Jzk{Gb{f90x_<dQlam6kO(j66Y;ea8vviM{Z_bW-cgD7%OhEf}2y|2E<B1NML~5
zQGOt;0U!dD!iqqJZjlR!8xFFRDJj1w7R2)f@pvFfGrqVav8d!0Q%S}xzMRZFP)`9G
z*0-38Qj5W**DdCxoJ91H=0^+Ct)LWQz`(%3!NkGF#mvIU!pOlS#Ky(Q!^p=d#KgkL
z$0))mz{thK#VElj!o<QV#3;td!NtN@1ge2ik_@Pb14RbN{hvuK;y{%PxLAS|Z_M!G
zjTu_JNtLi<u`Xb%ffaA;3ph&H7jVM!I-+=E0-FOV-axHrE^zV24KCj7O4ze_YPd_-
zvv{!-Z+zh5EfZu@7C)$Xt6_i_aBQ`FdEDqSDZHS94_d?tfQmDeB958-BCg0C6y=~+
zX%VR5SL6-if-^Xn0Oj%`P`ddB68C3dV5kE11VQB)N+T;gvm_%hAU@j{OQ{3OWZ<He
zfq?;3(!#SlsO<qNT^GVSfZ);x+~H%YVOYo{&H&B)n#@I@qyxzh;Ifk$QXGLY`YldS
zb1=RnKOVh3SOiMz|3LX1RM2rSOE6XGL6apR#}g{5iUL950S>kx5GxEsfI|#SfP)Q`
ztJoPC7>Xi5!DdLL&4>a7t;8vc1lbV<sySG0u_P9y7bA)laK>H9TvP~B0xxV<A{RE`
z!UMIODT)Ir2Nk+Sh(ZQj9-$U8Me!hY2_OPoWP#ezMM)qosLfcE3}S)34JJT-za@*;
zw_iYU0*X;khlGm_To^F&VJ!@#7{!=a*uaGWHw$A?3AXkQtS~6b29>aQ>T_ZfyBR3f
z2q$(=5)yk+4k(mB2}Y9{Ti3@Ek+_R;K}x}42_`@R3Qo#)AZO)+5)<BJjN~k8C1ZDx
z&4iONlFdXU<08-yP*D-cv&A5S<aFd|0&-R<NN+iMx&XCkU_Fb^6*zhpHQ){{wC8~6
zRj^9n>Q%5o)v}}XD%df470f9tuwKPVR=*-kkQ2dO3M&xH8k9^qAQ7$!5&-8-aGhQT
z;({tTa8srTRBjd7g2ccn0Zf3B!!1~EKQI7ZlDWobdzLUVFcgCdMFCKq%FM;c$B5Kj
zU@8K2%TVf4<X|cS1yfNaZpVRgETs7Y@9B`>INu%=$HDqCpr!zm6f+A`Q8m6eES^L4
zI0W?+G5VGqU{Myd9w(&th#AkUDQvKK)?`PD=a+x~|Ns9IRM`~~i1Awj;9(HZz^iX!
z1*q*#ObAOsi&v!R_k|j<1SymmSr{QnKn&D-WRwDpan*z3SLK!{(%7zET7FSEcyRX?
zYhGz?K_xh8-V)4BEGfvzFUiSF(krM0v2L+JY372Ql3UWb1v&90`T057nI*+~xgZ6`
zDSC+&sl~=Ax7eZlTWnAPrUK(zB1k4C!iL<7K*5A!S#o|(eo^r)HYm-KoS##4i@CV6
z_!b|y3!n$<0Th9n%(plT3m|>-Vn}`gwJ&b*KvY5oCW}BFI&ga!JPr#U=L3&|6@dnS
zz{3*Yo;0|v39e&`Km`goyMU8r5h&D<nz)FJ$YGP4pHiBW3M!i!ia|MqgMovIhf#=;
m2Rs_i0j7BvIoLROIfOVQIm9_6ITSdgIqW$cI2<@6cq9N+mUG+y

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..523e8c16464f8c63490ac3fca3f2ef086e483ae1
GIT binary patch
literal 14346
zcmZ3^%ge>Uz`zhWS1r}Yl7Zndhy%k+P{!wT3=9m@8B!Qh7;_jxKr~YhV-!;gLkd$4
zb1q91D<edVEtfr+iIE|fBZ?C&!kojE%N@lHW;5mR<nl)GLD~FK{9xSzQGzK9DJ(fc
zxx!JxU^&(tp<IzD5ipxAM>JO~N{o>qg}sF#N<4+5g&|5Jg|merN-~A3g&|5Rg}a3z
zN;-w7g&|5Ng|~$vN;ZYBg&|5Vg};R%N<KxPg&|5IMX-e-N-;&Kg&|5QMYx3_N;!oo
zm_bwICCGD{jJFuI{4^PF@w%1fB|GM2<|dY8=I3cL-r^|E&nZidFDw8_rljT-r^bUt
zii$NEZ?P5^7M0{CGlGnOVpawQ24)5ZhR+?~uqr{4D*=f^mDMmTV`N}h4VOjbxiG}4
z)H2pE)G$gy)iW@pFlMuWI7O@}U>O?*1_m@0OxYj>3`MLdOduJMiXPP(hF}IwX20iP
zDFzM)7~_B@(=FDb#Ju#>Tg*ABd74bOI8st`;!848OK!1c=B1=&7Z-yfUP0kkvVKN>
zZmNEIesMv5N@5Y1XN2J9l;)-A7ZfE{WESg}7R485<`(3n#wX|Jl@#UY=;tQpWfqj?
zB$nhC#g}DP=@;cE<(I@4mz1WZ>1XDZ#b;;c<?0nw-r`NpNz5$>OU=no&Mc`c5@29p
z5Mp3pD2`-cU}#{tD<nRbd4|VK-Z{J%h19MHseNEzWmN$a9c(wag(tXO;g+6}e2H7}
z0=MD~d8H1I31Js`WFEj|QZ8{TUEo%_A+HRT`Ei3=xWWH3C{&U`-UKl~7#@EXjG(k$
z!&t+R1ri72DlP_wS|;Rp%!2E!VX9%sg2ym2I}03dAOosc7#M1pK;$we28Pvey|v6Z
z%p}1?WE+v$MA*(!!(78s#m&G_%UZ*VLoX}L<+W@j+#tU|a19%3%B^LqVGCwh$>^ua
zc8j?nKJ^w;UcoKql>Cxg>{*G)@g+r>w-`gfk*mpmi?QeyW63SX)LYDXrMb75Q&RJ8
zu_opeWF!{xf?}SXfq_8*0*d$<7#MExX6BW7XXfR`dnG2vhZM;(Fff2{@kDSExxvBN
z;n?Ha$=Tp|my4%IZGzwg$0?o*7?*G^;l3!My(00Vi2g+`gDYGH4bD&nQzT~u&WW58
zeNj+jh2cd(ofU~|GIuEM(cGhbQP1@R<3&Bsi(Fn;xV##iZ}9L>Q10|>aJ|7VGNEuv
z<&^4+{7MS~FY>E3czk7J5Rjb_agkr`3W)oGi-AX^!M{k5fq?-fX@UF$N+6#v;7(fb
zB*DOdNLmX(x**|=MAk4Mq82R@m}{6J$*h(oouQT$CBhi8*kFdB>T+R-b*g2nVO;=n
zCrk;P1{FF8ouI^0!;r-RldfSy6jWK9FwqiZ-+-$ZkW30w6)OV+$OI6L>IO!J9%q!S
z08eT)tSF}OfJ_46HOy#PiM56W#RO1N0x4vu!RhlFX5_F0WlvB>uVDqVco`UKSxRKV
zMu3SL7F0Laved8yGib8-z5M_G|NnHb8ipVCD;aOGrj%3`q-ydPi7+rQ6p4b8Fk4P$
zUSdxAEtZ1Jys{#3kT}STw^;I0^3!gyB^DH<=B3<X%gWEpD=EIk3M!2gZ!r~Q-eOHG
zDoU)p#h#XvpIBmSbc;E)G8LMSia=psBn2`{8e|7&L2+tIemuwmkmqj+fK7={%P)%0
z$;{0xDZa&+T?~q3NZwL_WG+Ei<_aiIEltVy%quH0WME(b;o?i^8SJTm&=k(;e3STQ
z6wax<D4=;oK(oQ~2EW7%!_L|Uj}Jl&5>gjfMDFqmO)#8d)*0Iq`+<RxHG~mDb}-)H
zkhsnve~CkWfzpD&C1DFSH!yDD*de%u`-*|%(V!!tCxQ<~Uo;53s2FsSBlrqO@C7is
zD=s;u;(~&~UZy=<7sQ<}iaTEscfP<P@_>bBs?Zd%8H{t-Cdy5bTc8Qb#ut^$FAADp
zu=c(v;C+$ByTRirzYHReJ^&T|cyj0saOI7y@ULOWf>+vz99ja;Qw&JCasj9^fusi{
zvIG<<P(HG0$ZUkkpfre@-cWfU`C6p>UIHp9VWwkEsYSvJ3=EnakVFYiffAsk14^Dn
zDxmbk3`#km^Z`j<tZ3=0NDrhO<cVUC|G<d|l7z%Cl2Cj`Voq9-2?GOz7pO#!Mo&i{
zI2Z(lrf^Q-o|`l$bAIlu+_kP76t`&ZNZgaTC;Ot2=S3Z_D>_~m<-D)Rfg-ZO^8qZ4
zeHB0{ba6x|sOAL~o}WK~BeVn>FdzyMjSG;9DUb{VBiDndL0-#L!&Jiv4S0qHAUhyh
zkjP~W3=FH`{92Y8X5^M&7AWArvUmz5l=cM3HgKWD0%m~<C1#YUW=3-(a}9Ga11zfH
zaa*JciaSuG-x5wv%t`hQ$#-=xN=(U2%`1t|_ASx{NvnYfQ2R?08ug%31rh7FIMRy}
zQ+!MEUD4xT4jTW#nR)4sW%-#YiFwJX@rd%of`Ne{5tI({!0F&C8-t)Q{$fNx@H)TB
zC4Lon-O9$m%8M(r@36Qi;CzwAnYg?T%CX?2p@g$GPGQ8=FiU4hVOqqP&XB?kCRr9S
zrZc0|Pau=Q5xj;Kt$t!bu4@np0Yx{+ZD8Hl>tbXRkl6^+K#8P=Ifbo?gMp!j1ys*g
zF)*NYIlyYbjnP`x5>R6WY$F3h4J%qwVy$5fX3%8!OE(6C%l4Yg&`!h$dnki}p~xCk
zsxX78A}de|LKXnAA)-nQ3=A(p8BUWEQUZa~rZz}}4u}Ay%_4At1S*S)j6q`Hw8(~5
z4BcYQNz6@2yv354l9~r;FoJ>&TpU5GG*M`Jj893-El5oXE-XzfN-Zw3XJB9e1xIlY
zI2j_f8$WO`@Cr1z-sR@)v6v7Af(xWTa77ddUJ$W3%5;S50=M@?ZtpAH-VH7f_yu|@
zd#dLK&54{JJ1cgD;Tp>gg<C4ORA1C~IuLro@nFP-fRKan7qvq!%7$K%4ZX-8dLbh6
zB7Y=W68)ftqsa(r8-SA1X9I8|MJYoVq45Q3*nk2Q%uQij!-SULm{4?qOoOOoP6zdE
z;5tEx1dDFw6oy(Ba773&=RoC9Eh~0YYgn<E!G=RE10um<(Z!y^P|K0RP|KOZP|Jl<
zX)=NOD>Ym->@{pP95tLNENfVoaWXKhhBrKGxofzC88q4aZn350mnP+;7J-so5y-E%
zSW=Voi&rw<VuN(!HQB)pGo+FaoEE_?HgH-571b+w5Q*;=W56xO!dr~)MWABw7Gu&a
zM#o!BIZ3yeN|SCeW<XLRsCf!Wi4YP}KXO9TV_-m$9|Hr!EKpGhswf&5?s9PTuyqvn
zl+B2^z@c`LL+uKOT7x4<lw*S81mOvVQ_3dFEQq+kp?8r(?+S-rgX04Zo(4yxr2CYE
ztA}HP;0%om9Lg6tl&^3o!}JSIkzAm0fkXQuhxQc??FPpOoV*QA5CUWdsP`f<gK>u8
zjKl?k3mg{|u25Q$v_tFyzr#g-hb#OJ4IUrZaHLL98v&G_KF?tW^_5c?TNrAYQkYs8
zYMH?)2As}71USWjyCUffwQMQOEey5nB_N$3Nd^Xn5>OyR*|_Un)MCknAy%iBqlOiG
z{hNZ;^kHPEVZ)&kt+-=k=+P~KcUBmXIt@4qW7Zlrv>eBYT%2aXQzeQh+=d#K8WvRj
zsQ#?wN@u9$Mh>kShAdFI01huywJr>?>a{#I+}M4J9I`b$piTqW2!<?BY6UY<O=D!}
z(LixCsLcmfSHoQbDwDx%28Jv?D1%`QdPs0HG1RcvFrfRembZo>3*JyoVb2ETOok#=
z9N~^+7tAIGh8pG??i7w1-Wtv|oXc1l7*@m6P%U4cGC3x#;abMRz_1!#&eigzGt}}U
zhdfF;<;#LsnyBH1%5!0e)vpz(;l~~)C8|h1LNc*N01-c^=ArT!8F~y#)RD~KN0qPT
zui+17(B$?@M#^pspo$XI3xEz%)-WzWn1IAWAEc~hu3@TShGb^sU_ta&P(<N25=`RY
z76!Pnx{~FVcxq}uesLyf{M9k7B(=ybzo;BE>H{vPZ?S-eT#LjQ7#LQv-C_gxY>N?v
zGo(oYE|^(Ui;D7#ia}mQE`vGJQ%hW31M-Wj6rrUtsQv4bTAZwBlarsEm{V-02UD^g
zG>!`zMQvcX!N-4{PyG^~`U<v-eEL`T^gEbu@bFGx?sV^9yel9yg?%p59JU#S3j}9!
z&*8o(pmIe(rGxbbzeooUq?ZY=tQi;>KnV;S`k)5SbOw~l5w5O=p@spqSb#O>nZN-7
z4gyWaqEJvJ0@4L-^RSoX`xlgC=2j`-^d3~jUXV^uJ=ehSlwYu?Y(m<Mw2S<zSNK&g
zaHv9D4bGO(1OdukU{`~xuIUWqcBN_<YZ#X?Gcc@%rw*vUR<hh;El$oXsJz9I5Ar}}
zRq8DPpUk|}#3JYXyyB9g#LT=B@QBGuCP;*Xq8U0w4XIzblHm%glyQ0=q38%Geigut
zKu|3x+EaI(U+)sX-ipFCl{*x71YYE~zrt^Sfx{jg8bzQYy9iX8gIg0XL4kgYHP|OO
zFhG;}7F%v=Nk)FkEpCMI#kbg!@=NnliZz*vQbFz5G*A;+1d>KUC3q32;4RVsNrAF(
zQ7niR2kL^eIe|4oVmcTk3`+4u;GUT#hzo9{fYQ$`R`9s_EsosGyv$rspfOh5Vg-*9
zfxGW31tEb29v%t@X^jFAps*|Q2C;lV1gKYbizz9;2sC_E6b5nv4<v8J7ndX!mE2+~
z$+*RrlbHt^)`!N$EoM+-8dN9TVou6QEC!W^ker4@LL!DAEi#Hgsb?!F{heS0jqZP7
zVPO^hzyK$Bm{`?5Fu(~m22L*6(C!UR?gl3?@qvYz)q(K?1A_d@!@w^x)p-Wv9FB>;
zQ+zM-DP7@H0+}NYCOVjIaB%f=c5zNn>E!R>@8E}Vl{@)*_&WGH`0nxw^~7E0Rl3Bh
zw7_UV(M4XZE4*3_ZclmmdfcZvO>vuHI3sYP_Z07oJPKEM6dGJ_2#YrO+~pIV;5fyt
zGq)!fG`?j3CO~$HT<4I!#34PyagN7=pgBHQWb`g_=wIQ`zW_#eg+-@W%<$RDw1w+}
zu=Pb@>np<64L%P<<tIcfU|e8$QB?JcsA_}X4Gzv8_6devJRLlD`GkA&uJfs0;!|Ca
zzM|%U`-RB#3k78t_*5_Qm0#g2?_lm=eu_;ENI}LGzKjm$4_piaLJb~w`9-D%&k&v?
zIx%WW)J1-UEBp!|S8Icb4(1yi0{#45{Bv1nB+p4*;5;YmqJ-uZ3C)WfT30x<E`ZTj
z5tQK<@MH=&m!OPzgT{wJ`TTPmg$=|Sq|tEH3J%^31la>Est^>`W*~Nz*qVVL_d^Xs
zbX69BsuZXwf~vvR41_mv7?8%wk%w&%jYEQUB(`QCynMz86G$@<t50hfvOpyn*y$+E
zKv0nc7DY7;+zdqa38>TptE=HIfj9ja7_vY`0a%=*W+2=Icr#F~1bet6xdChqm;f~c
zYZ%}SK{f`4)$sVK<;znc&#X0E=uJwDrl2C&dN5Ig)Nn%$J5(OLDTp;*Mw@~RkQqp5
zgP<r7R5OKuY6(z`LMjz-eNqIf$ErZH$Dnp0%4lPFW=Te1Kzz0@W^3*n$hZg)0jjA`
z$|-o;rU5)mjH^8i>Z>CS>f)LRW<YPbp|pzOc@{KbjM~Bj4N5^hugP3w268HRbPQDY
zuVe-{^}v<+El$t~Y<x+6ylXKiDJdu@K*9x7wO8puD_cV00&NEV12uv`BeM+*AJ`Z~
z#jcBJUJ}u~D58BuM7x6%t!22DX${+o${lJKH5{*KI9}v;yujgzS?t4ulSWO&q8L!n
zB!E%|C^TWd2Q^}<42kqTqM63d$iPsP1TqnnF3?(eMadwE6i|1A<rYg~QF<|=DF<#W
ztz<5$1u20y@Kz!>@W2f=)V5tw21q$LUJ(sDa2pV{VONw1QU_Xl0BI(I)^-%-fW$!E
z;i6oS??DcR<O?JcoD^=!;thu)P(uF#iusFJ8(;#gtWJ!e)fI4(2YVZg3DNq3w8X$|
zFhqNZiFJ6ky$nyJUq~+ez`#(==*V<|PxT`M1HTi~7chyf9R^VkZmJbs;VbH3{=h@6
zmYCRROAK5K!-&zA*l0@(lon9iUZ}j$mKX!U4hLdTj-r;BCuU2`3{>L|ot7B3sd7)q
z&<v<$R#XA1ib-sTA%zOG9cBj#6i^Fr2(_n>e1T|Bxr01HxII-=18U3Efrxq#K~aOo
zv#6bcfx!f1b0f$wP{RnNg%9f6f}-fN6V)eDYlvC`f@OjeF|`V6FoTLL2JqNAb*5H9
zeI~G(SZqd}T1DztgU0u3SdgYxnb)vjn_6AT>R03l3TW`WsuPIi3>w_zfMg5AG7|7O
zWh+P?G|~(nzAXazw#XHfX+R?hNG(@LFXk3(8Bt&WybI$RpY2)H1)3>jWMC*>1s<{m
z&5mNunL_75uM4VO5>&e=sBuM5V*|$p4$F%imRC3|8yr8dF>vyt%$$PEq0!7Kj#Wvc
zi4l42GsTIqs1tY619ze+O?tjXpp_y$lqNm$r|dtlF>vvLhP^>E_ry)w6H0p9M$19a
zA_UMR4!D$LKw4^_Lfq0A4wRWYQ0)RXDTRfog*BYuWi9y1Ue+~i*vei_cBHcR<=_AR
z|Gxy;T|}T1z9j%&@CVwB;hR_in)HNBM8Oxi;46})prZ*$MY1o{q$1Gj`z4^faRpDY
z3@v1@3o2g{RK6&vdPPumMZ^UTvx^*NS2)Z-<uMP4K&;>=dO6EoPVOFw2}ToCW(dv^
zo2a{>;sU3^MNWe&oCXa}5IMsM%HXxZItwx`aOz*=)W5>1-{5oul#ibBi{oATg2+Xh
zDz`+Dwyx-<<rkHM_paPx%`43<s03Ghw*+$&OA2!GOL8)k^a?6LtXphQnz<mS<d$@9
zK~8*0etu4NW=XMLE=YlKie6$xYO!(3Ep{mX78_K6slfP_2$G43uzg8IpebY&%aZeR
z@{5XZu|a8;<oukXTg=6k#kcstld5|8&^cA`B;zg4!UD+hr(#HL3~DUg;(@4ytWYlk
zE$#u&AcFT<fY*Y97xET?#tOk}=)jAZz*BkP;UsVq3|vWri%W32Sjhlx&4P!desS33
z=BJeAq}mlNVPIeY<-Fo9Mh1ot%#4hTHyD&Jz|aE*p$jl{gF*fRD!Rd-aRCRq!Ju~m
z72RM^xPXdoFmN@1;SC1S3ovwpLGA)7LUs?tVVG(@@USzAd|-eRQoJB8oKO)1ap8oK
zAczYm^h7{hIAJ6U;=&0BMz{<Eh-GF5l7<s?j5Z)XoN!=*GZ{cE2PTjvuud>xfX0Qp
KkpYDTjw=ApGLaPk

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..889fd0df003b9827bf0a395a686252e188e1d423
GIT binary patch
literal 3429
zcmd1j<>g{vU|@)xtCp%K!NBks#6iZ)3=9ko3=9m#HVh05DGVu$ISf&ZoD3;UDa<Vl
zQA{Z;DXc9FQOqf9DeNr_Q7kDODV!|~QLOF^DO@SsEet8#%}h~jDU87knmjK-=K3YG
zfG8;DVPIfjXJBA(2HEh0k%6IvVF6<e<3h$-mJ)^qOkfr(Sd4igV=Wt448&qDVaQ@m
zXGmdO#F);I!UQIn7cr(YmoQ|pq_Ffd)pC?DEMTqS0I5r1O<}EJK*%j%1G#n~QySAk
zMzA_|h`Mx!6plrVFcV<1S<E#oDV#N|A`B_4>C82(H7p=HogoF)K2ETGoFMxkaximH
zYy+ucuYsvTw+$wa+gDs*Tev{B)G&ekg~L}Mb?h}@by$1_6K7n+n8I4amcm)XF2az`
zoWfeeUc;8oj4-p78*Cpp$i8%j6fQ*Qp!f=;ioFJ=3f))SH5@gZ0t_`=0t_|W0t~?n
zn%sVx%r6-k7#ND+geEhD%>)yL34m3AL{~E2VofQjEJy__d<jaIMPP9jumBPP5`Fpq
z|Ns9eW`GRR<haF@S8$6tIluT8b8%+gE!M=MqQuHu>}fgqi6zEHx7ad@OA?c_Z?PAp
z7H1?Dq~2mH0}E*K++s|*#afb)T9SB+vFH|SVopIu;w`4+l3Ps0CAXN86K^pUC*I;K
zDN4*Mj<1T(i!UfjExW~2lwX42aYAG(;`45?gCr{A^Hwqx$uKZ5{L0qP$j?pHPtPwd
z$WKWu0`rUz+?>+96#ast#EQ&f{nDcN;>_HFoYeT_{JfH){2cw<#JtRc(wxMS{G#}>
z%qsn&{G|Mn_~Me%v^4#!{LH+P_{8G)jQrgA+{6;Sg34Q5E*>TM9{IVxi6z3Ie5b^~
zz`(@_flAC8jB<=xENYB$j8HZ+h%LZWB*(zOfRcGRkuz@t18JGJh9!j&l&NaiL>OvV
zK?$&isfI;_A)N)3#XzDdj37Q#h7}}}!URg7H4N!2kSvzY0?TBz95o!l3@cgvia@0a
zBrULkk`gGHtYj_X0I@;tEdrHxMcg1RsO-_?ED`{*1wn)mhyaC1kqC$-3L?Zngg65O
z!!1@958uQRaCAWkS&%4qa#3nxNh(;*2o#tSNP#KGD8<Od$i~R@mxY<_FAGZ%D5Noh
z5ac#+`PW2d5T-EIFoA;&6qGE`pko(dsA1q{00kdd4#Z~x@oLy=7(i-p1!9pr$jb^0
z3=AX$4x2AH1Rwz;12O;{I>M09@y$=kOv_A70jse?4<#W+8AdVWP*MRUG*EVhRuT*#
zHmKGBhmsEi149i%2}2fR3Zn=^3X?QLHcOEJGQXCgh9L!16Tnr7Fk~|nsi2F2)CDtW
zviQ9OC7PEY8^Fm&lL?$&ZZQ>PYBCmqlLpvJU;^wfuH>A=+=38LY2XL)4k%FxG4e5r
zF$yuVFcxWmJOFb%j0TlT;EL3Qfq@~FA&M~tRDCk0FoCO2<`foi^~svT2C6<&*i$%K
z7^2uxI8(S<7^2uyxIq=@Emkj3c1{MRY?!eiHY=<KtpkM(YYjs@Lk(*RV>VL}ZwX@!
zQ!`@?Lp)O+a}5)St^x6wYnW>o;#q1~Y8c{KOV}2$*Dx&viF1^&E#QQ*xxnHqU~z7+
zIE2kp!dt_T#azRX#Z<$P#ZtqN#o7!qhc5+Gwlnz^fqbgTe~T3yNZ`PL1R^-6Au<^#
z-4}uLBsjZ)!mtQ6&w=yHEjCC|rO9@Sr8qSw4Qqzg0wp<c{D28?Byq&YXXa&=#K%X0
zq6m~P*%&#Pco^B3*cd^OhY<p07_0ce;i?DIkPJ$2umA?JLFogW_-4SeC_@$`z?e%I
z7Jz~P!ecFA$YKKpP6{I^SVS0V7_vYak0Fb-h9Qds%;yBjgGw`y3J@Ej2CRb_tOB79
zR_ZZiae!3SFl2F|WLs`fQ>TU@o(B~4;Eb!uTZ9N#aMDE!TyW6`FV%{`(E*MaP;pqK
z&A`C0lCek!OK@_R7No#Rx)M-`g0%86@-gZ#YA^~w^DoceDnUqCBB@RW6~eIK1hGLF
z9GsvMvxXsy0i0I3OBibyp=p()hB1YyggJ{vgh7k}oN5^sur6d^WB_?|B{L+Ay#yyT
zwjyxkgYq0CnSuSU4)TFM$T!Ro-+;pctWpLf1t!4BM5rjgB(WqlKCd)4wJ0+=F(;?8
z7UVsUF<eYMi~>wN%vBN)Phl#>@EbfYI)U;cQGP1{B@R%s0p%xlP(lIcCr*%Giwr;o
zF&Az>f(lDWmH@>^5xDvS#bOaCY|+bOey9h^GfOh!eO)}-Q9UZdR3(byQJ6Z63;_yx
za14TSZ3!ruGd6?T_>6v<Ot+YeONziLsYnkV)QIrrh>tHyEhvhQp9r!XWDplp282lY
zX>#1+h>y=p%uS7tzr__FpPQdjnge3<#K#wwCgwn7igZEdgJQYJ7{mgVtVL!Z7T7uj
z0gC7%O;9%nWG5)4b1-l)a!7G-a&Yl*fn_w=Z?Wc;<`z^!@{b(I6i{_t1P)+u>H-;n
n6ry0`K_=hguz|SG4pdeY^D!_ma4;}2aR_k8bMSBo2yp`d**2-)

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..936a5cf11707e3e29e66b708b2fad513c0bec527
GIT binary patch
literal 7553
zcmZ3^%ge>Uz`zhWS1naff`Q>Nhy%kcP{wB`1_p-d3@HpLj5!QZjGPQ9Of3viOexGQ
z3{lJ}EG-OCEGeul3{k914DJjmY%L5a?8}%K7*;bu^)p1Vr7#9FXmY#+3HT+$%mwi{
z7#J9s85kHoOR$0LC}9K%LU0zGUBkGHk%3_~oL|d=q7H5khB{Uhb#VPP%xLDZp{PTc
zhpvttMIFf9V5g=tq%baGOlL@80+Y;(7}J?cz%mdb3-04JET|z<%YkAdD?~j5Lk+4p
z+@=)P6xJFBVohJb1~U(tPGLijpEM@4@IbYl9i|(XtI`=#I2JJy=VJnP!F^i8lEPWT
zTE)u1kiweIT*F$!0;1CyQb-A1P8^|&Dh{_3VmfjDAUkYP?M8$xsyMQn$PZiGW|Es;
zxp0IisyN*BHB6usOY`)KYC9rKQN`h|qA0!MHj{A?V+v~xTMB0lJ1E_zGpDfDu-CAq
zGZX9oT5cSniz<%n4=!T!9ogv>)ow)CqKYHCiTw1+UBgksS;fM@P{Rc#xj|$wgC@71
zCi6>B@m&Neo56%8Gngs@6|G<vOaQC`B)XFE7HdjLWkD)f`X$JPVAU&Gzye4FNcGGA
z|Ns9-F#}|fCdVzNyn<WI$@#^%n2R&>Zm}j76(v^QVo%G-Pb@Juy2X}JT#}faeT%&)
zwKyZOAoUho8CXD*=N4niE!L8Z)RM$oj77It6LShO5^pgjm)v41F1f{&oOp|=IPn%=
zNl{{6aeP&LUVK4OYS}HGqWlsBj}szW5ubO99VAf^pI6Moz`&q@1b$`fXXNLm>Zj)y
z7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y$d~s%OK~8FXa(-S(QGSkoZem_$L1|86Nq$j$
zS!R`fQGQZ>NqlihX<C|oR(@t)Nqk~)d`5n5d~RZiUP0w8E*FoIe2@HG-^7w4c?Jds
zVFm_<;#o`#3=Ir-Ik|hpCa`wO_Q*Ck5y|<=#vmp!C1OT72(DlU!42XdctOtcsL&Cq
z3!*+3MSZS_`ZW066&0Tnxqxpg(-w{$iaQ*4B<>JD%XEh8f<f>_gWxL$!PgC<FBwE%
zh>5)r7k|+p;fg`R1^L8_qDfaolTbBF?vUIec~<F+)&+ymiw2=r3_>r+hg}p6zakpm
z;P;h-K}2kd#f+o{LJOQ0iY_Re5wpT+MbQSu6_y*)LGXf%-O;2YSr>!@FA4`<5e{td
zxho_xMRP{XTB9`<8{9$gsL>It3)+4cwf(MW`(4)#yQCd<Aw1$jWYk6N=quXM7i3~C
z3dLR#ibd7xxxsUT=h2`eQ5UoWE@}r{(GIvE6L?W5=!#HKgU?qs20`H|95d8GaDh1p
zu5bsz3z8;#o%VQL5OBXJ;C@BGy}|RYfY21q8J0_vmSk*T+@QF@aYgywq&-;|)I2Y$
zd0tWTysj2_NiFa~Q1FG2(2HteSJc8TNQGY%h`1sUfueauB?um6I>L29&HJL7_Z2no
z3sOE81$?gv_%?WcWn<tM?8%tH4uT657dR~_TA{SCbb-tU#tljvf>u;s5Qjt{pU*`;
zpDTPm4eodO1bQ-Oh%Z%IqOrmp1otZK(Ym1IdQr*sijwPfCErU*z9$kd1O#4G3c8{c
zbU`BcB45ZAz7SNMmMbh*Sc1K*<bF}f{fd(N1qqLfe4bbMJR95}a7)hcxyY@t!tx@w
z$%gWa+;$gO>^_59FWB3hR*XZp%~`{e!U$;})v$qTn;KR~ty9BP!vf-`vw+%65V;ga
zh!|W2xLuXblEMV4pK2J=Ss*Q+bQa>;KD8V*9Kj4LS^bJQ85kHK1u3W@Uj%Buu4F9&
zbt#ItK~kX7s)!fF;$vW7(Bv!<0<l4*Mv(}JB?=<MK!iAmkN^>q3=9mnSY14P6HAIg
z=EDoSA~}!@cXCl`Vo54kzDSjUfx!q=25n#<w+y<=!`I_6gK37-0->4i3zBB~tWa7}
zv{HQs;|`~tY!}4sZ%9Zj5L%G5P#hw@!D*!qlAPoMr4>Ot1TTnN-w?OD07f6!nZ@kB
zvN5<ZdN3Y|`oPZ28_4*P0YrWQlMSvPI2d>Y8+?is7#J8(QYOf|pnB@F&9F$BDNHp?
z;B*K{p)By!2yP$MFmN+KQY2UnM2rO@Qo~lm08vd|+ALB6r6gt2(j1#FI8k9sbi$BC
z=bN9BnU<NF0#;X~!N9;^2TFXa$xnQ|{5=sfgk}US5SkgbAaDia1qrPilF~DhW{NLR
zT2Qo5eTC7AqLpSF95)#4VA^PYLBbkcws3>d%CZX*W;Y~MX7J45`M}OBq4t%HLC;_f
zDCzN8eq><gv-|=g8r(qXPN=~blI}pwDNxY}!k`8aINfam_w;KRP|5+g$`nRW$&$iU
z#m&Hw4bsC<Bt%$cEkg}M3Tl5CtBD}9vq7$5C=w-BJ;>}}22B>fm!O8}OOTtuWs)Wn
zxDdL<RFJ93SOh9>ia~ZNK#CBq<ebFZf)G&KtVoxEfx!<HiILz)yur@XThvoJp|G=Z
zhG1v)C3dL`>{6gQm30Q=M9vwC6S+HBZ?N-p6ip495;`MrV&sg%iP0DN6)&+XUSL<e
zAs{q0X-evh#EF>;7$;_56j175y}>O2%j#gyK?#s^K+MlN;2=+Bh+<3u4XQAL2US1=
zEG*yw7FO^83mbTVg*}BWm_d{M7ONMiX_k!Ky9cqsF}MI+a@4TaFvNq52jd#n6vk{&
zbC#h<v;?FIEXu%8gVyh_VTcE-0!inw*DxXTYZz-7;^BI0m}?l~L7@#&%22~n!w?TD
z`@y^tP<an#F)(C-q6N&xVi%~&0L!810F{AYQ7k$@6$@AnMF+wT7SwT!8isgKErh89
zVF#uTP?ZJIAq}R$L=8h0!tV@O@KCB@K#c9wFl52QU>OSo!)iqMFvNqZKd?zD48aVV
zOn%^=h$jCnR&Y`Tr$TTtg;bD;x)J1KNL321B|(lWLai#nb>J;FNTXbn?G{UMYEBx&
z7*M)Ls%H&B$sCkWAT=r^FL1=iXXa&=#K%|hfm4+pj2{J3JqMC0M5Q`ddbl2N3rukA
zvg)w9E2+G|^`fNK6-liQmmaSUuLqKHAjS<rp$?WFHW&rb1!^Cw%@CcaJ4LsHrH7p~
zu9)Hil_hE`RMu#1NZFFMBV|w4MKRASVxAo=J=`7K9o(2j87NVKqU!T8a7IAsP{Z;Z
zLk&_CVk8-mZg5ng=tYW5MD%0mg(m<My`V4$n~0o@QW%jF45;v{LCP*rKHR(-hAfcJ
zz@|XuIKdnS29R&yZU)Jvvml#{Y$v)6U^k%XC14NSJrH|Rn2^;Yn_k0^1xmVLC*r8W
z;aL-XP_%|29+b|(dO_I*T$yX~f=hHrUH~U-v<v|%b&B8(NJz2=w>CjpuL#sgT*+8u
z3`*3XWB{!RxJwICV2#NtK}ZHb5&~6j#h1W&0FrJ+=15%>Qn@0ef=J4!LShnAG8U*T
zQD0%Sfn^K(N}C-jd(<y#J037OVtyjzO!!6TkPFfASE3Uyq-0!4$-Wq!eL*qjhK#}-
z_Z2E@)Hk?nalfc#y(8#=$`SRQF(+KkxL>plz7QIDB{cd%T+)@e)Qh317o^i}$jB~8
zS(3gXWlQ>jk|Sjor2TG4+g$*o5A4hePK;mK7`zyL7|%$3U}qK&1+_lKLm9s?fY_)x
z2Ojs(VR#V#Gbqzf2c;@-41fq6)d$+JJaToBCtkyt!c+oJ`3y(}B`EDwv4PSpqPnSJ
zSO8C3ARA$L83O|YsCfn9GcthE!%AjI@%a)|3~RC#fyy<c5)_o~ia@n$kvRhc!%Ajw
zq(cf<kji3^9~Bg!2|%bQza+6FH9oI2H?=4;IWZ@vvPuGy4lreEL8-?JoE#o-^Y>WI
z5bUz=u*X&!E>K*Wv?O(*=0!;zP{H_tje(Q*I)}t14vC8#Qdc;nF2E3^00TJ*;zI@o
zP>_N6pHG1s1~m*>Fc}8gM?(obQ-Y!=3*;8C?Z^ctS~iClOBi(?ynq5lT9GBFG0ufM
zvOukANVSEi{Xn_4$Ql&tpb&va8b35@$}>wc;(c8_szgy@3?|$T(iliabUhW*T%oc?
zZG*}dtsN<Q(hj5?$+{>OcttD_HS>VV5pd`rXC5tZtRWZwH4KQEEYu1km_d`#Pm}2u
zb8$%#xP&S)134LFE;M;^#K#w<78D`Z&Jg}YkU~)L*1&L=Uuc5K6zhxp@>lrfFL20%
zla!w($1RTd_`Jm2)cE*YT=DU_`6;D2AU02Yd|_!~4n(HN1k_Ig<&z>C5DV0uDRKm5
z8wRj3;5I^$K1d2gfZE8#8Q>6YV1U36Onj_fj2{?~NE0^JP{t1oNTd)4tMms3I3dE$
z8pQa40f`i0WL5aUfJs0c0kRs@?9D69EvST41E9f2aLWhW3xni-kg_6IkXu0=Yp~OQ
zao9jSZdc^az`y_s+2R%k28IvJjEsyo7=$js&<7SyMwSl>(v0>W7~q5*C!^p81~?(X
i$tVQkLJ0*{kSv@Kvt(5Gz<^171c`kC5ooetzX1U4<C&-c

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..52c98c629ed92fd3341ffabf8e1bbe2bf822ce9b
GIT binary patch
literal 1462
zcmd1j<>g{vU|@)xtCniW!octt#6iYP3=9ko3=9m#CJYP=DGVu$ISf&ZDNHHMEeugi
zDJ&_hEeui2DQqe1EeugCDI6)BEeuhtDO@SsEeug?DU87knmjK-=J_RqOaie$n3;ir
zfrWvA!5L(-6$1l93Bv-$8paw135JD?wM;ckDU6Z~DNNZ6Mbb4)Da=T078pB)HJBlV
zEto-*-LHs|fq^0Y&&rRNe3KIFUxIiqLAHgsX)@hn$}70VoS0Os$$X2sAU^dLTS;P3
zdTPln#?+MzMVt%_48IceGxBp&_0#i<3-VJEi@-c11UIKNFGatgD6t~5SiiI=zBn_t
zASX3GIX|zYC_hI(H!&}>pfo43B)=%WEVD|#C_gE`B)+($G%ZcPv?Md9Sg)Y+7FTLf
zQGQW;NosKk$k)Xj3=9l>j4F&Qj4Vt=+zbp1D1ifVI!GBfa0I}Clf}4z2^KuT3@aJ^
zG?|M)N^Y^`WacI2q~Bu6%P-2+WV*$hn4FQBT9%p;pPrwXbBhH;f_(`gKyiJG7bF;;
zl384mn3tRivc4E(r2u1*Ajm18-~+`G2!q5~VezEEz`#($kOjtBi~<a`j3rD9m`j+l
zSQfC>FfL?V$QaB}#KXYApviWNJu5LezN9Gg7Gns+p)AFPMI}YN3=9mnm{U?qG?{L(
zf`jT7qc6m1pr|MUyH6%LF((-mq^Z8pz)i}`$t<b#%u7kF;0L7`kk|Pbg&2z@&>aj)
zSRf1zJ1r~@&SGA`0&*-Ufq@cQ4buYFg$$Am!3>&AenlYvYO)mxF)%O`34;ibO;A@C
zm!;fdODsyy&CJteE)oODii5l(l9HL2o}ZVP6Q7?JUtE>~qA>g<2lrEOZemVOYH<nJ
zZ?2i?sd;6IIUu`=*}$nnfRTr>NCw?sAUROrfc@1)&|fJ`wair<B}`e&kf^C;31`TY
zVqpM@WU*BmGcwe&GBQ;0l(45TH#0Iaq%hVphciSnq%hR7*0JWXLg;eFa)uJ71rT#M
zL7}#gv4*LJX#p1~#A=vRSmrPXGib6x!VR1Ys}zF#z5PNx6hak(J^kE6eH?>4LtH}?
z5kQmq7B@K4#Akxy^Oj&SG(h50p@DIWyErp1y)-AWD6^zelN(!X6iI?Yo;fqG>=w6A
zW}ahCx+^G$-C`{$%FHV%5&=byD2R{(Cr3nV6-k3c<w3exed9Cp%8EdF5j_P;!(%HT
zwJ0sWC>NAbd{aw`GLwrzNv;?aKYWY=j4X^yi~@{8j2w&tj2sP2{{)!17?~K^{uQZ!
z>}9{jnpc`zPzlZf;Cv3wVc=8?j$*JfB+C(bfWsy?KczG$)ee*!i$R%+gMovYgPntg
IgN26`0QwI?5C8xG

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ac5e6c96318a30402f6390f79b59f8d9d753523f
GIT binary patch
literal 2594
zcmZ3^%ge>Uz`zhWS1r|$g@NHQhy%k+P{wB+1_p-d3@HpLj5!QZj44bl3{gxe%q<L2
z%qc7_3{fm8tSt;ttSM|Q3{h+;jKK_=>@PuD{gPoOfOsqn3=GT+3=E$ez^0Wjf&?Kr
z3(l@#tYN6)VqjRt$iT1~E?Ub}!<51(2~o_zz>va}4Q3Zf)i9+nV-e@8VM<}aBF>b;
z8qAQw26CZa5y)}re^!3H<eQXW{}N=`OOQ~An<mpOro4h%%!x_Gn#{MD3*u96v6Uni
zrKgtMVoWUt`9VQJ;a7rwMt*LpetLd!L4HbN5twI$;O3O(rRWzFC01k>>z5YA7iZ=c
z<fO(Y=jW9a<>%<<Cgx=pl;$Ls<QK)4Wmf4I<tOEr#21&8rlskZmSpA>>lIYq;z}(l
z$}fs9Ni8lZ;$dK50EJ+&3j+f~1H)Yot{%3IvKh>sH5WN#u5idSINo6A=n!lv=}2v=
zX{edOI74v`%Z$=lJTrJ!2(B<(BeH>YmCOp68~nl@B@g&TI!Zr-LJ)fdxj`ZbqyUN$
z5wrjlL{L!#g+1DW8CEj-X)+hFF)%RPV#~?QOUy~X#gdm_l&i^fi#IVjBQv!uH6=bh
zKQZSP3y3TR8LpsE#0}EG3lfP>$t*5O%u7x!5@BFq0Erh%Ljr)Cx5u*IuFGzQ$^zMo
zQhFD;^{;U2UtrNM;$>i9NCw3w%z3P^gx3T1R}DiJOojoOjqqa?3j;$fV+lwfRAm+@
z(!uN!P$Ym^3=COt_5vieP)P;`h8jlHWQ@uSW+>ukU|`T>yTzWBm>geHlzEFW1QJ#(
z#f3#BMFOBuU`|Ob(PX;C3Qi!m7=2NKNG3TkCmEEAQhlN6DJe52v!v29FD132NQQxd
z0c1n50XW2NaPao<&S0IxJBR-whkAqKU2fhU^QlHtET`K}vRxo}QCRtku<}K2l`GsT
z7g$s<gAo)2ApCg+X~75&BnAdVFfKq6#T{VCc^*^-)G%QW=w%EH46EVBAo6`MgC>(-
z5y&b{wjwbG28JSW5CKY$(9kU|OS#3CSd^ZdnWxEIBn6TMrLS8eDVd4s`FV*s@%d@-
z#bqfV$`_XPA?Z^N9{9nzi8(o`#U<cKaLr6l%_~dHDN<x$U=U_tU?_IR9{kd>b1diE
z&az!0cu`vWinR6$$1Bo$7g!{2aLZp%FuAT^eM!OkqJr%e1=}m!b{AOeFk=Li=)f_u
zgAr6dBPV8LHb%k*1qd{>D2tI4rdnoB29zuTau3*S>@kI&C2CpnjFfShkOhh-uvsb$
zMbfpbC7{F%<})y)Fr#KBMurr|TINWG6oy*XI@UZ62wl!t&QQS|$xx1(nK___;IMZA
zCrlohMt3=?{WVNAObfs@DY70A8&}$_VNPM0!yL?@$qGrK;OeMKA;{m`FVsUJR3X^Y
z&pp(~F~~E-HAE2sG?{O4gDa2tOi(7jC0GnieDSH!gm;U(I5RK3G$*krv!qg!8(X?B
zk^>hlnR#WmxP3D79COlLK{d@S)`FtUypkdbP&NSNq9S>40fopHMG7D(Rgi90-}ubD
zvLcY5i$M{k00xk(Aq~$O0jWi4`9-;)65Kbnq$o4FxJZkEfdOPg@dXm|g^+NA_Xj2x
zR*Aa;LQ^=W^G@QOk$6!+;fjDl2WtoG4^Vk#d4*fy0|OJQ1cJEB!Fz$v;39|N6%NA-
z9ELZ<#5=eV@W+oIKR&QBu}b{-@#8BSgN)o9^QA^hESK9ZvfUtfQQ7#4vhhV}lPl6D
zHxx}Tn0Z|{3%q0&c+o8Qidpa##gH3PsvlUHcr-pRKnPZiVo(9ISKd*U;h?OD6A$x2
zLqR8Y=0ohPPCU$-?6+9+N^=V;!G%1yMhDkw;DQ=lT!6!7B?CC8gG2HchfQvNN@-52
zU6C#W0|Tg5D^6ozVEDky$jEquLFfVuePCl{6#c*eC-@i{4L&fy2@WPk)ej7Cf`ggS
T{sRM?;NoV~_`rZkfZYKAJBtKD

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3b23fd5bc9161a79a4c7ff16b024515fc9c2e198
GIT binary patch
literal 4103
zcmd1j<>g{vU|@)xtCkuk$-wX!#6iYP3=9ko3=9m#B@7G<DGVu$ISf&ZDNHHMEeugi
zDJ&_hEeui2DQqe1EeugCDI6)BEeuhtDO@SsEeug?DLg5>EeuiYDSRpXEeuf{DFP{i
zEeuhdDMBg2EeugyDIzJNEeuiIDPk$&EeugSDH17?Eeuh-DN-rYEeuh7DU87knldjz
z?(s`z1W`~7auq8B1A{XI1H<P61_p+7hFZoN#sv&D3=0@j7#A`wWMpK3^O?~2%xHWT
zG(IaDpAEt<VM<|d2AdnqpvmEPOQ<*_zceQ$wJ5$IF)uzVKQpi7mM~l_Co`=CD#)Fh
zlayZ$<?xmlWfqsj8=`WJP`Sns?l1n#+=Bd~lGGH1-29UKqT-dzx7ZR33R3e@ZgHfi
zmiU4MZ?P5>W#*NvWV*##l%JGea*GYBmZ3<Hfq~&yp?*ewZmNEIesMv5N@5Y1XN2J9
zl;)-A7ZfE{WESg}7R485<`(3n#wX|Jl@#UY=;tQpWfqj?B$nhC#g}DP>4R;EFD@xf
zOVck;P0BARj!!JejL%5TDM&4fPb<wU)+?yI#gm>|;+T^Ic3v?%DD?<2Ffgz%3Lrra
zMi#~*VFm_<WM*ijf+!XS1_m~0#1>CtU|^_WTEGB`&|2mi<^_x?OyGz`Wii9W5G)qB
z7>vb~!kWUG!j{6ekcE+<hIs*YK6?r~iaeBG!koeZiux3WU<OT2zgz4nnZ*S;iItfw
z|9>!KDlz<L$b9ynf#GEe0|UcL8xUd5z`#%?n^Kfou27PZs*s#nl$?{Qke>(U6(<*^
zrsip~+~P`4EpdSub&I1owZu6;C%@<xM@ms*xqn$=P7yBy1A`{ZEw-Y>l+4oNTWlGr
znduoNMf?m53`HU!0u;mGXf6VU@GUNoPG_)PAn9TW1_lN$MlL2UMi_)e0S{x5EGY0n
zISCZ-APkBKSgtD8U|?V<VJKm&VQ6M-W^`eQ<*sF_VW?q}WLUsd!w8~5L0rfN3X+wK
zewxg;Sc?+#(o=6S=cMM{;x0}t2}{k%PtGi<)MPFaU|?Xl#a3L9nwnC4i!l@8VzAr6
z&gLtIrsZI;3T04=0i^-~MiEAq|3xYwhazWx6o;C?vOj|hLo8n{6DY-$Fs3jz!wLZ=
zNUAAhDPgW*s9|hoVq^e0T9euD7IS7^S`o-(O%|||1M-VAOEUBGZn382<R_LCaf8Ab
zWc^BrABvPf9$^J%Fmz8yXXa&=WG3chR;5A`zFU3~JP1HO0u{6@j2w)7j695d%tcz*
zd<9DXphB%!57}4jwM?MG43rNT846iIkpl5Y2~!FaJY6#TfxS}2my!uBFBCvd(qu+>
z1Dv3WKv7GK>tUe*3gKdy>v@>Cn0XkBw6VD!mUuq5z+6w`BDn^XFhQZiw16pv9h@Ld
zV0pC$RC#dv-I9bC$?#%2zBo0nIKSwY6kG~XTEpaokP2;>m<USQjV1~$!qG&bB{@{|
zB`6esNyAI^#Jm)69HpixK%7*(k_8-Fw*=EuOJLz23~?=6YF=VePAWJKS2BYOfm`fQ
zZMWDU4D@If0hPpgDV}+mpaL0UP_aEIHHu-D$gDh!984^XMWC_^rMgAQZwj!u1?4wR
zNXo2XTEMuF0UZ7;pl~me0~K6M5U+r0s<6bI(o{|6B1=&I;Rn~1@deP#R$OES%4Xo=
z08Bv2grd~M6faPv8eEcCk{SqdGY8ns9E<{>y2B1V1%iq)SPCoxaX^){K$cOSS+TK!
z0i;HaH@t-|VuT@Lj3HuzA!3RlVphZn@&+h|i}*k+uwTFg*h2!KsDM?9UWv*1(I78@
zq|hoxP!zbLdk#4YigZAs%jtJZ9Nlx!WQHMOgiFF0mxKu}2~%7WW>ASDW02=TWk!)H
zh-CpHz`+0}Kpws&0SXKBWC;qB3{aS0Nt$fXr0IhmE}(dZMf&FkSkh#qZA-8QW<EQ-
zWe9E_fRa9_dB6cl;E9H}U@Rjz%NWiwfwN5EEHgOEoRs9`U7DR(2Wkt0YJJq?B)|x6
zX@#N(9CCvCJY|3a4yF9&0GA&&CCn+DpgIj4m|T9hpru8kAvAr#1>lJqrUIUrVFK_(
z3=@DSUYGzZ(ZUTihuT7RDh5SGHz+DVZ7+<dfEBUv=+S|kj*EPu`CA~%sMs96@Iny*
z7hWhL;KB<<1YCHbh=2<(6cIB}afTv7DE)ydD0mwmly%J)fHHa*hydkcoE1n>ILLQg
z;L;1lMtB5^qj?)1(dZKJ2uGKIM?AU&JOQ9fz!L(x1SttZ5?8Tiz7`ZL2_OR0Th-*h
z#hO=|TTpq6DX#$B2?969!3|w-yAa&G0@uSuplYrNR2>w7%Dy5{J}v^Ko+3~n7lBMe
zs`C*gEQd{QeoARhsvW3xTP(xCz`()G!OX$S!Oy|Q!Og+VA;iJO!OJ1W!OJ1a!wUd-
CodpO0

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..fd4d62b162bb90fbf4b4b0b69abfcdf75bcc9425
GIT binary patch
literal 11565
zcmZ3^%ge>Uz`zhWS1mP8l7Zndhy%k+P{!vN1_p-d3@HpLj5!QZj44bl3{gxe%q<L2
z%qc7_3{fm8tSt;ttSM|Q3{h+;>@5sY>?s^A3{e~@oGlDdoGDx_3{hMu+${`I+$lUQ
z3{gBOye$k-yeWJw3{iY3jKK_={4YUP`6V-gC@5xSU|?WoU|{&n!345BouQVohH(K@
z5|yf9Sb(e(E}6o(jFEw1HCzsr$H+jkc}%D#pz_Etj~UejR2~`Tv7nlO$|J)(R#X#E
zd1RQ!hH3&TkCCAS6z9;`OJPTstpTMJ4!>JM#TogfIVq_{@db%_@mcwqc_p`m;bJ+N
zX(doW?$n&5{BkIVx4bB`xFp^Xm1~5`HHL71@n_~1<QJ8srYPj*m*f`}uVlW(mRL}b
znwN5mBR#dm7bJL#wV)_7uVf|DE!Lv^r2LXwY!J1@plqX{@T*WiBR@A)KRv&=AU`Fs
z2+T7=aC1uYQuGUo5-T!`^-GK5i!*Zza#G`y^YcoI@^kca6Z0|)N^=rR@{8ijGOP5#
zHpCZ~l%}QWm!~G>mlVe*7G%a}q~;W)7R9HP<`wG|RNmrAPc3oG$pJgBxJa0Rfq|WY
zfuZ;!0|P??!wp{k2Dck5T<yM1z7tfZ>rK+TplEtg$n1)c*+mxfD=g+0Sj^F+%`OU=
zUlB6D$YODY#o_{s1&Z_n*^5H@SA_I0vKU-pF}T2DfGR70QONL$kl{rZqbn>%7g&ra
zk-f{pd4Ws%B8$uw7MTkyGB>yd8+<>5(n&I?fB-Q-m<?87aDfYq8Yb+81$v=S%Ur{Z
zqxe9}?u-mnP>Gi1@t8@xO0;Z`$4qQ0LFp9~ekeGFHH9^WEro3v3j@Py7MK*+Wa_DB
zPhls?d@SlqKq(yRyc7=fA|r(%m_d`%?-qMXW^qAIVr3@F{~rvQN(}!QGN1itV0f9r
zz`*d*21HmhFfdffrWB=?E0koUDkNtXCFi6n<mZ8T#mPmfsd<_#x46<%OI#pE-Qp-t
zEpg7z$uGLaky4ac?q8OeQ^d!>z@W);i>)XzC9|~n7F$MYW_m_RkpKe&Ly;ILD}q8&
zK|w*GNQi-f;T9K2qchm5B6$V|1`&`FE=F)}<rBWnCwGZYZbs!rKFuq9nhov`Sh%mV
zh+JY3nGkW2MeYiV+yxf78*~*5o*_IVXaUnj7Ue4}$`@Fa5w=r92As<^FS2M|VbQt(
zLq(u4Nk&d`pgIDS?LW7HTO}p%+MR)+1SAek3^fd>i2;@8!Vs%m%T&Wq!z2k+&%m$%
z*%r8H4I{D|R8trkdQ?D3Y$c<gCi5-UqQt!P)LYCssd=}!i&IO&QgiZ?GfOHpnTrHL
zvCURokeZrOe2X!&801cPD&Q-I)+oVX`66`&1_otNO7H-e+jn_|rW#Kun`k}7`XaCV
z6<+xd42-OrV4{QZ1_xh9VGrkZ4%tf_vNO^a1kTC4$f0qCL*oJ%ePD(ug@hwgSTVqh
z%SGUD1h=mkTo__4YngBroyg&fUW}rLObsLU@JzudRC}zDGjR<=4dXH<28Pw}*kEJ;
zg|Q~H-!10MytE>axF!oYgah)6GfOh_^KP-G<>V)p6!C!a87TNxLV~<V6%_8Qka`&<
z%%wB)GD|WOb26(^A+@Djei1Cbiu6Fa7!(1q;0S<JoPy#X7?@Zk!9)k^4G!Mx9Fmtf
zB<HFwV4bNmN9Q7k`V|iK3mob<IQaT`yLcxgUgVIt!Xa@1jJ~olunJEwn_+g5Md=EQ
z(ghg8j1^D;0dnc*8q`=3uVup3T1AfuMur{<<e0)1q9`pbj2MB2H#n%P_);>V%^(F(
zplLEAf*D-i73qM23gl*ZipCbYu(&ERU|?V{0ci=s8M+UM3qx>$A$5^O`U;Em1sK8%
zQh0%318x_96Z&XJ0$jl1>^Pyd6R>1%uzqv`r+IYlNYpU8FvNng3=@t*2%~e+BY|89
zVYVc|oexgGTaxgO2E6kTU!0m(oL_WH3ND4{gTUm3kh&r;F%gs=37ROhV}d3M?VmtJ
zUxK>szog-PmBhRha28EXQGht9cqI!sv)&R+Pc4CEz+i}L*;4ZolX6nQS$HKgxQTL$
z9jfgX8<c^Pt3^P)i@X%iyi8Ch17b{Zkplw*gFPravqRf00uAmrSh(B$n*1i1UKiH7
zB&>Bo+wG#T`xRmLi!2^jSUfJUcwi`SzbNc+McCsai{}*<&kHP`7z*q!3Oig8cDTsm
zc!kCB0*fPt0>_KOPFIATF0wdZVR62|;!L@M4_pkwQlNSkLUgdfiuR<79FkW!Brkx`
zQ&8VT{UVFT6&8&PEE+esg&O?ultD?T#kwZ$V!eh5d#_{}0|Ucqc*aA|nLS#dlv<<+
zs=}Bckq_#zh9%~drfM=5*?}rVesG^Tz5rT#78ilz4<)Yoic%9(yg*&+;F83W)FKxK
z28KXTEZaf~Z83=k{~JP*pwKY_6QIx$zs@0di9>Ei+5Gxh^%vz0uE-l)<S@L#VR!+I
zZg5LoV3EQTHlqVQ@MwiHaF(Y-ynIG!eqfZvpcJgh;de_Q%P7yR*x0}TGAa{qh#_Kx
zA!3XnVuB%JiXmcF1S(34xIyhleo!(8H8NqnGyzb#2OCxKN=(i#@?l_Lhz2RU3ds#v
zMple33YlCHGP%fNdWFUG0*fii@W%zk!6XeX!O%um@Z{3bRunXI!l~h!OKI1N5=YP3
z&=$=tbO|F|62`bBOmIn<;*v0fN)%auvcDyWumTaFst&cME&(bD&|5&@5+Q_vfguA_
zBH(KUxm*->y&~*-k;Ux_i`xYjH;g9AU{vsdhX$>nh|!)Hy!3!DhHn{yeH0$2f6-b4
z=<QpSmIKCkFSzBv>353*(!xzNgtLs`EMqv!1kN&rv&`TubE4Y>-lf@zMWC^wI#3Dl
z7Dt<4fz(AIohw2*7g=<#u;^Z3(M4$z3=vuIkPGo`g`UxT4$0nd5@)&};w}0bQb*JX
z<a2^;4~)EzHiE;6UUGoT1}?u_(B5yNA+*|u3&7hNFct7N2223nwtxx1+Y~SXSX%;a
zs5#UYG8-4*q9u)ifuS2zv}oWNVzId>Y<or6_9Bbj6&AY-EOr=;iy^6iCSx$8tq*u{
z1Z9ktN|1m+Uvz?Ah7i~K5XdqrHb)<~KoJ3tTcC)5$1PAqz~dGuBH(cg6cIDfNCt`s
z-c}1}7#O}j1k`UaFUn_NU;wqyiof7!wp<ugV-+ZECX$^t+IoQ`bA}YOwI%53oaEYk
zwDp3t%n-CNj==mA*{v6G^s)uoz5tD!pcDk~)(pB1c&i3o0^Yhomw>l&&?Vrl9drre
zTRoDvhE&XpDi|0T))H#=48|c7jHVaKj-;^Jqsf1ZHLo<cpz;<|UIBO=Ab5EZcx4HA
zbpv=39z1mip4kBpXcvLTu)y8eB2a@9Tr+^<U?l^1G#I>O=@*AhZhlH>PO4oIXn_J~
zJxTEc1_p)?%#4hTA2=8og)cA&LC^;_PDY^*3~)k<mr?rz1Dud!V$}S=04I3Z8I?aU
zzzH!9Mwt%`a6(LgQTPJ`oKRt5l>Wc~Cq%eF7QqQAL6BlNp#zs-0I|?ywLoHE2{3^s
K%K$S490C9-rKdIk

literal 0
HcmV?d00001

diff --git a/manipulator_viz/robot_stuff/drawing.py b/manipulator_viz/robot_stuff/drawing.py
new file mode 100644
index 0000000..d2c85e2
--- /dev/null
+++ b/manipulator_viz/robot_stuff/drawing.py
@@ -0,0 +1,36 @@
+import numpy as np
+import matplotlib.pyplot as plt 
+import mpl_toolkits.mplot3d.axes3d as p3
+from matplotlib.animation import FuncAnimation
+import matplotlib.colors as colr
+
+
+def drawOrientation(ax, orientation, t_v, avg_link_lenth):
+    # every line is drawn be specifing its x, y and z coordinates
+    # thus we will take each relevant vector and turn every one of its coordinates into a steps array
+    # and every vector will be translated accordingly
+    # first we need to take rot mat and p out of hom mat
+    steps = np.arange(0.0, 1.0, 0.1) * (avg_link_lenth / 2)
+    
+    # now we draw the orientation of the current frame
+    col = ['b', 'g', 'r']
+    for i in range(0,3):
+        x = t_v[0] + orientation[i,0] * steps
+        y = t_v[1] + orientation[i,1] * steps
+        z = t_v[2] + orientation[i,2] * steps
+        ax.plot(x, y, z, color=col[i])
+
+
+def drawVector(ax, link, t_v, color_link):
+    # first let's draw the translation vector to the next frame
+    steps = np.arange(0.0, 1.0, 0.1)
+    x = t_v[0] + link[0] * steps
+    y = t_v[1] + link[1] * steps
+    z = t_v[2] + link[2] * steps
+    ax.plot(x, y, z, color=color_link)
+
+
+
+def drawPoint(ax, p, color_inside, marker):    
+    point, = ax.plot([p[0]], [p[1]], [p[2]], markerfacecolor=color_inside, markeredgecolor=color_inside, marker=marker, markersize=5.5, alpha=0.9)
+    return point
diff --git a/manipulator_viz/robot_stuff/drawing_for_anim.py b/manipulator_viz/robot_stuff/drawing_for_anim.py
new file mode 100644
index 0000000..f40ad14
--- /dev/null
+++ b/manipulator_viz/robot_stuff/drawing_for_anim.py
@@ -0,0 +1,38 @@
+import numpy as np
+import matplotlib.pyplot as plt 
+import mpl_toolkits.mplot3d.axes3d as p3
+from matplotlib.animation import FuncAnimation
+import matplotlib.colors as colr
+
+
+def drawOrientationAnim(ax, orientation, orientation_lines, t_v, avg_link_lenth):
+    # every line is drawn be specifing its x, y and z coordinates
+    # thus we will take each relevant vector and turn every one of its coordinates into a steps array
+    # and every vector will be translated accordingly
+    # first we need to take rot mat and p out of hom mat
+    steps = np.arange(0.0, 1.0, 0.1) * (avg_link_lenth / 2)
+    
+    # now we draw the orientation of the current frame
+    col = ['b', 'g', 'r']
+    for i in range(0,3):
+        x = t_v[0] + orientation[i,0] * steps
+        y = t_v[1] + orientation[i,1] * steps
+        z = t_v[2] + orientation[i,2] * steps
+#        ax.plot(x, y, z, color=col[i])
+
+        orientation_lines[i].set_data(x, y)
+        orientation_lines[i].set_3d_properties(z)
+
+def drawVectorAnim(ax, link, link_line, t_v, color_link):
+    # first let's draw the translation vector to the next frame
+    steps = np.arange(0.0, 1.0, 0.1)
+    x = t_v[0] + link[0] * steps
+    y = t_v[1] + link[1] * steps
+    z = t_v[2] + link[2] * steps
+    #ax.plot(x, y, z, color=color_link)
+    link_line.set_data(x, y)
+    link_line.set_3d_properties(z)
+
+
+
+
diff --git a/manipulator_viz/robot_stuff/follow_curve.py b/manipulator_viz/robot_stuff/follow_curve.py
new file mode 100644
index 0000000..49feb20
--- /dev/null
+++ b/manipulator_viz/robot_stuff/follow_curve.py
@@ -0,0 +1,102 @@
+"""control_test controller."""
+
+from robot_stuff.forw_kinm import *
+from robot_stuff.inv_kinm import *
+#from anim_func import *
+import numpy as np
+#import matplotlib.pyplot as plt 
+#import mpl_toolkits.mplot3d.axes3d as p3
+#from matplotlib.animation import FuncAnimation
+#import matplotlib.colors as colr
+import sys
+import subprocess
+import scipy.optimize
+import random
+
+
+
+# angle in radians ofc
+def turnAngleToPlusMinusPiRange(angle):
+    return np.arcsin(np.sin(angle))
+# let's trace out a circle
+# th circle formula is:
+# x = radius * cos(t)
+# y = radius * sin(t)
+# z = height
+# the direction you are to move in in order to trace a circle,
+# or any parametric curve for that matter is
+# in the direction of the derivative at the given point
+# the derivative w.r.t. t is, by coordinate:
+# x = radius * -1 * sin(t)
+# y = radius * cos(t)
+# z = 0
+def goInACirleViaDerivative(radius, height, current_parameter):
+    return np.array([radius * -1 * np.sin(curve_parameter), radius * np.cos(curve_parameter), 0], dtype=float32)
+
+
+# or just pass the next point to reach and use the vector to get there lel
+def goInACirleViaPositionAroundZ(radius, height, current_parameter):
+    return np.array([radius * np.cos(curve_parameter), radius * np.sin(curve_parameter), height], dtype=np.float32)
+
+# here height is distance from yz plane
+# but also from xy plane (we want it to be floating around axis parallel to x axis actually)
+def goInACirleViaPositionAroundLiftedX(radius, height, x_0, current_parameter):
+    return np.array([x_0, radius * np.sin(curve_parameter), height
+                + radius * np.cos(curve_parameter)], dtype=np.float32)
+
+
+def error_test(robot, t):
+    e = abs(t - r.p_e)
+    if e[0] < 0.001 and e[1] < 0.001 and e[2] < 0.001:
+        return True
+    else:
+        return False
+
+# let radius be 0.4
+def goInACircleViaJacobian(circle_param):
+    """ doing a fixed circle with axis around x axis because
+    i only need one circle and this is easy
+    arg: s - the current parameter in the parametric eq."""
+    # R0c is the rotational matrix for the circle
+    # p is the vector to the circle center
+
+    # need to have a hom. transf. mat. to circle center
+    # first 3 entries are rot. mat. and last one is translation vec.
+    # with added hom. mat. padding
+    circ_hom_mat = np.array([[0.0, 0.0, -1.0, 0.0], \
+                            [0.0, -1.0, 0.0, 0.0],
+                            [1.0, 0.0, 0.0, 0.0],
+                            [-0.59, 0.0, 0.59, 1.0]], dtype=np.float32)
+    center = circ_hom_mat[0:3, 3]
+    R0c = circ_hom_mat[0:3, 0:3]
+
+    # shouldn't jacobian be a 3x3 mat? 
+    circle_jacobian_circle = np.array([-1 * np.sin(circle_param), np.cos(circle_param), 0], dtype=np.float32)
+    circle_jacobian_base = R0c @ circle_jacobian_circle
+
+    # now we need a radij-vector
+    # you need circle center coordinates and some point on the circle
+    # i can calculate p from current parameter value 
+    # i do that in circle's frame
+    # and then i can transport it to base frame with my hom. transform
+
+    point_on_c_circle = np.array([np.cos(circle_param), np.sin(circle_param), 0.0], dtype=np.float32)
+    point_on_c_circle = np.hstack((point_on_c_circle, 1))
+    point_on_c_base = circ_hom_mat @ point_on_c_circle
+
+    # need to make center hom.
+    center = np.hstack((center, 0))
+    radij_vec = point_on_c_base - center
+
+    radius = 0.4
+
+    s =  radius * np.arctan2(radij_vec[1], radij_vec[0])
+
+    s = turnAngleToPlusMinusPiRange(s) + np.pi - s 
+    es = s * 2 * np.pi * radius - s
+
+    e = es * circle_jacobian_base
+    return e
+    
+
+
diff --git a/manipulator_viz/robot_stuff/forw_kinm.py b/manipulator_viz/robot_stuff/forw_kinm.py
new file mode 100644
index 0000000..267373b
--- /dev/null
+++ b/manipulator_viz/robot_stuff/forw_kinm.py
@@ -0,0 +1,540 @@
+import numpy as np
+#import matplotlib.pyplot as plt 
+#import mpl_toolkits.mplot3d.axes3d as p3
+#from matplotlib.animation import FuncAnimation
+#import matplotlib.colors as colr
+from robot_stuff.webots_api_helper_funs import *
+import scipy.linalg
+
+#from drawing import *
+from robot_stuff.joint_as_hom_mat import *
+from robot_stuff.drawing import *
+from robot_stuff.drawing_for_anim import *
+
+"""
+The Robot_raw class is a collection of joints which
+together constitute the manipulator. 
+It has methods for calculating the Jacobian,
+the velocity manipulability Jacobian and two
+different methods for calculating a gradient
+toward a better manipulability positions.
+
+The Jacobian is calculated from its geometric
+derivation as described in Siciliano chapter 3,
+and so is the velocity manipulability Jacobian,
+as described in Geometry Aware Manipulability 
+Learning, Tracking and Transfer or in 
+Symbolic differentiation of the velocity mapping for a serial kinematic chain,
+by Bruyninckx. Here i avoided the tensor 
+formulation as to my knowledge
+numpy does not support tensor operations.
+
+
+Calculations of the gradients are described in the 
+Singularity Avoidance Using Riemannian Geometry paper.
+
+
+notes on code:
+- we are doing only positions, not orientations
+- dh parameters are read from a file
+- there is no other way to import a robot
+
+
+"""
+# TODO 
+# TODO 
+# TODO 
+# TODO add the following flags:
+# one for (not) drawing in matplotlib
+# one for (not) using Webots API
+
+
+
+# add a drawing flag so that you can turn it on and off
+# if sim is to be had, you must pass motors=[list_of_motors] 
+# and sensors=[list_of_sensors] 
+class Robot_raw:
+    #def __init__(self, clamp):
+    def __init__(self, ax=None, **kwargs):
+        try:
+            self.motors = kwargs["motors"]
+            self.sensors = kwargs["sensors"]
+            self.robot_name = kwargs["robot_name"]
+            self.sim = 1
+        except KeyError:
+            if len(kwargs) > 1:
+                print(" if sim is to be had, you must pass \n \
+                        motors=[list_of_motors] \n \
+                         and sensors=[list_of_sensors] in named args syntax")
+                exit(1)
+            else:
+                self.sim = 0
+                self.robot_name = "no_sim"
+            pass
+        #self.mode = 'training'
+        self.mode = 'eval'
+        self.clamp = 0
+        #self.clamp = 1
+        self.joints = []
+        # TODO FIX READING FILES 
+        if self.robot_name == "no_sim":
+            #fil = open('inverse_kinematics_gymified/envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            #fil = open('envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            #fil = open('arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            #fil = open('/chalmers/users/guberina/ActorCriticManipulationControl/inverse_kinematics_gymified/inverse_kinematics_gymified/envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            #fil = open('./arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            fil = open('./arms/ur5e_dh', 'r')
+            #fil = open('/home/gospodar/chalmers/ADL/ActorCriticManipulationControl/inverse_kinematics_gymified/inverse_kinematics_gymified/envs/arms/kuka_lbw_iiwa_dh_params', 'r')
+            print("i'm using: testing_dh_parameters (which are UR10e params)")
+        if self.robot_name == "UR10e":
+            fil = open('arms/ur10e_dh_parameters_from_the_ur_site', 'r')
+            print('im using: ur10e_dh_parameters_from_the_ur_site')
+        if self.robot_name == "base_link":
+            fil = open('arms/kuka_lbw_iiwa_dh_params', 'r')
+#            fil = open('./da   /lbr_iiwa_jos_jedan_dh', 'r')
+            print("i'm using: kuka_lbw_iiwa_dh_params")
+        if self.robot_name == "j2n6s300":
+            fil = open('arms/j2n6s300_dh_params', 'r')
+            print("i'm using: j2n6s300_dh_params")
+        if self.robot_name == "j2n6s300":
+            fil = open('arms/j2n6s300_dh_params', 'r')
+            print("i'm using: j2n6s300_dh_params")
+        if self.robot_name == "j2s7s300_link_base":
+            fil = open('arms/j2s7s300_dh_params', 'r')
+            print("i'm using: j2n6s300_dh_params")
+
+        params = fil.read().split('\n')
+        params.pop()
+        for p in params:
+            p = p.split(';')
+            self.joints.append(Joint(float(p[0]), float(p[1]), float(p[2]), float(p[3]), self.clamp))
+
+        self.ndof = len(self.joints)
+
+        fil.close()
+        self.jacobian = 0
+        self.calcJacobian()
+
+
+# below are some utility functions for drawing
+    def initDrawing(self, ax, color_link):
+        #initialize the plot for each line in order to animate 'em
+        # each joint equates to four lines: 3 for orientation and 1 for the link
+        # and each line has its x,y and z coordinate
+        # thus 1 joint is: [x_hat, y_hat, z_hat, p] 
+        # and lines are a list of all of those
+        self.ax = ax
+        self.color_link = color_link
+        self.lines = []
+        avg_link_lenth = 0
+        for j in range(self.ndof):
+            x_hat = self.joints[j].HomMat[0:3,0]
+            y_hat = self.joints[j].HomMat[0:3,1]
+            z_hat = self.joints[j].HomMat[0:3,2]
+            p = self.joints[j].HomMat[0:3,3]
+
+            line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r') 
+            line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g')
+            line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b')
+            line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link)
+
+            self.lines += [[line_x, line_y, line_z, line_p]]
+            avg_link_lenth += self.joints[j].d + self.joints[j].r
+
+        self.avg_link_lenth = avg_link_lenth / self.ndof
+
+
+# NOTE switched from ax as argument to self.ax
+    def drawStateAnim(self):
+        toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
+        drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2)
+        for j in range(len(self.joints)):
+            toBase.append(toBase[-1] @ self.joints[j].HomMat)
+            drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth)
+            drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link)
+
+
+# needed only for drawing, and even that is optional 
+    def saveConfToFile(self):
+        fil = open('robot_parameters', 'r')
+        params = fil.read().split('\n')
+        fil.close()
+        fil = open('robot_parameters', 'w')
+        params.pop()
+        back_to_string = ''
+        for i in range(len(params)):
+            params[i] = params[i].split(';')
+            params[i][1] = self.joints[i].theta
+            for j in range(4): #n of d-h params
+                back_to_string += str(params[i][j])
+                if j != 3:
+                    back_to_string += ';'
+                else:
+                    back_to_string += '\n'
+        fil.write(back_to_string) 
+        fil.close()
+
+
+# the jacobian is calulated for the end effector in base frame coordinates
+#   TODO: the base frame ain't aligned with world coordinate frame!!!
+#       ==> the only offset is on the z axis tho
+#       ==> resolved by putting first joint's d parameter to height, there is no x nor y offset
+# the simulation is controlled via motors
+# we do calculations by reading from the sensors 
+# and we send the results to the motors
+
+# HERE WE ALSO CALCULATE THE MANIPULABILITY JACOBIAN BUT I WON'T RENAME
+    def calcJacobian(self):
+        z_is = [np.array([0,0,1], dtype=np.float32)]
+        p_is = [np.array([0,0,0], dtype=np.float32)]
+        toBase = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32)
+        self.p_e = np.array([0.,0.,0.], dtype=np.float32)
+        
+        for j in range(self.ndof):
+            toBase = toBase @ self.joints[j].HomMat
+            z_is.append(toBase[0:3, 2])
+            p_is.append(toBase[0:3, 3])
+        p_e = p_is[-1]
+        self.p_e = p_is[-1]
+        jac = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1)
+# we doin only revolute joints still 
+# we'll put these into array cos i don't trust myself that much
+        j_os = []
+        j_ps = []
+
+        for j in range(self.ndof):
+            j_p = np.cross(z_is[j], p_e - p_is[j])
+            j_ps.append(j_p)
+            j_p = j_p.reshape(3,1)
+            j_o = z_is[j]
+            j_os.append(z_is[j])
+            j_o = j_o.reshape(3,1)
+            j_i = np.vstack((j_p, j_o))
+            jac = np.hstack((jac, j_i))
+        self.jacobian = jac[0:6,1:]
+#        print("jacobian incoming")
+#        print(self.jacobian)
+        self.jac_tri = self.jacobian[0:3,:]
+
+    # the manipulability elipsoid is J @ J.T
+    # and we want to know its derivative w.r.t. q (all joints)
+    # thus we calculate the derivative of a matrix w.r.t a vector
+    # the result is a 3rd order tensor ('cos you get 1 Jacobian for each q_i)
+    # with size 6 x n x n
+    # the calculations are described in Geometry-aware learning, tracking.., eq. 66-68
+    # just figure out the shape of the tensor and it's all clear after that
+    # formulae straight from GAMLTT, last page, 66-68
+
+#        if self.mode == 'eval':
+        mjac = []
+        mjac_tri = []
+        # this first column is here so that we can stack in the for loop, it's removed later
+        mjac_j = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1)
+        mj_o = 0
+        mj_p = 0
+        # now we take a jth joint by which we will do the derivate
+        for j in range(self.ndof):
+            # and we do this for every ith joint
+            for i in range(self.ndof):
+                if j <= i:
+                    mj_o = np.cross(j_os[j], j_os[i]).reshape(3,1)
+                    mj_p = np.cross(j_os[j], j_ps[i]).reshape(3,1)
+                    mj_i = np.vstack((mj_p, mj_o))
+                    mjac_j = np.hstack((mjac_j, mj_i))
+
+                else:
+                    mj_o = np.array([0.,0.,0.], dtype=np.float32).reshape(3,1)
+                    mj_p = np.cross(j_ps[j], j_os[i]).reshape(3,1)
+                    mj_i = np.vstack((mj_p, mj_o))
+                    mj_i = -1 * mj_i
+                    mjac_j = np.hstack((mjac_j, mj_i))
+
+            # with that we calculated J derivative w.r.t. joint j, so append that and move on
+            mjac_j = mjac_j[0:6,1:]
+#            print("mjac_j", j + 1)
+#            print(mjac_j)
+            mjac_j_tri = mjac_j[0:3,:]
+            mjac.append(mjac_j)
+#            print("unutar calcJac")
+#            print(mjac_j)
+#            print("unutar calcJac")
+            mjac_tri.append(mjac_j_tri)
+            mjac_j = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1)
+        self.mjac = mjac
+        self.mjac_tri = mjac_tri
+
+# now we that we have the manipulability jacobian,
+# we can get the velocity manipulability jacobian
+    
+
+    # implementation of maric formula 12 (rightmostpart)
+    def calcMToEGradient_kM(self):
+        # first let's calculate the manipulability elipsoid
+        M = self.jacobian @ self.jacobian.T
+        M = M[0:3, 0:3]
+        k = np.trace(M)
+#        k = 2
+#        print(k)
+        k_log = np.log(k)
+
+        # this is the derivative of the manipulability jacobian w.r.t. joint angles
+   #     self.calcManipulabilityJacobian()
+
+        # we need M^-1 which might not exist in which case we'll return a 0 
+        # needs to throw an error or something along those lines in production tho
+        try:
+            M_inv = np.linalg.inv(M)
+        except np.linalg.LinAlgError as e:
+            print("ROKNUH U SINGULARITET!!!!!!!!!!!")
+#            M_inv = np.eye(M.shape[1], M.shape[0]) 
+            return np.array([0] * self.ndof)
+
+
+        # now we right-mul M_inv by each partial derivative and do a trace of that
+        resulting_coefs = []
+        for i in range(self.ndof):
+            # we first need to calculate an appropriate element of the
+            # velocity manipulability ellipsoid
+            # J^x_i = mjac[i] @ J.T + J @ mjac[i].T
+            #M_der_by_q_i = self.mjac_tri[i] @ self.jac_tri.T + self.jac_tri @ self.mjac_tri[i].T
+            M_der_by_q_i = self.mjac[i] @ self.jacobian.T + self.jacobian @ self.mjac[i].T
+            M_der_by_q_i = M_der_by_q_i[0:3, 0:3]
+            resulting_coefs.append(-2 * k_log * np.trace(M_der_by_q_i @ M_inv))
+        resulting_coefs = np.array(resulting_coefs)
+#        print(resulting_coefs)
+        return resulting_coefs
+
+       
+    # let's actually strech toward the sphere sigma = kI
+    def calcMToEGradient_kI(self):
+        # first let's calculate the manipulability elipsoid
+        M = self.jacobian @ self.jacobian.T
+        M = M[0:3, 0:3]
+        k = np.trace(M)
+        sigma = k * np.eye(3)
+#        sigma = 10 * k * np.eye(3)
+        sigma_sqrt = scipy.linalg.fractional_matrix_power(sigma, -0.5)
+        Theta = sigma_sqrt @ M @ sigma_sqrt
+
+        log_Theta = scipy.linalg.logm(Theta)
+        Theta_der_wrt_q_i = []
+        # calc the M derivate wrt q_is and same for Theta 
+        for i in range(self.ndof):
+            M_der_by_q_i = self.mjac[i] @ self.jacobian.T + self.jacobian @ self.mjac[i].T
+            M_der_by_q_i = M_der_by_q_i[0:3, 0:3]
+            Theta_der_wrt_q_i.append(sigma_sqrt @ M_der_by_q_i @ sigma_sqrt)
+        # now this is the E
+        E = np.array(Theta_der_wrt_q_i)
+
+        # to compute the derivative we have to use the frechet derivative
+        # on the [[Theta, E], [0, Theta]]
+#        gradMtoE = []
+        resulting_coefs = []
+        for i in range(self.ndof):
+            mat_for_frechet = np.vstack((np.hstack((Theta, E[i])), \
+                np.hstack((np.eye(3) - np.eye(3), Theta))))
+            frechet_der = scipy.linalg.logm(mat_for_frechet)
+            der_theta_q_i = frechet_der[0:3, -3:]
+            resulting_coefs.append(2 * np.trace(der_theta_q_i @ log_Theta.T))
+
+        return np.array(resulting_coefs)
+
+
+    def calcManipMaxGrad(self):
+        M = self.jacobian @ self.jacobian.T
+        M = M[0:3, 0:3]
+        resulting_coefs = []
+        print("pinv")
+        print(np.linalg.pinv(self.jacobian))
+        for i in range(self.ndof):
+            A = self.mjac[i] @ np.linalg.pinv(self.jacobian)
+#            print("A", i)
+#            print(A)
+#            A = A[0:3, 0:3]
+            resulting_coefs.append(-1 * np.sqrt(np.linalg.det(M)) * np.trace(A))
+
+        return np.array(resulting_coefs)
+
+
+
+            
+
+    def drawState(self):
+        toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
+        drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)))
+        for j in range(self.ndof):
+            toBase.append(toBase[-1] @ self.joints[j].HomMat)
+            drawOrientation(self.ax, toBase[-1][0:3,0:3], toBase[-1][0:3,3])
+            drawVector(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), toBase[-2][0:3,3],self.color_link)
+
+
+    def updateJointsAndJacobian(self):
+        thetas = np.array(readJointState(self.sensors, dtype=np.float32))
+# potential clamping for joint rotation limits
+# offset for j2n6s300
+        if self.robot_name == "j2n6s300":
+            q1 = np.array([-0.01497, 1.5708, -1.5708, -1.5708, -3.14159, 0.0], dtype=np.float32)
+            thetas = thetas + q1
+#        if self.robot_name == "base_link":
+#            thetas = thetas + np.array([np.pi, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32) 
+        for i in range(len(thetas)):
+            if self.clamp == 1:
+                self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp)
+            else:
+                self.joints[i].rotate_numerically(thetas[i], self.clamp)
+        self.calcJacobian()
+
+    # only here for plots, hence does not update jacobian
+    def setJoints(self, thetas):
+        for i in range(len(thetas)):
+            if self.clamp == 1:
+                self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp)
+            else:
+                self.joints[i].rotate_numerically(thetas[i], self.clamp)
+
+
+
+    def forwardKinmViaPositions(self, thetas, extra_damping):
+# potential clamping for joint rotation limits
+#   ==> ur10e does not have joint limits, but other robots might
+    # offset for j2n6s300
+        if self.robot_name == "j2n6s300":
+            thetas = thetas + np.array([np.pi,  np.pi,  np.pi,  \
+                    np.pi,  0.0,  1.57079633], dtype=np.float32)
+#        if self.robot_name == "base_link":
+#            thetas = thetas + np.array([np.pi, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 
+        for i in range(len(thetas)):
+            # clamp
+            if self.clamp == 1:
+                if self.sim == 1:
+                    self.motors[i].setPosition(clampTheta(self.joints[i].theta + thetas[i]))
+                else:
+                    self.joints[i].rotate_numerically(clampTheta(self.joints[i].theta + thetas[i] / extra_damping), self.clamp)
+            # no clamp
+            else:
+                if self.sim == 1:
+                    self.motors[i].setPosition(self.joints[i].theta + thetas[i])
+                else:
+                    self.joints[i].rotate_numerically(thetas[i] + self.joints[i].theta / extra_damping, self.clamp)
+
+        if self.sim == 1:
+            self.updateJointsAndJacobian()
+        else:
+            if self.mode == "eval":
+                self.calcJacobian()
+
+
+
+    def forwardKinmNumericsOnlyDebug(self, thetas):
+#        if self.robot_name == "j2n6s300":
+#            thetas = thetas + np.array([np.pi,  np.pi, np.pi, np.pi, 0.0, np.pi]) 
+        for i in range(len(thetas)):
+            # clamp
+            if self.clamp == 1:
+                if self.sim == 1:
+                    self.motors[i].setPosition(clampTheta(thetas[i]))
+#                    self.updateJointsAndJacobian()
+                else:
+                    self.joints[i].rotate_numerically(clampTheta( thetas[i]), self.clamp)
+                    self.calcJacobian()
+            # no clamp
+            else:
+                if self.sim == 1:
+#                    self.motors[i].setPosition(thetas[i])
+                    self.updateJointsAndJacobian()
+                else:
+                    self.joints[i].rotate_numerically(thetas[i] , self.clamp)
+                    self.calcJacobian()
+
+
+    def forwardKinmNumericsOnlyDebug2(self, thetas):
+        print("======================================")
+#        if self.robot_name == "j2n6s300":
+#            thetas = thetas + np.array([np.pi,  np.pi /2,  -np.pi/2,  \
+#                    0.0,  0.0,  1.57079633])
+        for i in range(self.ndof):
+            if self.clamp == 1:
+                self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp)
+            else:
+        #        print("----------- +", thetas[i])
+                self.joints[i].rotate_numerically(thetas[i], self.clamp)
+        #        print("after:", self.joints[i].theta)
+        #        print("")
+        print("")
+        print("")
+        print("")
+        self.calcJacobian()
+#        print(self.jacobian)
+#        print(self.mjac)
+
+
+
+# in webots, i get this for free with a position sensor
+# but since such a device is not available in reality.
+# thus, the gps device in webots shall be used to measure accuracy.
+    def eePositionAfterForwKinm(self, thetas):
+        joints2 = self.joints
+        for i in range(len(thetas)):
+            joints2[i].rotate(joints2[i].theta + thetas[i])
+
+        toBase = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32)
+        
+        for j in range(len(joints2)):
+            toBase = toBase @ joints2[j].HomMat
+        p_e = toBase[0:3,3]
+        return p_e
+        
+    
+    def bruteAnimForwKinm(self, ax, thetas):
+        shots = np.linspace(0,1,20)
+        for shot in shots:
+            for i in range(len(thetas)):
+                self.joints[i].rotate(self.joints[i].theta + thetas[i] * shot)
+            self.drawState(ax, 'bisque')
+        self.calcJacobian()
+        self.drawState(ax, 'b')
+
+    
+    # remake this into the FuncAnimation update function
+    # divide each angle from the current to desired angle in the same number of steps
+    # then for each joint and each of its steps calculate the new position,
+    # and append it in the appropriate "line" list
+# there is no set_data() for 3dim, so we use set_3_properties() for the z axis
+def forwardKinAnim(frame, r, thetas, n_of_frames, ax):
+    thetas_sliced = []
+#    print("frame")
+ #   print(frame)
+    for j in range(len(r.joints)):
+        # i have no idea why but the sum of frame(i)/n_of_frames adds up to 0.5
+        # we need it to add to to 1 so that in total the manipulator rotates by the specified angles
+        thetas_sliced.append(thetas[j] * (2 * frame / n_of_frames))
+    r.forwardKinm(thetas_sliced)
+    toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32)]
+    x_hat_dat =[[0],[0],[0]]
+    y_hat_dat =[[0],[0],[0]]
+    z_hat_dat =[[0],[0],[0]]
+    p_dat = [[0],[0],[0]]
+    for j in range(len(r.joints)):
+        toBase.append(toBase[-1] @ r.joints[j].HomMat)
+        orientation = toBase[-1][0:3,0:3] 
+        x_hat = orientation[0:3,0]  + toBase[-1][0:3,3]
+        y_hat = orientation[0:3,1] + toBase[-1][0:3,3]
+        z_hat = orientation[0:3,2] + toBase[-1][0:3,3]
+        p = (-1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ) + toBase[-2][0:3,3])
+        for i in range(3):
+            x_hat_dat[i].append(x_hat[i])
+            y_hat_dat[i].append(y_hat[i])
+            z_hat_dat[i].append(z_hat[i])
+            p_dat[i].append(p[i])
+#            r.lines[j][0].set_data(x_hat_dat[0], x_hat_dat[1])
+#            r.lines[j][0].set_3d_properties(x_hat_dat[2])
+#            r.lines[j][1].set_data(y_hat_dat[0], y_hat_dat[1])
+#            r.lines[j][1].set_3d_properties(y_hat_dat[2])
+#            r.lines[j][2].set_data(z_hat_dat[0], z_hat_dat[1])
+#            r.lines[j][2].set_3d_properties(z_hat_dat[2])
+            r.lines[j][3].set_data(p_dat[0], p_dat[1])
+            r.lines[j][3].set_3d_properties(p_dat[2])
+#    print(frame / frame_length)
+    if frame == 1:
+        r.drawState(ax, 'r')
+
diff --git a/manipulator_viz/robot_stuff/inv_kinm.py b/manipulator_viz/robot_stuff/inv_kinm.py
new file mode 100644
index 0000000..2906bfd
--- /dev/null
+++ b/manipulator_viz/robot_stuff/inv_kinm.py
@@ -0,0 +1,434 @@
+from robot_stuff.forw_kinm import *
+#from anim_func import *
+import numpy as np
+import matplotlib.pyplot as plt 
+import mpl_toolkits.mplot3d.axes3d as p3
+from matplotlib.animation import FuncAnimation
+import matplotlib.colors as colr
+import sys
+import scipy.optimize
+from qpsolvers import solve_qp
+from qpsolvers import dense_solvers 
+from scipy.linalg import sqrtm
+
+
+
+# hardcoded for all joints
+# of course each joint can have its own limit
+def clampVelocity(del_thet):
+    for indeks in range(len(del_thet)):
+        if del_thet[indeks] > 3.0:
+            del_thet[indeks] = 3.0 
+        
+        if del_thet[indeks] < -3.0:
+            del_thet[indeks] = -3.0 
+    return del_thet
+# r is the Robot_raw
+# t is the target position
+
+
+
+def invKinm_Jac_T(r, t):
+    e = t - r.p_e
+    num = np.dot(e, r.jac_tri @ r.jac_tri.T @ e)
+    den = np.dot(r.jac_tri @ r.jac_tri.T @ e, r.jac_tri @ r.jac_tri.T @ e)
+    alpha = num / den
+    del_thet = alpha * r.jac_tri.T @ e
+
+# clamping for joint rotation limits
+    del_thet = clampVelocity(del_thet)
+
+# if you want a damping constant other than alpha
+#        del_thet = 0.011 * r.jac_tri.T @ e
+
+    return del_thet
+
+
+# using the nullspace for the comfort function
+# when doing manipulability, use the nullspace then go to vec_toward_greater_manip
+def invKinm_PseudoInv(r, t):
+    e = t - r.p_e
+
+    psedo_inv = np.linalg.pinv(r.jac_tri)
+    del_thet = psedo_inv @ e
+# we can add any nulspace vector to del_thet
+# and given the constraints, we should implement some sort of a comfort function
+# the min and max theta for each angle are hardcoded, but that will be changed 
+# they are hardcoded to +/- pi # 3/4 with center at 0
+# thus for all i q_iM - q_im = pi * 6/4
+# the added q_0 must be left multiplyed by (np.eye(n) - np.linalg.pinv(r.jac_tri) @ r.jac_tri)
+# we take into account the current theta (punish more if closer to the limit)
+# the formula is 3.57 in siciliano 
+    theta_for_limits = []
+    for k in range(r.ndof):
+        theta_for_limits.append( (-1/r.ndof) * (r.joints[k].theta / (np.pi * 1.5)))
+    theta_for_limits = np.array(theta_for_limits, dtype=np.float32)
+
+    del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ theta_for_limits
+
+    del_thet = clampVelocity(del_thet)
+
+    return del_thet
+
+# what is this, i don't know
+def invKinm_PseudoInv_half(r, t):
+    e = t - r.p_e
+
+    #psedo_inv = np.linalg.pinv(r.jac_tri)
+    #psedo_inv = r.jac_tri.T @ sqrtm(r.jac_tri @ r.jac_tri.T)
+    psedo_inv = r.jac_tri.T @ np.linalg.inv(sqrtm(r.jac_tri @ r.jac_tri.T))
+    #psedo_inv =  sqrtm(r.jac_tri.T @ r.jac_tri) @ r.jac_tri.T
+    del_thet = psedo_inv @ e
+#    theta_for_limits = []
+#    for k in range(r.ndof):
+#        theta_for_limits.append( (-1/r.ndof) * (r.joints[k].theta / (np.pi * 1.5)))
+#    theta_for_limits = np.array(theta_for_limits, dtype=np.float32)
+#
+#    del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ theta_for_limits
+
+    del_thet = clampVelocity(del_thet)
+
+    return del_thet
+
+
+
+
+# using the nullspace singularity avoidance
+def invKinmSingAvoidance_PseudoInv(r, e):
+#    e = t - r.p_e
+
+    psedo_inv = np.linalg.pinv(r.jac_tri)
+    del_thet = psedo_inv @ e
+# we can add any nulspace vector to del_thet
+# and given the constraints, we should implement some sort of a comfort function
+# the min and max theta for each angle are hardcoded, but that will be changed 
+# they are hardcoded to +/- pi # 3/4 with center at 0
+# thus for all i q_iM - q_im = pi * 6/4
+# the added q_0 must be left multiplyed by (np.eye(n) - np.linalg.pinv(r.jac_tri) @ r.jac_tri)
+# we take into account the current theta (punish more if closer to the limit)
+# the formula is 3.57 in siciliano 
+    gradMtoE = r.calcMToEGradient_kM()
+#    print(gradMtoE)
+
+    del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ gradMtoE
+
+    del_thet = clampVelocity(del_thet)
+
+    return del_thet
+
+
+
+
+def invKinm_dampedSquares(r, t):
+    e = t - r.p_e
+#    e = np.array([0.0,0.0,-1.0], dtype=np.float32)
+#    e = np.array([0.0,1.0,0.0], dtype=np.float32)
+#    e = np.array([1.0,0.0,0.0], dtype=np.float32)
+    lamda = 0.3
+    iden = np.array([[1.,0.,0.], [0.,1.,0.], [0.,0.,1.]], dtype=np.float32)
+
+    del_thet = r.jac_tri.T @ np.linalg.inv(r.jac_tri @ r.jac_tri.T + lamda**2 * iden) @ e
+
+    del_thet = clampVelocity(del_thet)
+
+    # let's try to use the calculation which uses svd
+    # the derivation is in the iksurvey section 6
+    # the final equation is (11) and it calculates all left of e in above del_thet
+
+    # something is wrong here and i have  no idea what
+#    m = 3
+#    n = len(r.jac_tri[0,:])
+#    svdd = np.zeros((n, m))
+#    svd = np.linalg.svd(r.jac_tri) # the output is a list of 3 matrices: U, D and V
+#    # important note: D is not returned as a diagonal matrix, but as the list of diagonal entries
+#    for s in range(m): # 3 is the maximum rank of jacobian
+#        svdd = svdd + (svd[1][s] / (svd[1][s] ** 2 + lamda ** 2)) * svd[2][:,s].reshape(n,1) @ svd[0][:,s].reshape(1, m)
+
+
+#        del_thet = svdd @ e
+#        del_thet = clampVelocity(del_thet)
+
+    return del_thet
+
+
+def invKinmQP(r, t):
+    P = np.eye(r.ndof, dtype="double")
+    q = np.array([0] * r.ndof, dtype="double") # should be q imo
+    #G = np.eye(r.ndof, dtype="double")
+    G = None
+    e = t - r.p_e
+#        e = np.array([0.0, 1.0, 0.0])
+    b = np.array(e, dtype="double")
+    A = np.array(r.jac_tri, dtype="double")
+    #lb = np.array([-3] * r.ndof, dtype="double")
+    lb = None
+    #ub = np.array([3] * r.ndof, dtype="double")
+    ub = None
+    #h = ub
+    h = None
+ 
+ 
+    del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+    #del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+ 
+    return del_thet
+
+
+
+
+
+# qp formulation, solved with scipy
+def invKinmGradDesc(r, t):
+    
+    def getEEPos(thetas, r, t):
+        p_e = r.eePositionAfterForwKinm(thetas)
+        e = t - p_e
+        error = np.sqrt(np.dot(e,e))
+        return error
+    
+    def toOptim(thetas):
+        #return np.sqrt(np.dot(thetas, thetas))
+        return np.dot(thetas, thetas)
+    e = t - r.p_e
+    lb = []
+    ub = []
+
+    def constraint(r, e):
+        # jac_tri @ del_thet must be equal to e
+        # when doing manipulability optimization it will be e + vec_toward_greater_manip
+        return scipy.optimize.LinearConstraint(r.jac_tri, e, e)
+
+
+    for bo in range(len(r.joints)):
+        lb.append(-3.0)
+        ub.append(3.0)
+    bounds = scipy.optimize.Bounds(lb, ub)
+
+    error = np.sqrt(np.dot(e,e))
+    thetas_start = []
+    for th in range(r.ndof):
+        thetas_start.append(r.joints[th].theta)
+    thetas_start = np.array(thetas_start, dtype=np.float32)
+
+    lin_constraint = constraint(r, e)
+    if (r.clamp == 1):
+        res = scipy.optimize.minimize(toOptim, thetas_start, method='SLSQP', constraints=lin_constraint, bounds=bounds)
+    else:
+        res = scipy.optimize.minimize(toOptim, thetas_start, method='SLSQP', constraints=lin_constraint)
+#        res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds)
+#        res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds)
+    # without constraints it returns some crazy big numbres like 10**300 or sth
+    # so something is seriously wrong there
+    del_thet = []
+    for bla in range(len(res.x)):
+        del_thet.append(float(res.x[bla]))
+#            del_thet.append(res.x[bla] - 0.01)
+#        for bla in range(len(res.x)):
+#            del_thet.append(float(res.x[bla]))
+#            del_thet[bla] += 0.01
+#            print(del_thet[bla])
+#        print("del_thet")
+#        print(del_thet)
+
+#        del_thet = np.array(del_thet, dtype=np.float32)
+    del_thet = clampVelocity(del_thet)
+    return del_thet
+
+
+
+###############################################################
+# IK with singularity avoidance via QP
+###############################################################
+
+# qp formulation, solved with scipy
+def invKinmSingAvoidanceWithQP_kM(r, t):
+    
+    def getEEPos(thetas, r, t):
+        p_e = r.eePositionAfterForwKinm(thetas)
+        e = t - p_e
+        error = np.sqrt(np.dot(e,e))
+        return error
+    
+    # E is shere toward which the manipulability elipsoid M should move
+    # the calculation is defined as a method in the robot_raw class
+    def toOptim(thetas, r):
+        # alpha is a coef to select the amount of moving toward E
+#        aplha = 3.03
+        grad_to_E = r.calcMToEGradient_kM()
+#        return np.dot(thetas, thetas) #+ coef_to_E @ thetas
+        return np.dot(thetas, thetas) + np.dot(grad_to_E, thetas)
+    e = t - r.p_e
+    lb = []
+    ub = []
+    def constraint(r, e):
+        # jac_tri @ del_thet must be equal to e
+        # when doing manipulability optimization it will be e + vec_toward_greater_manip
+        return scipy.optimize.LinearConstraint(r.jac_tri, e, e)
+
+
+    for bo in range(r.ndof):
+        lb.append(-3.0)
+        ub.append(3.0)
+    bounds = scipy.optimize.Bounds(lb, ub)
+
+    error = np.sqrt(np.dot(e,e))
+    thetas_start = []
+    for th in range(r.ndof):
+        thetas_start.append(r.joints[th].theta)
+    thetas_start = np.array(thetas_start, dtype=np.float32)
+
+    lin_constraint = constraint(r, e)
+    if (r.clamp == 1):
+        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint, bounds=bounds)
+    else:
+        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint)
+#        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='CG', constraints=lin_constraint)
+#        res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds)
+#        res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds)
+    # without constraints it returns some crazy big numbres like 10**300 or sth
+    # so something is seriously wrong there
+    del_thet = []
+    for bla in range(len(res.x)):
+        del_thet.append(float(res.x[bla]))
+#            del_thet.append(res.x[bla] - 0.01)
+#        for bla in range(len(res.x)):
+#            del_thet.append(float(res.x[bla]))
+#            del_thet[bla] += 0.01
+#            print(del_thet[bla])
+#        print("del_thet")
+#        print(del_thet)
+
+#        del_thet = np.array(del_thet, dtype=np.float32)
+#    print(del_thet)
+    del_thet = clampVelocity(del_thet)
+    return del_thet
+
+
+
+def invKinmSingAvoidanceWithQP_kI(r, t):
+    
+    def getEEPos(thetas, r, t):
+        p_e = r.eePositionAfterForwKinm(thetas)
+        e = t - p_e
+        error = np.sqrt(np.dot(e,e))
+        return error
+    
+    # E is shere toward which the manipulability elipsoid M should move
+    # the calculation is defined as a method in the robot_raw class
+    def toOptim(thetas, r):
+        # alpha is a coef to select the amount of moving toward E
+#        aplha = 3.03
+        grad_to_E = r.calcMToEGradient_kI()
+#        return np.dot(thetas, thetas) #+ coef_to_E @ thetas
+        return np.dot(thetas, thetas) + np.dot(grad_to_E, thetas)
+    e = t - r.p_e
+    lb = []
+    ub = []
+    def constraint(r, e):
+        # jac_tri @ del_thet must be equal to e
+        # when doing manipulability optimization it will be e + vec_toward_greater_manip
+        return scipy.optimize.LinearConstraint(r.jac_tri, e, e)
+
+
+    for bo in range(r.ndof):
+        lb.append(-3.0)
+        ub.append(3.0)
+    bounds = scipy.optimize.Bounds(lb, ub)
+
+    error = np.sqrt(np.dot(e,e))
+    thetas_start = []
+    for th in range(r.ndof):
+        thetas_start.append(r.joints[th].theta)
+    thetas_start = np.array(thetas_start, dtype=np.float32)
+
+    lin_constraint = constraint(r, e)
+    if (r.clamp == 1):
+        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint, bounds=bounds)
+    else:
+        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint)
+#        res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='CG', constraints=lin_constraint)
+#        res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds)
+#        res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds)
+    # without constraints it returns some crazy big numbres like 10**300 or sth
+    # so something is seriously wrong there
+    del_thet = []
+    for bla in range(len(res.x)):
+        del_thet.append(float(res.x[bla]))
+#            del_thet.append(res.x[bla] - 0.01)
+#        for bla in range(len(res.x)):
+#            del_thet.append(float(res.x[bla]))
+#            del_thet[bla] += 0.01
+#            print(del_thet[bla])
+#        print("del_thet")
+#        print(del_thet)
+
+#        del_thet = np.array(del_thet, dtype=np.float32)
+#    print(del_thet)
+    del_thet = clampVelocity(del_thet)
+    return del_thet
+
+
+
+def invKinmQPSingAvoidE_kI(r, t):
+    P = np.eye(r.ndof, dtype="double")
+#    q = 0.5 * np.array(r.calcMToEGradient_kI(), dtype="double")
+    q = np.array(r.calcMToEGradient_kI(), dtype="double")
+#    G = np.eye(r.ndof, dtype="double")
+#    G = []
+    G = None
+    e = t - r.p_e
+    b = np.array(e, dtype="double")
+    A = np.array(r.jac_tri, dtype="double")
+    #lb = np.array([-3] * r.ndof, dtype="double")
+    lb = None
+    #ub = np.array([3] * r.ndof, dtype="double")
+    ub = None
+#    h = ub 
+    h = None
+ 
+#    del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+    del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+ 
+    return del_thet
+
+
+
+def invKinmQPSingAvoidE_kM(r, t):
+    P = np.eye(r.ndof, dtype="double")
+#    q = 0.5 * np.array(r.calcMToEGradient_kM(), dtype="double")
+    q = np.array(r.calcMToEGradient_kM(), dtype="double")
+    #G = np.eye(r.ndof, dtype="double")
+    G = None
+    e = t - r.p_e
+#        e = np.array([0.0, 1.0, 0.0])
+    b = np.array(e, dtype="double")
+    A = np.array(r.jac_tri, dtype="double")
+    #lb = np.array([-3] * r.ndof, dtype="double")
+    #ub = np.array([3] * r.ndof, dtype="double")
+    lb = None
+    ub = None
+    #h = ub
+    h = None
+ 
+ 
+    del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+ 
+    return del_thet
+
+
+
+def invKinmQPSingAvoidManipMax(r, t):
+    P = np.eye(r.ndof, dtype="double")
+    q = np.array(r.calcManipMaxGrad(), dtype="double")
+    G = None
+    e = t - r.p_e
+    b = np.array(e, dtype="double")
+    A = np.array(r.jac_tri, dtype="double")
+    lb = np.array([-3] * r.ndof, dtype="double")
+    ub = np.array([3] * r.ndof, dtype="double")
+    h = None
+ 
+    del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+ 
+    return del_thet
+
diff --git a/manipulator_viz/robot_stuff/joint_as_hom_mat.py b/manipulator_viz/robot_stuff/joint_as_hom_mat.py
new file mode 100644
index 0000000..66233e1
--- /dev/null
+++ b/manipulator_viz/robot_stuff/joint_as_hom_mat.py
@@ -0,0 +1,146 @@
+import numpy as np
+
+"""
+Here we formalize the robot as a series of rigid bodies (links)
+connected with joints. Each joint is endowed with a coordinate frame
+which is derived via the Denavit-Hartenberg parameters. 
+Here all joints are revolute, meaning rotating, and rotations are 
+reprezented as rotation matrices.
+Rotating matrices and translation vectors are combined
+into homogenous matrices and then a frame change amounts
+to matrix multiplication.
+"""
+
+# deriving the homogeneus transformation matrix
+# as a series of transformations between two coordinate frames
+# detailed description: Wikipedia article on Denavit-Hartenberg parameters
+
+# this function is wrong!!!!!!!!!!!!!1
+def DHtoHomMat(d, theta, r, alpha):
+    ct = np.cos(theta)
+    st = np.sin(theta)
+    ca = np.cos(alpha)
+    sa = np.sin(alpha)
+
+    # the d: translation along i^th z-axis until the common normal
+    trans_z_n_prev = np.array([[1,0,0], [0,1,0], [0,0,1]], dtype=np.float32)
+    trans_z_n_prev = np.hstack((trans_z_n_prev, np.array([0,0,d], dtype=np.float32).reshape(3,1)))
+    trans_z_n_prev = np.vstack((trans_z_n_prev, np.array([0,0,0,1], dtype=np.float32)))
+
+    # the theta: rotation about i^th z-axis until i^th x-axis coincides with the common normal
+    # this is the parameter we get to control
+    rot_z_n_prev = np.array([[ct,-1*st,0], [st,ct,0], [0,0,1]], dtype=np.float32)
+    rot_z_n_prev= np.hstack((rot_z_n_prev, np.array([0,0,1], dtype=np.float32).reshape(3,1)))
+    rot_z_n_prev= np.vstack((rot_z_n_prev, np.array([0,0,0,1], dtype=np.float32)))
+    
+    # the r: traslation along the i^th x-axis, now common normal, until the origin of i+1^th frame
+    trans_x_n = np.array([[1,0,0], [0,1,0], [0,0,1]], dtype=np.float32)
+    trans_x_n = np.hstack((trans_x_n, np.array([r,0,0], dtype=np.float32).reshape(3,1)))
+    trans_x_n = np.vstack((trans_x_n, np.array([0,0,0,1], dtype=np.float32)))
+    
+    # the a: rotation about common normal so that z-axis aling 
+    rot_x_n = np.array([[1,0,0], [0,ca,-1*sa], [0,sa,ca]], dtype=np.float32)
+    rot_x_n = np.hstack((rot_x_n, np.array([0,0,0], dtype=np.float32).reshape(3,1)))
+    rot_x_n = np.vstack((rot_x_n, np.array([0,0,0,1], dtype=np.float32)))
+
+    return trans_z_n_prev @ rot_z_n_prev @ trans_x_n @ rot_x_n
+
+# the above, but multiplied into the final form (less calculation needed, hence better)
+# also correct
+def createDHMat(d, theta, r, alpha):
+    ct = np.cos(theta)
+    st = np.sin(theta)
+    ca = np.cos(alpha)
+    sa = np.sin(alpha)
+    DHMat = np.array([ [ct, -1 * st * ca, st * sa,      r * ct], \
+                       [st,      ct * ca, -1 * ct * sa, r * st], \
+                       [0,        sa,        ca,         d], \
+                       [0,        0,       0,           1]  ], dtype=np.float32)
+    return DHMat
+
+
+def createModifiedDHMat(d, theta, r, alpha):
+    ct = np.cos(theta)
+    st = np.sin(theta)
+    ca = np.cos(alpha)
+    sa = np.sin(alpha)
+    MDHMat = np.array( [[ct, -1* st, 0, r], \
+                        [st * ca, ct * ca, -1* sa, -d * sa], \
+                        [st * sa, ct * sa, ca, d * ca], \
+                        [0, 0, 0, 1]], dtype=np.float32)
+    return MDHMat
+
+
+# clamping of all joints to rotate only by 7/8 of the cirle
+# of course this can be changed to other limits as needed
+# however, it is hardcoded here because it's good enough for demonstrative purposes
+# the ur arms all do not have joint limits!
+def clampTheta(theta):
+    if theta > np.pi * 7/4:
+        theta = np.pi * 7/4
+    if theta < -1 * (np.pi * 7/4):
+        theta = -1 * (np.pi * 7/4)
+    return theta
+
+# if clamp is == 0 then don't clamp and if it is 1 then do clamp the angles
+# this class does not reprezent a geometric entity, but rather the coordinate system
+# in which kinematics is defined for a particular joint
+# and that coordinate system is defined via the D-H parameters
+class Joint:
+    def __init__(self, d, theta, r, alpha, clamp):
+        self.clamp = clamp
+# potential clamping for joint rotation limits
+        if clamp == 1:
+            self.theta = clampTheta(theta)
+        else:
+            self.theta = theta
+        self.d = d
+        self.r = r
+        self.alpha = alpha
+        self.ct = np.cos(theta)
+        self.st = np.sin(theta)
+        self.ca = np.cos(alpha)
+        self.sa = np.sin(alpha)
+        self.HomMat = createDHMat(self.d, self.theta, self.r, self.alpha)
+   
+
+# we simply recalculate the homogenous transformation matrix for the given joint
+# and that equates to it being rotated
+# the timely rotation is simply the rotation broken into discete parts
+########################################################################
+# this is fundamentaly different from how the physics simulation works!
+# there rotation equates to setting positions/velocities to individual motors,
+# which then do the moving.
+# there you can only SENSE via SENSORS what the result of the rotation is
+# in this brach we essentially just do numerics!
+    def updateDHMat(self):
+        self.ct = np.cos(self.theta)
+        self.st = np.sin(self.theta)
+        self.HomMat = np.array([ [self.ct, -1 * self.st * self.ca, self.st * self.sa,      self.r * self.ct], \
+                           [self.st,      self.ct * self.ca, -1 * self.ct * self.sa, self.r * self.st], \
+                           [0,        self.sa,        self.ca,         self.d], \
+                           [0,        0,       0,           1]  ], dtype=np.float32)
+
+
+    def rotate_numerically(self,theta, clamp):
+# potential clamping for joint rotation limits
+        if self.clamp == 1:
+            self.theta = clampTheta(theta)
+        else:
+            self.theta = theta % (2 * np.pi)
+        #    self.theta = theta 
+        #self.HomMat = createDHMat(self.d, self.theta, self.r, self.alpha)
+        self.updateDHMat()
+
+    def rotate_with_MDH(self, theta, clamp):
+# potential clamping for joint rotation limits
+        if self.clamp == 1:
+            self.theta = clampTheta(theta)
+        else:
+            self.theta = theta % (2 * np.pi)
+            #self.theta = theta
+        self.HomMat = createModifiedDHMat(self.d, self.theta, self.r, self.alpha)
+
+
+    def __repr__(self):
+        return str(self.HomMat)
diff --git a/manipulator_viz/robot_stuff/utils.py b/manipulator_viz/robot_stuff/utils.py
new file mode 100644
index 0000000..e359695
--- /dev/null
+++ b/manipulator_viz/robot_stuff/utils.py
@@ -0,0 +1,36 @@
+import numpy as np
+
+
+def error_test(p_e, target):
+    e = np.abs(target - p_e)
+    if e[0] < 0.002 and e[1] < 0.002 and e[2] < 0.002:
+        return True
+    else:
+        return False
+
+def goal_distance(achieved_goal, goal):
+    return np.linalg.norm(goal - achieved_goal)
+
+
+def calculateManipulabilityIndex(robot):
+    M = robot.jac_tri @ robot.jac_tri.T
+    return np.sqrt(np.linalg.det(M))
+
+def calculateSmallestManipEigenval(robot):
+    M = robot.jac_tri @ robot.jac_tri.T
+    diagonal_of_svd_of_M = np.linalg.svd(M)[1]
+    return diagonal_of_svd_of_M[diagonal_of_svd_of_M.argmin()]
+
+def calculatePerformanceMetrics(robot):
+    M = robot.jac_tri @ robot.jac_tri.T
+    diagonal_of_svd_of_M = np.linalg.svd(M)[1]
+    singularity = 0 
+    try:
+        M_inv = np.linalg.inv(M)
+    except np.linalg.LinAlgError as e:
+        print("ROKNUH U SINGULARITET!!!!!!!!!!!")
+        singularity = 1
+    return {'manip_index': np.sqrt(np.linalg.det(M)),
+            'smallest_eigenval': diagonal_of_svd_of_M[diagonal_of_svd_of_M.argmin()],
+            'singularity':singularity }
+
diff --git a/manipulator_viz/robot_stuff/webots_api_helper_funs.py b/manipulator_viz/robot_stuff/webots_api_helper_funs.py
new file mode 100644
index 0000000..8299d4b
--- /dev/null
+++ b/manipulator_viz/robot_stuff/webots_api_helper_funs.py
@@ -0,0 +1,191 @@
+#########################################################3
+# for now only for ur10e, later write out for other robots
+#########################################################3
+import numpy as np
+
+
+def getAllMotors(robot):
+    motors = []
+    motors.append(robot.getMotor('shoulder_pan_joint'))
+    motors.append(robot.getMotor('shoulder_lift_joint'))
+    motors.append(robot.getMotor('elbow_joint'))
+    motors.append(robot.getMotor('wrist_1_joint'))
+    motors.append(robot.getMotor('wrist_2_joint'))
+    motors.append(robot.getMotor('wrist_3_joint'))
+    print("imported motors")
+    return motors
+
+
+# for validation purposes, we must plot this circle
+def drawCircle(radius, height, robot):
+    display = robot.getDisplay("display")
+#    display.setColor(0xFFFFF)
+    display.setColor(0xF8FF04)
+    display.setColor(0xF8FF04)
+    display.setColor(0xFF0022)
+    display.setColor(0xFF0022)
+    display.setColor(0x00FFE6)
+    display.setColor(0x00FFE6)
+    display.drawOval(100,100,60,60)
+    display.drawOval(100,100,60,60)
+    display.drawOval(100,100,59,59)
+    display.drawOval(100,100,59,59)
+    print("drew the circle on the screen")
+
+def setMotorSpeeds(motors, speeds):
+    for i in range(len(motors)):
+        motors[i].setVelocity(speeds[i])
+
+
+
+def initializeMotorsForVelocity(motors):
+    speeds = []
+    for motor in motors:
+        motor.setPosition(float('inf'))
+        speeds.append(0)
+
+    
+    setMotorSpeeds(motors, speeds)
+
+
+
+def initializeMotorsForPosition(motors):
+    speeds = []
+    for motor in motors:
+#        motor.setVelocity(float('inf'))
+        speeds.append(0)
+
+    
+    setMotorSpeeds(motors, speeds)
+    print("did motor init")
+
+
+def getAndInitAllSensors(robot):
+    sensors = []
+    sensors.append(robot.getPositionSensor('shoulder_pan_joint_sensor'))
+    sensors.append(robot.getPositionSensor('shoulder_lift_joint_sensor'))
+    sensors.append(robot.getPositionSensor('elbow_joint_sensor'))
+    sensors.append(robot.getPositionSensor('wrist_1_joint_sensor'))
+    sensors.append(robot.getPositionSensor('wrist_2_joint_sensor'))
+    sensors.append(robot.getPositionSensor('wrist_3_joint_sensor'))
+   
+   # now we must specify how often do sensors get read in miliseconds
+   # i've put 10ms since this seems like it should work smoothly,
+   # but ofc you should play with this value later on
+    for sensor in sensors:
+        sensor.enable(10)
+
+    print("imported and inited sensors")
+    return sensors
+
+def readJointState(sensors):
+    joint_positions = []
+    for sensor in sensors:
+        joint_positions.append(sensor.getValue())
+    return joint_positions
+
+
+
+
+
+def getAllMotorsJaco(robot):
+    motors = []
+    motors.append(robot.getMotor('j2n6s300_joint_1'))
+    motors.append(robot.getMotor('j2n6s300_joint_2'))
+    motors.append(robot.getMotor('j2n6s300_joint_3'))
+    motors.append(robot.getMotor('j2n6s300_joint_4'))
+    motors.append(robot.getMotor('j2n6s300_joint_5'))
+    motors.append(robot.getMotor('j2n6s300_joint_6'))
+#    motors.append(robot.getMotor('j2n6s300_joint7'))
+    print("imported motors")
+    return motors
+
+
+def getAndInitAllSensorsJaco(robot):
+    sensors = []
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_1_sensor'))
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_2_sensor'))
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_3_sensor'))
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_4_sensor'))
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_5_sensor'))
+    sensors.append(robot.getPositionSensor('j2n6s300_joint_6_sensor'))
+#    sensors.append(robot.getPositionSensor('j2n6s300_joint7_sensor'))
+   
+   # now we must specify how often do sensors get read in miliseconds
+   # i've put 10ms since this seems like it should work smoothly,
+   # but ofc you should play with this value later on
+    for sensor in sensors:
+        sensor.enable(10)
+
+    print("imported and inited sensors")
+    return sensors
+
+
+
+
+def getAllMotorsKuka(robot):
+    motors = []
+    motors.append(robot.getMotor('joint_a1'))
+    motors.append(robot.getMotor('joint_a2'))
+    motors.append(robot.getMotor('joint_a3'))
+    motors.append(robot.getMotor('joint_a4'))
+    motors.append(robot.getMotor('joint_a5'))
+    motors.append(robot.getMotor('joint_a6'))
+    motors.append(robot.getMotor('joint_a7'))
+    print("imported motors")
+    return motors
+
+
+def getAndInitAllSensorsKuka(robot):
+    sensors = []
+    sensors.append(robot.getPositionSensor('joint_a1_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a2_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a3_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a4_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a5_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a6_sensor'))
+    sensors.append(robot.getPositionSensor('joint_a7_sensor'))
+   
+   # now we must specify how often do sensors get read in miliseconds
+   # i've put 10ms since this seems like it should work smoothly,
+   # but ofc you should play with this value later on
+    for sensor in sensors:
+        sensor.enable(10)
+
+    print("imported and inited sensors")
+    return sensors
+
+
+
+def getAllMotorsJaco7(robot):
+    motors = []
+    motors.append(robot.getMotor('j2s7s300_joint_1'))
+    motors.append(robot.getMotor('j2s7s300_joint_2'))
+    motors.append(robot.getMotor('j2s7s300_joint_3'))
+    motors.append(robot.getMotor('j2s7s300_joint_4'))
+    motors.append(robot.getMotor('j2s7s300_joint_5'))
+    motors.append(robot.getMotor('j2s7s300_joint_6'))
+    motors.append(robot.getMotor('j2s7s300_joint_7'))
+    print("imported motors")
+    return motors
+
+
+def getAndInitAllSensorsJaco7(robot):
+    sensors = []
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_1_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_2_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_3_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_4_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_5_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_6_sensor'))
+    sensors.append(robot.getPositionSensor('j2s7s300_joint_7_sensor'))
+   
+   # now we must specify how often do sensors get read in miliseconds
+   # i've put 10ms since this seems like it should work smoothly,
+   # but ofc you should play with this value later on
+    for sensor in sensors:
+        sensor.enable(10)
+
+    print("imported and inited sensors")
+    return sensors
+
diff --git a/manipulator_viz/run_classic_ik_algs.py b/manipulator_viz/run_classic_ik_algs.py
new file mode 100644
index 0000000..47352be
--- /dev/null
+++ b/manipulator_viz/run_classic_ik_algs.py
@@ -0,0 +1,57 @@
+import numpy as np
+import gym
+import inverse_kinematics_gymified
+import inverse_kinematics_gymified.envs.forw_kinm
+from inverse_kinematics_gymified.envs.inv_kinm import *
+
+#env = gym.make('custom_fetch-v0')
+env = gym.make('inverse_kinematics-v0')
+#env = gym.make('inverse_kinematics-with-manip-rewards-v0')
+#env = gym.make('FetchPickAndPlace-v1')
+obs = env.reset() # need to call first 'cos new version errors
+env.render()
+#obs = env.reset()
+done = False
+print(env.action_space)
+#obs = env._get_obs()
+
+def policy(robot, desired_goal):
+#    del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal)
+#    del_thet = invKinm_Jac_T(robot, desired_goal)
+#    del_thet = invKinm_PseudoInv(robot, desired_goal)
+#    del_thet = invKinm_dampedSquares(robot, desired_goal)
+    del_thet = invKinm_PseudoInv_half(robot, desired_goal)
+#    del_thet = invKinmQP(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidE_kM(robot, desired_goal)
+#    del_thet = invKinmQPSingAvoidManipMax(robot, desired_goal)
+
+    return del_thet
+
+for experiment in range(400):
+    print('experiment #:',  experiment)
+    print(obs['desired_goal'])
+    thetas = []
+    for joint in env.robot.joints:
+        thetas.append(joint.theta)
+    print("POST:", thetas)
+    for step in range(50):
+        env.render()
+        #action = policy(obs['observation'], obs['desired_goal'])
+        action = policy(env.robot, obs['desired_goal'])
+        #obs, reward, done, info = env.step(action)
+        action_fix = list(action)
+        action_fix.append(1.0)
+        action_fix = np.array(action_fix)
+        #print(type(action[0]), action)
+        #print(type(list(action)[0]))
+        #print(list(action) + [1.0])
+        #print(type(action_fix), action_fix)
+        obs, reward, done, info = env.step(action_fix)
+    thetas = []
+    for joint in env.robot.joints:
+        thetas.append(joint.theta)
+    obs = env.reset_test()
+    print("PRE:", thetas)
+
+
diff --git a/simple_raw_comm/.main.cpp.swp b/simple_raw_comm/.main.cpp.swp
new file mode 100644
index 0000000000000000000000000000000000000000..f312dd4c830401aadfa304574c58b8f0a6961b6e
GIT binary patch
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|{g?RZG>fsASln&%ltLUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbM&FyqQvs}<ow)R
z{oKUNJiX+C0*Ilb<Y)+th5+FZC@o3TwcrgkHZn8-DOOfeR1g*l1u;kQXb6mkz-S1J
zhQMeDjE2By2#kinXb6mkzz7L}k^*LidIkmtCa8ZMpfn>I%>w1?Kxro^4O7Pq<@-SC
zTqq5bhjK@$(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7!82|2!X^D28J|N
z28Jd!$oxO7|G$NwfnhU0149-+1A{p~1A_@a1H(r?28OkK3=B<t3=H*r3=F<}3=Cd;
z3=GVC3=I2t85oLq85p#A85lTu85r1j85s8RFfi=lVPGicVPMGOVPHt-VPJ6LVPMea
zVPMeUVPLqy&A@P&n}Oi~Hv_{SZU%;8ZU%-TZUzQNZUzP!ZU%;zTnr3*xfmGsa4|5f
z;9_8y#>K!em5YI)o{NE@j*EdopNoOv9w!6C3Qh)wL{0_<Ax;K{qZ|wjbsP)~wHyo#
zVH^w$svHaq>>LaXZ`c_aUb8bWEM#Y3n99z;kigEspwG_0@PUni;Sd`G!wNPAhGlFF
z3`^M{VPVe3z+lbBz#z}Yz`)GL!0?=vf#EbO1H*b&28MO43=EyD3=DCs3=BT33=Hb5
z3=GPw3=B%F3=E%G7#Pm7FfdGGVPGg?VPLRfVPMc>VPMc=VPMc?VPN>j%)oGsnSo(8
zGXp~}GXp~>GXp~hGXp~zGXsMMGXn!3GXujvCI*HdObiU)nHU(JF)=XgWMW`wU}9h>
zU}9j%Vq##(WMW_lWMW{DXJTM@#K^#KiIIWfB7C4lUtd8%L7^zMq_ikc!O)rk!Ve8{
z_KElLcaL`s3i1!qP;$;M%}G(n%P&zVPR&bEa4Jg7P7O-UNlh$HRRFEBNX$!7(u8Qv
zOjFQMOpnj4j891{&Pd8nEK1R}gQ`K+r=h8-P|cvAps%k0GpQsaRUrvvbTI?i9kpPR
zWY-1cm!}pf<mZv%GO!;1JS0bfMHDhi6!P<^;hMBG(w&o*hSfc3X$%Ys3bhQV5eMQC
zh_>RA`~ro7qWtut#9U<8C@A2IwBVBb0wi~UxG+O>KpIOjQj1a*GK&@R^HLRxO7rqE
z^U_IoU~+z5UTSiQLP@>?Jl%kj6i&B+mBZYHn%?w^Qj1edH1bl*6<lCx&^aeFHLpaY
zC_gE`BtEl1Q`4FOoc5ey_GY9ifYKjW0YV!@k#AylW?njyd{Js*N~J<tei2v<7Nkj)
zR!AO)+N5BsP+XE?WmQ~Kl$n>Vkyw;o7Ht>{wjLDfiACwj3U&&Hkj#LjQUUCe_{;(Y
zTLl+aH^)$)koX{fC;yOm&j6^~0}_jhA+F9WP)ICFFU?KOD=7xWNqS~kY97cf>G4_l
znRzAg#RaLUDaFyIu?n^dx(0fNAQRB!Oz_AV<B>DMBWDPa%gifL0IkE)05L#5)=@~#
zNGwv&0*6K{$nPK-0!ow^7#J9dP{#nt8lVgT!ZtRL2uRIK$pLvABnH7YHn4;o4^6iU
zHZ}@MF$TuQ(FVCOc}ffl5Ut7ir6mX{Lu+HB+yH1cf@-u<Py#6c$6r!@eokUeK}I4Z
z@dOls+zd)vC8-r93YmEd=|!ojc?!k(3gxK^#i^;FLMAD-q$IV7p)5Z$MIj?GFC`~6
z2$X)Hwgi_XmZWNc%td!FDCqR{6;e`55;Jp(^*{w?c&b8PYHEr?qC#?FPEJx{a<)QR
zX<l+kW_}(hcNL|kXBL;F7U?Mjr=}`|7P%B<mZcV{7b_Gb7A5AUmZTOHgAz6aLs@Ea
zNq&)8N_>d|YD%%jCRLDMoC&g`*qQ+x2&H+Mg{7(S1tmo`XnD|10X4T`YJfQkrV{Kz
zYX*ko{Ji24NQ^*olY+COkEc_RV~D4}U%a!2tFw1-sIP*pf>Lr~PG(ZPp^>?Tg@J{M
ziHU`Yk+Gqfv8joXk~J1%JpBSfL*j#6ojn6wLE2L*5<zPUi}j0Ai}Op1l2eQIi%L>b
z<1_OLN=xF4Qj;?aQuRtIO0XK{9~wfOY5AokL>L$B9OM}g67S~e19lq1MX41fsYQ8-
zIk1(FdZk6h$wiq3*j<HQgn+zhXlbNpXl9{jsAp&Z3K)HTg`CX1ROFPPuaKw!Nt+7E
z`FUxX>7_-9plDVot}HG|%~dEaP0mnAELL#J&o3^~gC^Jb`~r}gVo+-I%u`5IC`wJt
z(Jjv}%1Kdx1}!MfmxCJ>NvSXkO7a!bQ%e*|GV+U470MEGN>hs!(u(roRTTq6X)&l6
z&P&WqEiOn*PE{x^O3txnU{KD?OU@}xNma1PP0h_Os<cBA$;>Y<DN0SuwL?}`S`?oQ
zt`FjKGLwoDiz@X?i$F_BQ@|-rF9X>CtZLy^6x19-T1qMlQi};G0yUEKq03m~bMsSD
zK^hqJ^>wKN7!<Vh85Fb_6tonIQi}^xlS?woQWerNb5e`-K!U!B*{KS}rA4XWLZUQJ
zp(G=-7*^as;vuu7SOJnB6jCyaz{Nu)s65LrN=YpOX-mt@O99o0P<vuh5=&A+MXiy6
zk%6v(g|2}a$flUY(vpn)A_WDv)SS!;1=ota)FOr4#M0yphqS6py_8gtc6}|y7?9(r
z=YLRY)YVmR^>b11@pN|e3wBj-^6_`}Rv<Na8T9o*X60n&W|n|6L$N|>9ykOtQWbnM
zlT-7GQ^6%MsDK5P`%p1Zu?h+u1yJZ`re!88<Rs>$mnNpCDx~L^r51r2P6`F7MY);9
z#hLke#R`deDIkl&JwrVFLqin89fN`#{X#rlgBARP6rBD2Ts%PqU$BC|n}VZXq=L7n
zpNoz{Y9^>brjS}uP?TC+tdL)%09qfNnVJH2Qc5PM<V-3pNln3YG^i1WtRO#6Au&%u
z$uU^LGgwK%$uZb7SO?^w(7YUw(M73+rI|&kDGEuI3W)^;Iho0cNja$sIf>;UTN6RG
zTnZ>vWacT97iE@Y=B4W>6z8XvlqVLYLc<W0I5J8~3as??%gf945(^TOGg9^Pi_-OT
zAmLc74~;8bBRvDCrpWwKh1|qSh5V$F#7t1}lAK>qsgR!r4qI4QCzgO*S_xGO>baDH
z+e(?m;54t0T9KSu0IrRb^K%PwG86NXQx(cHOEQp60S6h>MC9;t1iKR!*E$MesYRd|
zQ!vsqP|yIyxDr$os09m2EzbD`l|`B986^s!%%I>F<*DG7UsRl&QJR-tT$P$vQdF9&
z;F*_}UzD3zl9>&1ysoZ-ldHR@A4SO(+;;(IWsuMFixg6G6O*A~0B&f2N>p7fT?J5q
zW2KO+t!=H4nU|89SE5^zm{hC_5zfoZQP9=WWdQa6cQ7(Auz)7;1sE7$^Y_2_85n-@
zGccUsXJ9za&%iL1pMjyApMk-EpMl{y9|OYzJ_d$#J_ZH}J_d&4ybKIeco`Tb^D;1G
z@iH)E@-i^^@iH*@@-i^!@G>xP^D;0z;9+1`!^6N3%frAR$OGx~7jQE$Byck@sBkkd
z{N;l5_cwDfFqCjHFc@($Fg)dCU|7t_z);4?z>vquz>v$yz~I2iz#zrRz;Kg;fngE{
z149Z21A{UL0|O@q0|O%m1H%n=28Q+Q3=D1T3=FaC3=Agh3=G`t3=F&27#MQc7#Pf;
z{rMxT3=ET585ojS85lHJ85n-DFfc4(VPKfV!oX0)!oZ-z!oVQK!oVQJ!oaYWnSo&e
zGXp~mGXp~yGXp~qGXsMOGXsMVGXnz~GbHR5F)=XAhb^KQfN`9`VP!1?BLfISTr&mK
zyoWFuV8auW;bNdRJ#>`90Mv?ycXd7e-263^eDljvLA@Vvx0o1pDfxM+pxmhdZH<C!
zeqB3I7ugxIlSBjDM+4al>uf71fNTYc(AEa<$V<3mkRS5E3#e(7R+L(tp^%bTlBiIS
zn4Ar&Ilv7ha90uH8wG`0h18tlRPfjesCWfA8El}7t5c}Eh7w43d_i`)RZO00v62qh
zG!S3ct|UJg(nke#j@20$7&sUh80NrZfPn!fHXBI{WCbF8kjH8uG7u7d014bFg$+UI
zDR>rxOI@%xKn;qb)ZF|M(8v;aOOHLwXCSlS-hqrvfd@gLV+2L1C7|vQCwiw1JY<y!
z>7c=SGVtz7v_UMW<E8|vs6Zt*#AODE-W+<j5!^2eN=;79EK3c^%uUTNEwQr7&CJQk
zEKW_<Ff=d#g&xSI5Dc>r(gD{2``OA0n#6qbQ&O$0eEnTq<AVcSU0u9%p#A}O^@>tU
z6l_tGC#W7x^#b+io%3^Z6Z29uKoteVCItlrqTB`<;e+Ubxegv2cpT^&6z1;*uK&S$
zNpK=qJ!qgWv=~~)CMTAHdI+GnE-1=RDTOpfp^XTT45&>C)~2toP@b8SqmT<~o`NC>
z)`5U_ycKfui&7PeQgbrFjgS01g}nScT~LRkBr`V^+D$<=EjYCV)D!_HI4t3%0M-Vp
zD<S<rg@V$eg8brC&=6NnrJh1aMrJWIqA&swG{mB9tB{$OTm&wVKxGxkBbW+JVikx}
zXiTg^BVrXA606VvyFySA2=bHy!k-GZ3I=)xMlkOxC=?_X7pJDg7w6=c#}_0Pfs!&j
zP!w!HiVYQNYA}?+41ktmpdm8MpfinCuu;%82IY2$Gf*9(YitclZQxc<PGV7dszO$2
zZUH16C4!O~s9OTgkP5}1k%GK*Xl%k=ot9W!g6V2IbXUWb!CZ~gxpoRjuGQC92nIFE
zQx$UaL5Uq)C?w{kfZDJ*iNz%f;6eyAd<XI!BCty`it@`L;~zz(d7y+EpITIuU!(yh
z<8zDC!BrMW6K<>oE@dH!SfMxr+y@6Wk24Yr3R3eFm0(_hmZG5ULSDK8+y>CNflDT2
zE&$x!Q%_7TDNW3YFH{HRW>D<{nj<KI^cX=SXb`99>w~k60@x3rHYd#L)Uwnf1&HMe
zrI3zAZe}rTupu=C8a1GhRRoz1sjJdcOI#955;c@ywkzqtN2%b28FYpL<N)w2Lt=_T
za%oXfY92I%72ql%t_6n-xIzS53bGPrBXUxM3|~PT3{ZyzK&oR%n*kbhNJC)I(W8>o
zqO`>1R4Xfx+o4+Qz!rjhrC<xTR$m_!kf7MrgYk=#Gg4DZb5c_vJrq#srVCP_P*9Ya
zUzAyb+6zz3gLLCF5=$VR_{?JPP#`oc_4O6fKwY=gykuyvDYIB1CqKVHPXT?f2B|=X
zm{gJh8u^ALo1|2j17Ra}nxGyYxM+ohCAjELO-qA}Hsn+)B&C8p2O9iMPX#6SVnk^S
zGb<=HF-0LKu_U#)L?I}|#TC}nfhJk7kx4oEpkdAYB85bS4A2Zya(+r`3b^bB4b^HC
zr-GbTo{^c8s-U5$fl(Hzslf^!cov2x7I^yt(yk0H0To@j`DLIa3#xv={(>|}z%?M$
zXt*&7whC#9ImM}<Y=o=;xg3NlU{Fv<$u9+UnGppexD++C2IYN}WL6Av2PiZ^^?qqy
zW-@r-0hHVzgUsNv4l*bM4&^jZtq9fxa(HEaDQJWxGY=Ba5G|Q`(B!C)RGOBSTBHLS
z$IAx|9+YQ->S<6_os$C|k}Jq8NCl4!f`%#5a`MZIL6MPKl$w@bq@$3M4>8am)P)AQ
z5nP_Z9SIs$0J|hLMF%wEosn1u9%BO6=#Z3}4;mp%fn>sx3{cd8@+=;VZQK{N-GxK5
zhpVHDk85zS4x|{(%`Z#UD9O(U4Wj1e#uulSlon{h8euq$010~{4H)YvsDV4um?QQe
zgOFkoX<7=FU*PjmItobRygCXfW5^(5AVc5a1O@Ygf~`VHQ7Jf~p^vzS<mcyr5=t=0
zKXwXe9t5W@eSHPL{BoqMnpq5vgi3`HP;7vkw+e}{@jvh=f<i60|KGyJz!1O<ng55a
z@4w2=z_62_fnhE`14AP}14Al51A`|&1A{I<1A`7f0|Prh1H&CY28L673=B*87#J2p
z#{>%a7#O_y7#KwO7#KeAGBE7oWnfst%fPULmw{n9F9SmjF9SmeF9U-lbX<U)7qV91
z3l9UsF&+kn9v%jU8Xg9QG9CtoQXU3|6dnc!a~=kUf7}cV@3<KlUT`xoJm+R$IM2<%
zFc&(00CqdHpE*j6hQMeDjE2By2#kinXb6mkz-S1Jh5$W705TR^G`jk-Jhf<a^(D3?
zv!LacuvLzral;bC63)@pm*DQ}=;}*cD>|X8FR`xnG=Qwkgvf!`QCV3Pr<VBSr~9Oq
TrRHcrL||MiE6`||rZocq9(K%X

literal 0
HcmV?d00001

-- 
GitLab