From 19caf2f31051e1be7e503242326844757262c824 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 27 Nov 2023 11:55:11 +0100
Subject: [PATCH] smack in the middle of creating a GUI class
---
.../__pycache__/make_run.cpython-310.pyc | Bin 0 -> 1506 bytes
.../visualize/arms/another_debug_dh | 9 +
.../visualize/arms/j2n6s300_dh_params | 6 +
.../visualize/arms/j2s7s300_dh_params | 7 +
.../visualize/arms/kinova_jaco_params | 7 +
.../visualize/arms/kuka_lbw_iiwa_dh_params | 7 +
.../visualize/arms/lbr_iiwa_jos_jedan_dh | 7 +
.../visualize/arms/robot_parameters2 | 9 +
.../visualize/arms/testing_dh_parameters | 6 +
.../arms/ur10e_dh_parameters_from_the_ur_site | 6 +
.../ur_simple_control/visualize/arms/ur5e_dh | 6 +
python/ur_simple_control/visualize/main.py | 78 ++
.../ur_simple_control/visualize/make_run.py | 45 ++
.../manipulator_visual_motion_analyzer.py | 709 ++++++++++++++++++
.../robot_stuff/InverseKinematics.py | 226 ++++++
.../InverseKinematics.cpython-310.pyc | Bin 0 -> 4796 bytes
.../InverseKinematics.cpython-311.pyc | Bin 0 -> 7195 bytes
.../__pycache__/drawing.cpython-310.pyc | Bin 0 -> 1443 bytes
.../__pycache__/drawing.cpython-311.pyc | Bin 0 -> 2272 bytes
.../drawing_for_anim.cpython-310.pyc | Bin 0 -> 1239 bytes
.../drawing_for_anim.cpython-311.pyc | Bin 0 -> 2032 bytes
.../__pycache__/follow_curve.cpython-310.pyc | Bin 0 -> 2382 bytes
.../__pycache__/follow_curve.cpython-311.pyc | Bin 0 -> 4189 bytes
.../__pycache__/forw_kinm.cpython-310.pyc | Bin 0 -> 11484 bytes
.../__pycache__/forw_kinm.cpython-311.pyc | Bin 0 -> 27512 bytes
.../__pycache__/inv_kinm.cpython-310.pyc | Bin 0 -> 6584 bytes
.../__pycache__/inv_kinm.cpython-311.pyc | Bin 0 -> 14346 bytes
.../joint_as_hom_mat.cpython-310.pyc | Bin 0 -> 3448 bytes
.../joint_as_hom_mat.cpython-311.pyc | Bin 0 -> 7553 bytes
.../__pycache__/utils.cpython-310.pyc | Bin 0 -> 1481 bytes
.../__pycache__/utils.cpython-311.pyc | Bin 0 -> 2594 bytes
.../webots_api_helper_funs.cpython-310.pyc | Bin 0 -> 4122 bytes
.../webots_api_helper_funs.cpython-311.pyc | Bin 0 -> 11565 bytes
.../visualize/robot_stuff/drawing.py | 36 +
.../visualize/robot_stuff/drawing_for_anim.py | 38 +
.../visualize/robot_stuff/follow_curve.py | 102 +++
.../visualize/robot_stuff/forw_kinm.py | 544 ++++++++++++++
.../visualize/robot_stuff/inv_kinm.py | 434 +++++++++++
.../visualize/robot_stuff/joint_as_hom_mat.py | 146 ++++
.../visualize/robot_stuff/utils.py | 36 +
.../robot_stuff/webots_api_helper_funs.py | 191 +++++
41 files changed, 2655 insertions(+)
create mode 100644 python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/arms/another_debug_dh
create mode 100644 python/ur_simple_control/visualize/arms/j2n6s300_dh_params
create mode 100644 python/ur_simple_control/visualize/arms/j2s7s300_dh_params
create mode 100644 python/ur_simple_control/visualize/arms/kinova_jaco_params
create mode 100644 python/ur_simple_control/visualize/arms/kuka_lbw_iiwa_dh_params
create mode 100644 python/ur_simple_control/visualize/arms/lbr_iiwa_jos_jedan_dh
create mode 100644 python/ur_simple_control/visualize/arms/robot_parameters2
create mode 100644 python/ur_simple_control/visualize/arms/testing_dh_parameters
create mode 100644 python/ur_simple_control/visualize/arms/ur10e_dh_parameters_from_the_ur_site
create mode 100644 python/ur_simple_control/visualize/arms/ur5e_dh
create mode 100644 python/ur_simple_control/visualize/main.py
create mode 100644 python/ur_simple_control/visualize/make_run.py
create mode 100644 python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
create mode 100644 python/ur_simple_control/visualize/robot_stuff/drawing.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/drawing_for_anim.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/follow_curve.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/inv_kinm.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/joint_as_hom_mat.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/utils.py
create mode 100644 python/ur_simple_control/visualize/robot_stuff/webots_api_helper_funs.py
diff --git a/python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cc59155c44e721a1377bb4db4c29fbecfe833313
GIT binary patch
literal 1506
zcmd1j<>g{vU|=XnO-c3VU|@I*;vi!t1_lNP1_p*=3kC*;6owSW9EM!RC`LvQn<<AW
z1VS@IX%;ZeoWmN$mco+4+QJaUp28f=pvm?UWTqzLEfLSWvecsDRPW5Z)ZE09%;aL%
zyfRJ3TZ~$M$t)l>P|VN3zyNZXGsx;Wj0_AV3=0@*m}(fam};0M8M2tum=-eP7E577
z$fq!-Fs3oXRHMi+AymQS7c!<W&*7+LDPgH$YG!m{i0!Xstzm%KTf>;e+RRkTR>Qb}
zt%fayrG|MSGb6}t>?y1%Y%OqgSsX45&5R2fYuRhqYgkg)B^hd1Z5V3UYFJV@plp~=
zIBVDh7;0EjIMKwpV0LD4WpN8I)N+(CEZ`|&$l_hVSHrOoWHx6FXAMgV7epUeh94vo
z!(7W%%U#O@QeVTpkg<j(g&U%#hNp%lg$K&!uHjxFu#iELp_Uh{R}gNW5XhGj3=0`+
z`D*xTSW<YQI$$mqs$ogtgNPN{)UX6IX!85LWM*Jsc*(}Vz@W)-i>a{q7F%I_N`6W4
zE#ch6yv&05)SR67)XenMyt2d`B*Dyr_~NpZ_#%)>mV)@y;#*8*#kY7<GK))!<4f}6
z)AJK^Zt+5l$jnPgO-?ONhoq1X_Q{M87MKJjQ;-vaLFq+^fq@~Np@t!rwU)7lF^gd$
zQz26@!%7BC##^i<8L1_SnoPHt^bBsXX60w*l@#$YFff3LUs?JY`MIh3>G{P4`6-D-
zV4e|zn^T&XqF+#ySdm$*Us@DjoS9pYlNz6#pI1_ppQB$;S(1^Thee_+v$!-dC$lP5
zKQ}QuHNL1cPp_cz7MD#<W^qY!er|!CEXdIy9|$p2NrTh`mFDT$<m4wO<`moMA=DLt
z!ncSW<XmnL!3!cdKm;d<-~kb!Ort4$iz%<*7Hd^%QGW3)wxawbaQNn><fq+YElSKw
zPrbzg4p}aU7lKPt3vRI{78NB{-eLoXN%1X?ti<H}q|C&;TkKhh$?+vcnYS22Zm|>>
z7M0v$%gM}3%t^n+oRV5{i@CTg<rZ6FQF?A>-Yu5A{G!}j%mwkOn*6u8pdpi!T6Bvo
zGdn&tuk03kUVLUrYEkhmZm_H3L6KT<izOwoB=HtwCfIMc*uZgAe2dZd7B?d5D{e8y
z-{MHgOiTwQnOn?7`6agmi*plma#D*+;A!g?4=C-xk`6dlAp|H*++v3YE-0jm%@`OM
zM3_Ywc^LT^c^G*ZIT$$@*?#bF3o-IB$}oXMB$%Wag&27lg&2jvd?`jAMlnVf#v)Tl
zI+um`p}3?pElm%jU@HQ}=q+JnH7P}j<(YZuw?vQyGxN&gvorH@ZwVrcmX>7Z6yIXa
wE6pvaED{05EXY!bU*HM02<$5ko80`A(wtN~P=+W5`HhDG<QWb|4kjKp0KeRu0RR91
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/arms/another_debug_dh b/python/ur_simple_control/visualize/arms/another_debug_dh
new file mode 100644
index 0000000..ab4e0ac
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/j2n6s300_dh_params b/python/ur_simple_control/visualize/arms/j2n6s300_dh_params
new file mode 100644
index 0000000..44eed8e
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/j2s7s300_dh_params b/python/ur_simple_control/visualize/arms/j2s7s300_dh_params
new file mode 100644
index 0000000..7d1b3fa
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/kinova_jaco_params b/python/ur_simple_control/visualize/arms/kinova_jaco_params
new file mode 100644
index 0000000..57b26c9
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/kuka_lbw_iiwa_dh_params b/python/ur_simple_control/visualize/arms/kuka_lbw_iiwa_dh_params
new file mode 100644
index 0000000..3411eae
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/lbr_iiwa_jos_jedan_dh b/python/ur_simple_control/visualize/arms/lbr_iiwa_jos_jedan_dh
new file mode 100644
index 0000000..69b01f6
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/robot_parameters2 b/python/ur_simple_control/visualize/arms/robot_parameters2
new file mode 100644
index 0000000..6487a20
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/testing_dh_parameters b/python/ur_simple_control/visualize/arms/testing_dh_parameters
new file mode 100644
index 0000000..8c60750
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/ur10e_dh_parameters_from_the_ur_site b/python/ur_simple_control/visualize/arms/ur10e_dh_parameters_from_the_ur_site
new file mode 100644
index 0000000..991315d
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/arms/ur5e_dh b/python/ur_simple_control/visualize/arms/ur5e_dh
new file mode 100644
index 0000000..09af722
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/main.py b/python/ur_simple_control/visualize/main.py
new file mode 100644
index 0000000..19b3626
--- /dev/null
+++ b/python/ur_simple_control/visualize/main.py
@@ -0,0 +1,78 @@
+# 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
+import time
+
+from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+
+#rtde_c = RTDEControl("192.168.1.102")
+rtde_c = RTDEControl("127.0.0.1")
+
+#rtde_r = RTDEReceive("192.168.1.102")
+rtde_r = RTDEReceive("127.0.0.1")
+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.2
+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:
+ start = time.time()
+ 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)
+
+ end = time.time()
+ print("time on rendering", end - start)
+ print("fps: ", 1/ (end - start))
+ 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/python/ur_simple_control/visualize/make_run.py b/python/ur_simple_control/visualize/make_run.py
new file mode 100644
index 0000000..240c7fd
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
new file mode 100644
index 0000000..11fb982
--- /dev/null
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -0,0 +1,709 @@
+# 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
+# needed to communicate with the gui in real time
+from multiprocessing import Queue
+# don't want to refactor yet, but obv
+# TODO: refactor, have normal names for things
+# it's just for getting the fps reading while optimizing
+import time as ttime
+
+
+
+# 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)
+
+
+#######################################################################
+# CONTROL STUFF #
+#######################################################################
+
+# TODO: remove this/import it from clik
+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
+
+"""
+GetCommandThread
+-----------------
+- NOTE: NOT USED ATM
+- requires separate thread to accomodate the updating
+ weirdness of tkinter.
+- i know how this works, so i'm running with it,
+ if you want to make it better, be my guest, code it up
+- TODO: make it work for the new application
+--> just send q commands here
+- there are 2 queues because i didn't know what i was doing honestly,
+ but hey, it works and it ain't hurting nobody
+- the point is that you generate an event for the gui, and then get what
+ that event actually was by poping from the queue.
+ and you can shove anything, ex. whole dicts into the queue because python
+"""
+class GetCommandThread(threading.Thread):
+ def __init__(self, queue, localQueue, _check):
+ super(GetCommandThread, self).__init__()
+ self.queue = queue
+ self.localQueue = localQueue
+ self._check = _check
+
+ def run(self):
+ #print("worker thread doing stuff")
+ commands = self.queue.get()
+ # maybe just wait untill it's non-empty with another blocking call?
+ # then .get() from the main thread
+ self.localQueue.put(commands)
+# if random.randint(0, 1) == 0:
+# self.localQueue.put({'set_image': 'smile', 'set_text': "ready to work" })
+# else:
+# self.localQueue.put({'set_image': 'thumb_up', 'set_text': "updated" })
+
+ if self._check.get() == 0:
+ self._check.set(1)
+ # print('registered new callback')
+ else:
+ self._check.set(0)
+ # print('registered new callback')
+
+
+"""
+ManipulatorVisualMotionAnalyzer
+----------------------------------
+- for now leaving run generation here for easier testing
+- later load run and run on that
+- add option to load run while the robot is running
+- some possibly unused stuff will be added here as a weird transplant
+ from an old project, but will be trimmed later
+"""
+# BIG TODO:
+# shove all artists to be updated into a single list called updating_artists
+# and then update all of them in a single for loop.
+# shove all other non-updating artists into a single list.
+# this list is to be updated only if you load a new run.
+# or even skip this and reload everything, who cares.
+# NOTE: better alternative: shove them into dicts, not lists,
+# so that they have names. the for loop functions the same way,
+# but you get to know what you have, which might be useful later.
+class ManipulatorVisualMotionAnalyzer:
+ def __init__(self, root, queue, data=data):
+ # need to put this in the main program,
+ # so you need to pass it here to use it
+ self.root = root
+ self.root.wm_title("Embedding in Tk")
+ # for real-time updates
+ self.queue = queue
+ #######################
+ # visual parameters #
+ #######################
+ self.DPI = 80
+ self.SLIDER_SIZE = 200
+ #DPI = 200
+ screensize = pyautogui.size()
+ self.SCREEN_WIDTH = screensize.width
+ self.SCREEN_HEIGHT = screensize.height
+ #self.SCALING_FACTOR_HEIGHT = 0.8
+ #self.SCALING_FACTOR_WIDTH = 0.5
+ self.SCALING_FACTOR_HEIGHT = 0.3
+ self.SCALING_FACTOR_WIDTH = 0.3
+
+ #####################################################
+ # LAYOUT OF THE GUI #
+ # putting plots into: frames -> notebooks -> tabs #
+ #####################################################
+ self.notebook_left = Notebook(root, height=int(SCREEN_HEIGHT))
+ self.notebook_right = Notebook(root, height=int(SCREEN_HEIGHT))
+ self.frame_manipulator = Frame(notebook_left)
+ self.frame_manip_graphs = Frame(notebook_right)
+ self.frame_imu = Frame(notebook_right)
+ self.frame_ee = Frame(notebook_left)
+ self.tabs_left = notebook_left.add(frame_manipulator, text='manipulator')
+ self.tabs_left = notebook_left.add(frame_ee, text="end-effector")
+ self.tabs_right = notebook_right.add(frame_manip_graphs, text='manipulator-graphs')
+ self.tabs_right = notebook_right.add(frame_imu, text="ee-graphs")
+ self.notebook_left.grid(row=0, column=0, sticky='ew')
+ self.notebook_right.grid(row=0, column=1, sticky='ew')
+
+ ########################
+ # run related things #
+ ########################
+ # TODO: remove from here,
+ # we won't be generating runs here later
+ self.n_iters = 200
+ # the word time is used for timing
+ self.t = np.arange(n_iters)
+ # local kinematics integrator
+ # but this thing handles plotting
+ # only plotting needs to be run, so feel free to
+ # TODO butcher this later on.
+ # NOTE: this thing holds axes and artists.
+ # make this class hold them, because then it can manage
+ # all of them in all figures, this is scitzo af bruv
+ self.ik_env = InverseKinematicsEnv()
+ # TODO: do this depending on the number of runs you'll be loading
+ #self.ik_env.robots.append(Robot_raw(robot_name="no_sim"))
+ self.ik_env.damping = 25
+ # putting it into this class so that python remembers it 'cos reasons, whatever
+ # TODO: load this from run log files later
+ # or just remove it since you're not generating runs from here
+ self.controller1 = getController("")
+ self.controller2 = getController("invKinm_Jac_T")
+ # TODO: load runs, not make them
+ # but deliver the same format.
+ # TODO: ensure you're saving AT LEAST what's required here
+ # NOTE: the jacobian svd is computed offline in these
+ self.ik_env.data.append(makeRun(controller1, ik_env, n_iters, 0))
+ self.ik_env.data.append(makeRun(controller2, ik_env, n_iters, 1))
+
+
+
+ # how to add plots will be documented below on an easier example
+ #######################################################################
+ # UNUSED PLOT #
+ #######################################################################
+ self.fig_ee = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.rec3D_ax = self.fig_ee.add_subplot(projection='3d')
+ self.rec3D_ax.set(xticklabels=[], yticklabels=[], zticklabels=[])
+
+ #######################################################################
+ # MANIPULATOR PLOT #
+ #######################################################################
+ # robot plot
+ self.fig_manipulator = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.ik_env.ax = self.fig_manipulator.add_subplot(projection='3d')
+ # fix array sizes to be the workspace as that won't change. otherwise it's updated as you go,
+ # and that's impossible to look at.
+ self.ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b')
+ self.ik_env.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b')
+ self.ik_env.ax.set_xlim([-1.0,1.0])
+ self.ik_env.ax.set_ylim([-1.0,1.0])
+ # TODO: generate this from a colormap
+ self.link_colors = ['black', 'violet']
+ self.trajectory_plots = []
+ # this way you can have more robots plotted and see the differences between algorithms
+ # but it of course works on a single robot too
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ robot.initDrawing(self.ik_env.ax, self.link_colors[robot_index])
+ # ee point
+ # TODO: blit the point because it moves
+ self.ik_env.p_e_point_plots.append(drawPoint(ik_env.ax, ik_env.data[robot_index]['p_es'][0], 'red', 'o'))
+ # plot ee position trajectory. not blitted because it is fixed
+ # TODO: blit it for real-time operation
+ self.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
+ # only makes sense for clik, but keep it here anyway, it doesn't hurt
+ # TODO: add a button to turn it off
+ self.ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'maroon', '*')
+
+ # the manipulability ellipses
+ # TODO: BLIT THEM
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ radii = 1.0/self.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)):
+ # TODO: add if to compute this in real-time if you're running real-time
+ # although, again, primary purpose is to log stuff for later
+ [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], self.ik_env.data[robot_index]["manip_elip_svd_rots"][0]) + self.ik_env.data[robot_index]['p_es'][0]
+
+ self.ik_env.ellipsoid_surf_plots.append(self.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
+ self.fig_manip_graphs = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ # NOTE: this ax object lives on in some other matplotlib object
+ # so i don't have to save it
+ ax = self.fig_manip_graphs.add_subplot(311)
+ ax.set_title('Manipulability ellipsoid eigenvalues')
+ ax.grid()
+ ax.set_xticks([])
+ self.eigen1_lines = []
+ self.eigen2_lines = []
+ self.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])
+ self.eigen1_lines.append(eigen1_line)
+ eigen2_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,1], color=link_colors[robot_index])
+ self.eigen2_lines.append(eigen2_line)
+ eigen3_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,2], color=link_colors[robot_index])
+ self.eigen3_lines.append(eigen3_line)
+ # TODO: blit this line
+ self.point_in_time_eigen1_line = ax.axvline(x=0, color='red')
+
+ # manipulability index (volume of manipulability ellipsoid)
+ ax = self.fig_manip_graphs.add_subplot(312)
+ ax.set_title('Manipulability index')
+ ax.grid()
+ ax.set_xticks([])
+ self.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])
+ self.manip_index_lines.append(manip_index_line)
+ self.point_in_time_manip_index_line = ax.axvline(x=0, color='red')
+
+ # dist to goal (this could be/should be elsewhere)
+ ax = self.fig_manip_graphs.add_subplot(313)
+ ax.set_title('Distance to goal')
+ ax.grid()
+ ax.set_xlabel('iter')
+ self.dist_to_goal_lines = []
+ for robot_index, robot in enumerate(ik_env.robots):
+ self.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)
+ # TODO: blit this line
+ self.point_in_time_dist_to_goal_line = ax.axvline(x=0, color='red')
+
+
+ #######################################################################
+ # IMU plots #
+ #######################################################################
+ self.fig_imu = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.v_x_lines = []
+ self.v_y_lines = []
+ self.v_z_lines = []
+ self.omega_x_lines = []
+ self.omega_y_lines = []
+ self.omega_z_lines = []
+
+ ax_v_x = self.fig_imu.add_subplot(321)
+ ax_v_x.grid()
+ ax_v_y = self.fig_imu.add_subplot(322)
+ ax_v_y.grid()
+ ax_v_z = self.fig_imu.add_subplot(323)
+ ax_v_z.grid()
+ ax_omega_x = self.fig_imu.add_subplot(324)
+ ax_omega_x.grid()
+ ax_omega_y = self.fig_imu.add_subplot(325)
+ ax_omega_y.grid()
+ ax_omega_z = self.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(self.t, self.ik_env.data[robot_index]["vs"][:,0], color=self.link_colors[robot_index])
+ v_y_line, = ax_v_y.plot(self.t, self.ik_env.data[robot_index]["vs"][:,1], color=self.link_colors[robot_index])
+ v_z_line, = ax_v_z.plot(self.t, self.ik_env.data[robot_index]["vs"][:,2], color=self.link_colors[robot_index])
+ omega_x_line, = ax_omega_x.plot(self.t, self.ik_env.data[robot_index]["vs"][:,3], color=self.link_colors[robot_index])
+ omega_y_line, = ax_omega_y.plot(self.t, self.ik_env.data[robot_index]["vs"][:,4], color=self.link_colors[robot_index])
+ omega_z_line, = ax_omega_z.plot(self.t, self.ik_env.data[robot_index]["vs"][:,5], color=self.link_colors[robot_index])
+ self.v_x_lines.append(v_x_line)
+ self.v_y_lines.append(v_y_line)
+ self.v_z_lines.append(v_z_line)
+ self.omega_x_lines.append(omega_x_line)
+ self.omega_y_lines.append(omega_y_line)
+ self.omega_z_lines.append(omega_z_line)
+
+ # TODO: blit these
+ self.point_in_time_ax_v_x_line = ax_v_x.axvline(x=0, color='red')
+ self.point_in_time_ax_v_y_line = ax_v_y.axvline(x=0, color='red')
+ self.point_in_time_ax_v_z_line = ax_v_z.axvline(x=0, color='red')
+ self.point_in_time_ax_omega_x_line = ax_omega_x.axvline(x=0, color='red')
+ self.point_in_time_ax_omega_y_line = ax_omega_y.axvline(x=0, color='red')
+ self.point_in_time_ax_omega_z_line = ax_omega_z.axvline(x=0, color='red')
+
+
+
+# tkinterize these plots
+canvas_manipulator = FigureCanvasTkAgg(fig_manipulator, master=frame_manipulator)
+canvas_manipulator.draw()
+# NEW
+# TODO maybe you want another background idk
+# worked elsewhere with figure.bbox
+background_manipulator = canvas_manipulator.copy_from_bbox(fig_manipulator.bbox)
+#background_manipulator = canvas_manipulator.copy_from_bbox(canvas_manipulator.bbox)
+
+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(self.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
+###########################################################################
+ start = ttime.time()
+ #fig_manipulator.canvas.restore_region(background_manipulator)
+ canvas_manipulator.restore_region(background_manipulator)
+ 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()
+
+ # NEW AND BROKEN
+ for link in robot.lines:
+ for line in link:
+ ik_env.ax.draw_artist(line)
+ # 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()
+ # NEW AND BROKEN
+# fig_manipulator.canvas.blit(fig_manipulator.bbox)
+# fig_manipulator.canvas.flush_events()
+ canvas_manipulator.blit(fig_manipulator.bbox)
+ canvas_manipulator.flush_events()
+ #canvas_manipulator.draw()
+ # might need to manually update all artists here from ax_something
+ #canvas_manipulator.blit(fig_manipulator.bbox)
+ canvas_manip_graphs.draw()
+ canvas_imu.draw()
+ # TODO update may not be needed as we're going by slider here
+ root.update()
+ end = ttime.time()
+ print("time per update:", end - start)
+ print("fps", 1 / (end - start))
+
+
+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])
+
+if name == "__main__":
+ queue = Queue()
+ root = Tk()
+ gui = ManipulatorVisualMotionAnalyzer(root, queue)
+
+ # have it 'cos from tkinter import *
+ mainloop()
+ # alternative if someone complains
+ #root.mainloop()
+
diff --git a/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py b/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
new file mode 100644
index 0000000..9cd579d
--- /dev/null
+++ b/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
@@ -0,0 +1,226 @@
+# 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
+import matplotlib.pyplot as plt
+
+# ######################################### 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.robots = [self.robot]
+ 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
+ plt.pause(0.1)
+ # put this here for good measure
+ self.robot.drawStateAnim()
+ self.bg = self.fig.canvas.copy_from_bbox(self.fig.bbox)
+ # manual blitting basically
+ # restore region
+ self.fig.canvas.restore_region(self.bg)
+ # updating all the artists (set new properties (position bla))
+ self.robot.drawStateAnim()
+ # now you need to manually draw all the artist (otherwise you update everything)
+ # thank god that's just lines, all in a list.
+ # btw, that should definitely be a dictionary, not a list,
+ # this makes the code fully unreadable (although correct).
+ for link in self.robot.lines:
+ for line in link:
+ self.ax.draw_artist(line)
+ self.fig.canvas.blit(self.fig.bbox)
+ # NOTE: this might not work
+ self.ax.set_title(str(self.n_of_tries_for_point) + 'th iteration toward goal')
+ drawPoint(self.ax, self.goal, 'red', 'o')
+ # if no draw it is kinda faster
+ #self.fig.canvas.draw()
+ # this is even faster
+ #self.fig.canvas.update()
+ 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/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c59cab0f7519bb21e35ad76ebfa41a84b0376333
GIT binary patch
literal 4796
zcmd1j<>g{vU|=XnO-Y>~&%p2)#6iZ)3=9ko3=9m#ZVU_zDGVu$ISe5Xnh{DfL1|_%
z&6L9u#hSv9!j!|7%O1tS$l%V9!kog=!jQt!%oN3$!W7J)$@&sxgC^rGMlHW&5EB{0
zElgryU`S<%VoYI(VoH%nVM<|cVT@u<VM$?aVTfW$VM}3eVTfW);Yi_ZVTfW&;Y#6d
zVTfW+kxb!B;csD#;z$ul5o}?I;!F`r5pH3K;!2T95lInkVT|HV5lazoVTj^MVGL%_
zl)fe6nOBxtRGjLanU|WISdy7s?3!2Rm&}UfDi9mwFlSIm)G#nGlrW?)HZw9Zq%hSm
z#50yKf&AFZSi=y{T*HvXoDO2Kq%+ho#IvTb)-c4grGVJ%DQq<i@f={16D-RGCb_{R
z4_KVHgfEMK0e=enLdFyh5s)r{6wVrkctNnNPzpmZgC>_>6?bZ0nL=_=YGO%hirX!=
zy!`m$%v?>zTU<r?N%<x5d5O8HFF}qjVq{=ocnNa)^E5DcW`Bz{B{i=&Ra5vDYe7+F
zUdb))<c!RmocN;D;?$B`oIzj%iW19jv4YLI#RjH|Z}Dd4WtPMj7RQ76x7bq>a|<%_
z(r<C578T_e#h0Zf-xA1+&rgdl$j{6xDUMIc&r7`}0v0JL%1kYePs=ZY$lYQ|Ni0dc
z#a|Gg3K57e$jL7$z9o{HlapCcoS&HzUtC(01{IYpPRvb>FD^+eD#^@Ck1xnC&Me8y
z&x;44w6t4Hc?GxFiW2iu@^f#oq~|B*+~O`uEl(^;i7%-vNWI0IT98?spOPA1oSa{j
zs>ynbr8qSw?G{&VeoAUid_iJK#w{L@$1)Rh;tLD%i*Iq}#V00%EGWLkl^0)Jnp9kp
zT2Q=_p-72=f#FxZenx(7s(yNYaY24cViA~Ugy80s=B4Nt6eU(<7VDQ5f#RefCpA7f
zKd+=HKS#fyvLquv4~s-uW^rj^PG(iAJ~(XSi%Uw=()2NsrCvehEspqjkaOeX#Xw0>
zfq{X6gOQDqhlz`k4F;Lmm>_H+W;PZsrYczsgIx2<^kC*Bvmm8&HU<U;P+|e6^DG7i
zh8l(}h7`tZrXukY#u|n!rYunUW@%<B7Aj#~z?Q<akTIA6%wh(yz^YlYnTm`{*lQST
zn3|btS<v+9r?A$rh%nT$*02UMq_9D0P^xD4yTw*qkXTflnhtT%hy757ibD}6C>j6%
z|NsBX6b1$cO^zaI1_p*(T#%$!l3H9+#0wH&E{IPpl44+BD3Sni+0%0J6HAPZZt;Q=
zMtn+UaY<rca;hd<kt|4>H!(RQGqo%=B_1RV)_02~GcPUw76(k}EjCDE0>?gtPzGt`
zOU}<NC@o2ihe#WPA_HV47n1-Z7b6!F7b6#=03!zz3p2}q7DkqzRmxZ+0o4?gr~pL^
z2!o?S4H^{-7;C`MAk9$2w2+CBp@xBlA&WVM(VT&aA)XmrI57FW1m$^67D$MQr{x!c
za)WnfUT#=sVgNMv6c-7ALP;1Dm@JUcU@MGI$uEIO++xklOG&MOco>xMi@?6-D$dL;
z$Vm+@NiA>%`55Fr5k?`#DtWBFL#R$>f<!5p1SK+1*#!34G$fyas+U$KNl>C=l4hu3
zOktE{K=LIM#FsS;3s@F1)H0<o*D{x|f|6VoTMa`Ndoxp!ZVF2Y#{$k2)`g6jjI}Iz
z>`*=%gkQt3fUAZng&k}IdkxD%=2}*82y)l3g3acxVX9%RVXk3GV+m%^<nSv3g?o`9
z0|SHGEw0Ss_~O#!<kaF~XtMlZ4`nbg6zPBhk_Q~*MMj{Ig$M5~j`;M{lKA|j;vx-@
zgeHi9C($A;5Z3@i$bgK1rc5?S5p;_=KdJZ@3#cS5(gmr}2NB@-02AOyVF49Tp`hph
zB|rs6K1L2k7Em%|V*Ag<%JN@?nU9f&u}Ttq6qTeFB!kimIA$0a7(fvNiXd?090Aq&
zpt=l_&=^x7QNWl25@Q0_TFmJTwV)))lEv!65X)D~RKvJ{Erq#+J&R)j$3g}Xh7=YF
zhJ}ob425Df3|TA-IBOVFph-82D}^<isVF9et(Lilxj2SsldMrqD&Yq4n;BEsvssFw
zQdmKz6`9sBEZ|wlP|H#SjY^Ow!BLqm-}*Xmhn%DXl)>o`;+Ad<28Zo6d5b{tP$UEj
zXi)Ab5&^Nmg$r92xMI7-mRL}bnwJ7DVu~a|(%=~929*mk$%#41r8$WuslJJMnFXae
ziAk9`nI)B;;Ot}$iegr1b}Pw9ElDiC#R@j{mSAyeUU4QwaeQ({YI1gwCCF%SJc9{v
zWV3>6X;8^r3`$LG%zWTdpM{-=QHqg|k&B6i5d<MTJ`npC2M-Hll@!+42OD;aHKQ~)
zG4JIUMg|6y5{MmCxiBy=fRjQGBLhPfJ1Dn-ssl!baE3e{7KReWN?}F@5CkU)hDe4y
z#&S^pg;W|%B}@yL7lIlwETAaLVg<G0QdoLHjV4f@Wy@lR<kxh@g^Xw{Hjr2fdkRM{
z3#gpS!l#xKS#1f^0<IL!6t;9GP_ipwTELyc1>>b~*MeNavw*jTA&aes1ys7E@YFEG
zGn6nb;7j2J>8N4I;#t7IkO6EfOP0U_!5W4vmMo!#phPbWW(zD3sbR<x#!$_ZCF;Tu
z%L}U1To__GYQcV7AXdW$Qdu~?a57Z4IG7d!iGtj*K%zvlh9OI;nK6Y=9GqE7q(R~}
z3|TTM{3!y>kPra5MHb{Pa5*pN=k_uml;oI<Q$P*Bf};GaRB*!u+~$K8O+V~!F(!dp
z+Q~2hSf%=4|1H*}oW$hpkn~wIL13qSl|)H~LS{*7Q6i}Qqfn9$syGxtb>l7OqSTaI
zjQN^!w|G;E63a95(mnGsOHxyA@i~^16lErrmZZ9Znt`{N3vx<sv883EmlmboVou9U
zzr~%H0&34Cf!cVtm=Y^)v4B|M+;fXHv8X7q@)k=)PG;^cmdc#WTyPP3iyPF&ae-KL
zi?txJv^W)Fad1guNvdOBX6`Mv<ixzP#Nu0g$@vA9@o7c*x$#L!`4zWVz$8;r`Ypbq
z)Z&u-B2XQjo|&I_i!~=RFSYm<H^>9=pw?t@$t{+ooXnD2oW-dn@g<ohIjOgpi%W`%
zKpFoQCrDiYD9b}y20UpwrNtTXsb#5oCB>TTkX+0HYDeB;Eze9T$+*Rqk(!yFQF4nb
zIX@@AC_X1MFZ&h?oJ>WpP1%Z4^HNfadO=0E1p@;E6C<dg=3->}&&ABeB*dh{q{GO^
z2+G6)jBMamg$SbnGZzyND+ePVBh!B&CNU-<Mgi6;X&jjwWI9UW4yuJYU^zMfZ{c3T
zynqGLuuEZC$b^lR&J4~G>@^Iag1(lq2GpBk^#i9qNI?tkaHKukUj(vKlM}sU1yxr?
zpo$Azh~8o^&C5*7FUkdnb`iL!WC8~=)`E}=Tz18Sntr=M0m#F^z`(``Dh;)mIapY5
zHAxV<Pyz;2Ie;*@r~|cXKv`@hqaQfRG#Mez0GGmG0#Xzw=j0cso&;GA3Re~;9EB`c
z8A<~Pq#WML2DRf$7(flXg^V$vf{_svl<ADY3`GT?>VoMOdvR)N3b?10R}c^B>1aYi
z2Gp5JO#wR+LVz4rRKmc(Z~^2jkVimC2Up#nnwo+V3Ly0$3=V|^1_lObjm(h62rkY*
zbvsiHQx-F*;3{M*VaZ~xVJu<EVoPD{WdgSX7_!*GiGjI<WdTzSa}DD{CQxk*sx}}p
zH4IstAQdT0bC`n}G@1Q08E>(sfO=}0%(uAnlZsP|%D|-$xJ7k~CndEwvk2Ni*W>~h
z4n;PgdeIhC^s$2LP;gNKNl4(<6sU!Div?6HfRmCYq<vdt57Gu2%D|SM!0GKOD9}Ll
zJqsfbBOjvxBM&1FBNH<hBL^efUtHZNXyZ<k#m~>rPm{R_6nI50AiKZ>I1YJ1-8N94
zJU;#wSA2YKeoARhYJB`Hp7{8}(!?ByOpyvm6Syg83SwD<2qzGc3nD;eeNiEZ1&Uv=
zdm(WSPLW^&9Q&Yz1uDOaK?#e4fs2_#mP3d`fJ2l^nM03*kBgm$wFsm}lk1i!Qg2fa
z)K7@d&dkfbC4wxRnO6o8l|U9v%g@QlFON?yEh<aBC5S9jT9TPle2X=&G`FA<95J^9
za}!HIC4Nq3l3qb2h;@q%LKpdiyaRF!HlN($uz>`W9VllO^MP6;j2sZi%)=<e1pvtu
ByTJee
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc b/python/ur_simple_control/visualize/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%>Q~>-`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/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c4ec066dd49fb82dc8ca4b1a060b56e912243739
GIT binary patch
literal 1443
zcmd1j<>g{vU|=XnO-T)4Wng#=;vi!t1_lNP1_p-WC<X?G6owSW9EK=HFwK<99L2&2
z5o66|V`5-r$Yqb>0E;l?aOQGFae>*4Io!ECQM`-{Da<J>Eeuh7DXb}MEeuioDeNg6
zEeuftDU87knw&2|w)$x@-r{vD%}aL7%gjwI$;{77W&w#qG03Ux3=9m;AO}rgU|=X=
zSiqRVn8K98ypXwuIfX@<L4=`}C7mIKbrEANYY9^dTMBzKlM6$vVJ%w?Qwp0TLk(jM
zTMAnmlO#h8iwJ`_LoIs^Qwj%EoC8UmqlPJk1uD*hB+gmGki}fXUc*ttS;JbxCdrV(
z*~`MnP$-hZ5X_*-<@X%wRt6~JgZ=YaGeKad{Yy|_YBJwqOuEIGev7fFh>?MT;UzPO
z0I^mw-eOJ8&&e;+WVywZS8$6hu_!SwJ@pnVgkmYk$uH65y2X@Oaf>^@C^I#$1RO%Q
zm`mczZt*3SrN`%F=4Hp{q~?`m++r;*Ni8V8#hjd<bBi(a7GuRN#>!iaRVx{aWEdD2
zewFHH<maa9r{@<J<fkMSfq6y<Zcb@lihe;+Vnt@LerZvBab|8oPHKE|eqKpYevW=Y
zWl2VU9u|qR%;M6-oXo0J{i6J&{F3<MlG3y^{gk4_^31$+y@JYH{2;bJ!tETOh!kL8
zU=U;FVCG|#fI^l(MRE)b3@C9AQpyI4`vkPOuVsP77C6ozG0R%Rl)?&%K`;psgQo&;
zT7ZbL*Dz!;f#R};y@ol39Tc13xa9CF0>w=cC<PX=GB7ZJ<Ew}p!~(gbh=+lJL6f<N
z4<x`3A_PGK97RG53=FqeK#@@-0%CH3V-*}bMPeXzk|06~L`Z`OP>g`1rAQX6927`l
zsmUezMG~O!0i}9ACJ{yu6k{w>L=PE|(?Dv#xx$EnfuV*Wi=l=wg;A0row0^7g$YVC
zgK4H3CQwcRWx`&L7{*$b8kS%NO?E#t&!t0>v6w^p*)t&U#$J>47JqJHQFdxkT4Hi4
z*h?^h)Rc6n02@TbEiMRMoLQB6i#0K)AR|$eu?QSItdQVgEV#u3HX}YWuQ)R$wMYfz
zb=HFX%)AmvSb#$X93Y&a00{ueD1pKQ6d!twER023px_p~#hO=|TTls(hFgNUi6x+X
zl9QRFS5OIJ-C~2%%mq0mx1@6ma^g$!^K-H@ON#Y!K?;mh^b#vli;Yumu|xT{*q{PT
z1;)2TkW5U3l}bgRoQz@_*e}Jm*q}5^a(+$`I1z*63+z-RZz24~VUwGmQks)$2Py%I
XL0N%=frn9uk%y6ok%O5-fQJhJ7UyAa
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..edfb210d0a6e98b4d5e63a27394a09f5931708f4
GIT binary patch
literal 1239
zcmd1j<>g{vU|=XnO-XfTWng#=;vi!t1_lNP1_p-WAO;486owSW9EK=HFwK<99L2&2
z5o66|V`5-r$Yqb>0E;l?aOQGFae>*4Io!ECQM`-{Da<J>Eeuh7DXb}MEeuioDU87k
zn(Qw@w)tr?-r{vD%}aL7%gjwI$;{77W(A2uF*gGP1BmYoa>xt@28I%b1&k?-DNHHM
z3z=(JQdp!JL>OvW(-~4&7cthdl`y5SrLZ?MxiG|<*0R?yr?5#f)G*etr?90lNix*1
ziZF;X)N<4?r*J^UIgrFTYnW45pyDh@;#@UMHSCfM3z%y-YB(1%F)~2;EHzvU85tQ0
zMN$}o88kWlo<rTt0A+lze?Ds_2<)_f2?|V2=39(Ow;0oJF%}guGB7Z_WCjr+mL}^h
zro4h%Y>7pQdFiRQSRoWgacW6?N@7XkEkO{=I3>QID8C@Js3bGBSd;q}Q)0y}?);+6
z)VvaKxZM&&;>71<=A{<jVlIg<yTzASmJZ@)$LFNxm1NvvEiOqdD89v<oS$=xG4mE<
z#Vy9lTZ~mJ8H!{W7#M!l>1X8Urs}8X7Z>EGBo={rMhI?BX<mwcK~Z8wX0d*0QG9V`
zZb43Jd~$wXNl|`|enDkPMt&X^iL%V%(!`w1s#N`={G|Mn_~Me%v^4#cqQvsdy!80A
z{G#~8yv$s^g34RMAVGhGzd#9t0~Gy23=9lntQ^dIj1pidz%0PX^0P>efq?-f&2S>8
znH01%Q_BjAWpG?WQUqHKa|$ac=D{RH44!(xX$T_5QNy%=sfMkF9g+$_DFT!N!12%S
zR|JaBA{GV)h9Xds1;=p_C;=C-F)%P_vJ~-vq<BFDKZp<jiE<VRGB7aQV#&$O%f7`4
zB0*7=S|kDz<x0-a$u9y4WEY8nv`B&oDG(tIB0%v4j<h0Kka9jy$c3dQm*f|L0u7|D
z7-T0OlL#XS@-Xo+7Ab*j6}ZKkSDIT;33k#g!Q8}>f}H%4oXjM>f=UqU78{giF32gl
zC7oN46JL^_pOc+gQmmH?Qed2-mspWnY@Bk79m>DO1{Gi`Fuo;%WFjbD!3neolnGEQ
z1ADLd78{giNzTtH0>>xVl@Ony`H{mWH$SB`C)Ey=Z;JUC7#KJhco>Bkc^G*ZIhc8b
E0PCAMBLDyZ
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..82f14ea95893dce69a57d1c4cc66d153d88e93c2
GIT binary patch
literal 2382
zcmd1j<>g{vU|=XnO-W_sV_<j;;vi!t1_lNP1_p-W0tN<#6ox2<6vh;$9L5j`%><^I
zbC{!8QW#U1b6BHT!8BVG8<=K~;sDc}QJg6(DXc9FQCul(DeNr_QQRpUDV!|~Q9LPJ
zDcmg#QM@TUDZDKVQG6+UDf}%AQT!>)!3>%LRT9bhc_l^pIq@Z_#U%<*PEKl(-b;`x
zH5qR)YWXEIg4j?DVzV+ZFgSx8Ex^FQP{Oc)v4mj(Qw_sH#)XW*44RC7n#{MD@(OOT
zB^D(YXXf2vF3!x;WW2?in3tZDx{{%Yoq>VjSCxK7er~FMdVX<1eoA5ym}i9G=9K28
z=ob_vR%90ImlnkrXXX~<q{c(tr(aN6l98W>MWQUTxHK^*vno};C_gE`B)+($G%Za(
zEk7qGzdSy<w5Tjqub}dlbV+GZo+HRLA^8D0rNzFPd8NexnL&wp>8ZjXe}Q5}fU$^^
zfq@~J8R~Hm1!A+oJg)=xd=_I3Lkgn^C>WSZm=`jNFw}t9ED&}IQ#x}AYYKBOQ!s-j
zi{H!t|Ns9l0tMkp##^i@C6xuKE7^)brru&rEGkN@ECL1XEq<^|;|mgt5_3~aQj2ae
zC+8R6Vo%G-Pb@JuTFHEittc@iv$XgYTSjVTdPd1D!Q|4SqSU++gcfihLkJEA28LVW
z>G_^{j?S4yIjLcpi7u%{nPrJ3nPsVpFz@Ox7K!2Yt`^+8pxB0Z5810VjOolJ3|Xux
zj3CdhWb!NGW?*2@gm|xrkAZ=q2$Wok_(3d?H;V*8EFlm9N)bqol|gZAKz?y%NoIbY
zV^Mx-UP_c2%*9%aMG_zvvw&g-ia~6Uf5E9!59VSfusdPV!wBX<BS@S9+0{kjAlLAM
z+|5!1N(n_Gpul6Uh&Lz_1xcYfR0)qmeKONZQd1(dLCyu00V0e%j4c0I7}@?8Nr9XV
zN@Gx0Gk{V62!ow%1$K4~V+msxlLSLEV=Yq+QwpObLkd$iLy>e1QwlQ@n+3*BVGU+T
zVGCx^WcLFnu=GDGKVI@p3b20(N^UPfwuQK9GT&lOOe(&`SagfIAU;)-`4%fUpWI?B
zxy6_Yi2<+|L1B1{E48R7zX)717=U~M669l4VPs)sVJcDv`2l1!%nzVYgXawiMo__!
z#hA{J!ng=TGJ#2EFv+rrF`cD^A&V)6wU?=uv4#;;X0<Y<u%|IeGSo7``D`idtxRAs
zP{LwN;V5BPzziy8z`2bD#7bcUM<2u_mKx?776FD@))Iy+P>}>y0n?MhiKLRPgkb?|
z4O<FV8q-3?TDBU-8a4rjT6VA~Qwkecl&OZjhDm~<mLr9`ma~Slgkb?&4Mz%>Bts2H
z3Y#RuLM9Q0TCNiI8m?x>5{4`eafTW$35Hs(8m<%`ko&SYL>Ou~MIfTwHQY6<A`G=W
zH9Wx#n!HuB6jJgt^U@U(71A;*Qd1O?GmDaQQWeTGOEMHdMV>+;IISyGKzK>1$%&=K
zsay&Q3JRGD`FS~&3VEriDGK>{sW7dHc_|7d8JWcjnZ*jJiN%#*wTVUPRtm)mx(X#3
zsS3~{LIGZ?C}ie=C7=RDnaK*Ng?gGS&tWlIq{6_!0Ofty59VpIfT<!?P-Xy0gSl`$
zAMC+=1fj{2{;*19o%_Q_5dAQoeGw=dgG&f-YJCYx{Y9W`0?{kYz`&3`YbFTXv@enY
zWk^nNISDC9z$FK`3;`7>x7ad@OA?c_Z?Pv9C6^@T8G#G#TTBI+n!LAoAYltB86mv*
zjQrgA+{BVwY{{v4;NmsNAo-RURAE+Pa(+@~VqQFie@g^H$HT>w5{pxB2^QpM=9R?f
z=fx+(6!Iepfz@z=O6jclveaa7S#pc9_!d)Yagh=z?7+DUOn`FLEiqU@k_;}Ny`XNe
z0A)K+J;TMt#t4E;e_5E>{tB>&Fsd<gFe-8IF>)~SF>^4oFi9{8F^Ms9FtIQenSp{!
z;1+v)d`f<DeEcm@a2|)$g?efEMdk6?nR&UlM399u^U5Hita+um1(o2izQtTzS$vDD
zxHPGtC_g#1xcC-dadKusrCxqPNoH<lRq8EPFdtMM<)!53f`c7gi+~Gea4`oiC%`U3
uDqRr%=CH}lPbtkwwF8wT#h`d*Vc=lmVB}!pVC7&0Lskwx4ptE4;Q#>RzDZ92
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8d2d9b38d7d0ce710da387e407cfcc3cab8e0f3f
GIT binary patch
literal 11484
zcmd1j<>g{vU|=XnO-a>@U|@I*;vi#Y1_lNP1_p*=9|i`76owSW9EK>y6owS09HtNm
zpE-&J%x4AjnR3{mG<ygGCxbgf3Udle3quM^GgA~t3Tp~m3qurV3S%&XCi_c}J${;u
zw-~jOK}=)}x3-Ugfgu%aWfW72QVLTFa|>e>a|%leYYRgZOA1>GdkaGpYYImSXA46V
zTMAbScMC%ldkRkqZwo^dM+#pGKiEQn6u}mTD6SNt6yX+zDDD)I6wwxjD4rCt6!8{@
zDBcu_6v-BbD83Y_6zLX*DE<_g6xkMrD1j8Y6!{j0D8Uqk6vY;XC?T-Jm2Ytd<tOEr
z#1|!&`z13WIh&1vfdLep-k<<m#mK-=B~rtb!YIj5!w}C<!<51VW--<<r7(k8Oerii
z4Drn240&=a3?(d;MvM#~Si)Mv)XbQ|lFd@2Rl=6S+RVtvP{N+V0;2QSQrJLxQrK%4
z;+Y~D@)*k*QaEZD;yJ+LoarFDxM~=(m{QoYnTothxKlV&xSE-2nM>GGxIrd><#@80
ziXuw5Q+VNWd~iAbY^I{L67CcMxSSwdPAHqHsHB8DMHnt8f+E+1LryfCsc1q8cZwLA
zKIR2H3mF#hrid?OtYxWTS-`iDfsvtx#f2eOrIxjZbpdaR#6ree)*6N^t_A!h0wsbq
ztSM}g49$#S77Lsu31>-xSTzh;oXxBY85tQ0=aztVH#62S#0%9hFAxUV01hjW8ioa;
zU>-v-gQm3KEw<eJlKi6LTkOTDdBq?GR}na#^AdAYUotW<FjOs8$V^ix&dgQFELJGV
zS4c`#$Vg1lQK-x>Rmd$ZE>S2*EG|~yQoscu=G#W+WEPji=cmO(__26&C?w{kC_tTq
z&<7QWRmjX!068v2A+adESfRKwuOzXe2;|aRY<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=Wi;?RWqxDNrvG5X9ifM`!u`n<&
z6tRK`Pyum^85A!Z-l>(YMMe2Vx0rKM^KP*g6lLa>++s<s$Sk?VlAE8BdW$tVCo#9+
z7F$++W?o70EtdR()Vy0PMX8A?w^)k{axzP9F&E?)++s^CC`ipqxy9-QQg@3rEhj&*
z<Q7X_N`4y1i2UNzTO3)5$@xi{iFvnpk`r^1y`Vfzwp%R4sX1x4m=Y^)v1ONo($FpD
zw9K4aY>=3~#aOVCp-6{;f#FxVenx(7s(yNYaY24cViA~Ugy80s=B4Nt6eU(<g3=f$
z;T7bh#wX|Jl@#UY=oeI$WaQ`RgA+q8vP4;CacN>sW>u;_B%>9Vl%}QWr{x!w$7g5e
z<?0nw-r|Ul&&<m#iI2Aem8oG23=B++Tu{iv$iv9N$i>XY#Ky?N$n>3!n~jl;iHnJg
zje`lKh6Ah)g$)s5<YVFh>14EF<YD4qVq@fCtP+Bk>3T3LlR@zZE8ck-7#KJh7#N&E
z?kHhoV5k9=C=4}BH4O2L;8K?<g|U{ogt>+xi=~;-g&}rfElUkU7HbWQBtsTk3S$aW
zD^m(%8j~agxI9A@X9kP2VW?*Ti?d^hGl9i9Y8bK@ve-*F7I3CBEM!EdLDg*vdoODY
zV=X5El^md&6s!_dci=OV6RwgQznL`*S&VSCJT(j~3|UMyoHblE+%-JuEa{Bq3``91
zOf{hJf`kRgW(bR0oWY!-mbp*>EXyJds?&H<K&3R7UlFKyD-r_bCUy`Z2qHk0Zipu1
zEsn&z%-qD1)RbF{>9-h@G<l2E85kIDaV6*H<QIW6?=9Ax%)Hd%TdYNidFiP|N+8`*
zAT?|r`MJKJoKujKUvi5nuizGIVo_0I<t@gPB9QU7_!7&~!A8XAq~?`mXz~`Rfi#0G
zC^7=EKq>SVW7aL!iujDgl3T2m5UL796={R*0NEX1af=O1S3>D3C|v-~Y7jyfq?{X+
z@?DA&%QN%RD?rIkfq{X6jg5^Fl-&6kC7@7?O^Z>BQHN22Nsdu~sY(=)kdgE!gMtl~
zmO<$no|csu85l|!7BHqWq%bZ5kxXEc8BDS)VoYaQ$e7Mp%UHrx!;r;X!<fP<$pDJi
zRwf7uN{J;53s_P(7BYg;X$q$ZLo*8_LkU|6DBW^2GlFwNY%Nm_;{r~I{u+iXjv6LO
zhAb`t22d?s0!m9b%&uX`;sL2mVMt+Z1+}1%UBC{q9b{`WD<eY*FUW-{tTl`&TyQr+
zRAA_lU=U$|o6IW7fJF~O79YseLPK!M<Mvz0QUp$Mppb;5LJ(^u3xtg-1rr6SK<Gjd
zg-gBs|NsAgh#{b4y^;}<x<Lv1<-h;`|7-FWnS;`o1t?AMrxYcY`xj-V=9MItWaj5V
z5;$vBYEgc15hw)~nSj)af(TH$E|LeaOhH<O(X=||W#$$cgJk$XGGVF7CHX~QK~QpD
z$qY{4x7bSZof3;vi>yKBfa(II1j-B27hIB9k_yr`1C&Jh7#J8-7!4SO7^N897$X>2
z{#J=25+sUxlq3nN89^9aydGcxB}q_`+00bSSi`u0aUsJ3rWEFdjNnAX3~IEMFr+Yo
zdKI;dHB1XwKq4uuwag`~C2XL^GdRf^*Rs?wfr{S+Oet&&89{6tP^7bi#6SrUM5nOZ
zFqE*SaDa74*0R<xmvDkqN;1^2f|`<`f~bZyg)^JE$iId;g$=|nw5nlFVTP~@6>At5
zaMdt_l5q{=0<hb_35Lt>mH?z$1J|bDHWj!eDdGo(Ib-=Ph9Ut_pn_7)OK=+j6#bgq
zMbaQq84w{0B9uXd9Ed=T+FPt88L1_Sx0s7dif*x%7iE^DYO;YNtw<B3S_@<iUs7Un
zc6>>Gd~r!pW?uR&#!Rd+$yc0Mmg=0Jmll%mmYI{f2o#l|vXP6C4_r}lFtRWTFbXk>
zF!C^pfJL~Nn0|Ay@i10NAfgx55R~{8VPIeYc@!MKY|Nne%>orRjEfjcK%Et)UM6rM
z!vGd#MiFHyfrx?%BZd@KaKXd|CfPy76HI?CGgKW1LN}<ZR06Js!SSn5%L1-a*g!F#
z#V){5%Ur{_fTM;P)JbY(O5p|-C*YW5LK5YIi56PbFs1NFf+}z@4K^L(HeQ6C3phdD
z&V@|1Z0QWO?CA`(97ry(sAVZ(Sin`o2y#IUD<q{!Ff3%M<*eaYz)`~qww-eUX9`ye
zKUfs3vX-lceE|nV7gz-sTm@GN!vbzl#hS*nkg=A#gkb?s4O<O2n8#LFUBi~bnZnn~
zl)|6F&<bug)G)+@3QAt6pBa$E`O+C`dD0nbd7&YR%~yOWoVENZoV5Z-zM4|Yg2YRx
z6|7;Y5zJ<sz*r;?_ANNa)v(tHN-`{Dg1ej_Nsa?eP5|sHff|0WKZL-(;;Rt?^Y{wi
zLfwZH(`cr0fWsHvMl@4Xcxr@17-}(0ui--vvsyl|7*{J(3Ou}Og==^gaMbXD<AZkr
zM~yIu1$7zLkSlyv!&Ac$&tJn^!w@e3E+qu~G?^h~KfFFz$#{!3rKGYT6-5Rt?E=cz
zt{}n<M7T3BFlaJAhgIuEaUel30agtvPKrR)NRbCfAsdoCV0i=q%9$@g#V@$7(iB9h
zaM-~e3S*;N%mwkOkje#AFcg6+7f`(du3>Jm7o`?wBo=_WEk*go#kbhXic1ocvv09w
zfaxL?P+`QLm6#k~Qj~d%B{wTE`4$I=1o1URz{S=rma6#7;#({Q@tMU%0U*DCE1g7;
zR%Va}maO>v;#(|P@dd@Vn6u&wKtw)>$Sg_+DbE0@WCNR;1+pVPA50b$MS&z(a<k$y
zZ*f7SLDoWQGH{&+YHt;(GB7ag2bFvY3=9lnjAD!$jAD#@OahEzj1u57Pll0&nT3&$
zNrI6J-1XpN<YDAv1j{qC@USq-FtIR#<oTGm7-bk`z;ZIoAew_wfKh~5gqe$xi$#u+
zjggD7N(@nMBKOKrN=;7afD*XWWB_$K7_vY$7(*7L00XFI1l5tCZUv}w0V<_H&9nu~
zHH@H|vV>^?OAV+@V6NgQVaj4%zy=m)31`StWMKe_WU*JeFf!D#GBQ*NmvE#ogNAZS
zK>3p~9n_TL5@A@#7|y`P5Xq3jP|I4!8Vse&8Os^c8EV<U%{DG@$*NNeYV7gUu!9D+
zvcT>Gbq~NzEOtpSU!0+qqlN>+#T+$k3wTplYM4bBN|+Wf*KpLZfEtA$yBQe@eL(K1
zVOz*p%T~h{%%I7N)B&hc2=e##3-wS4RS5R<a}V`#4Dt+d4N*jZA}>(F=Yym^#t?9_
zzQtNnl$e})i#aDh9o+Ix%t^n+oS9d4i`yqN&oL(*+$}E>10^U>lMB*E09U=>L<VY5
zX>vjmETiu&#_U_H+3_H=SbgI`rZA@7;x9@qF3kZCsU+v8rWHfdACGT*N@`JjQe}K$
zd?s4jyCs~Qn3L=qlJDwXl$Zi)-Nk473Ntb=6oWDjACm~E<I2p$4yuDW80DBa8kqjF
zu=6phu!=CsF>*1oFjdK-rYAJBP?8m>0RrmNfRj}iiOH&l36!u>m=-dMFn}6dS!@f~
zLCv9sOrR0G8b$#IPz2Vpmat~AE#RnO1w|dGA}j&*K_Q7ny%yAS<*wntNGxD6P~sM6
zsO7BT#7HchHS7y`YFKJGA+~@D+(PpbrUkq;><bxdxzZVGxsmL#s^utQTEJJrw1B^c
zwT261AE*KW@nL>|(Gm<btROiMuH}Kcoo69qEpH7k)D<kSin5lkhI;`|3M)9d^3||`
z+zSc~uxqlxuHjw?8scU{O01x?!@$7sVE@a%pyocPSk@E(C(NQUP*MXo@ykKw6mx22
z>Mhpd<jjJ~qDqi}cv?|nGN`eim=m9ySW=W(5nqsBo?3K^B_}^U7pY7J*#Rz&i@*st
z1!Qh2C<pL^lWS28NT3#EBx`YIdT!z^E-)2eTv$|ci!}t?n&iyMPmhN%1;G?J(UupL
zfKx4~AT4SFnZW2;)Ci*aa}!JA)AEbr(~44)Gg3=#aYGp(eYbc)DIVNR1#1VFwO|5N
z;9^Vko|d3Q4^je2^n8qbOgxM{OrT;_g;9i2gprGhg_Vy{gi(f(?LQ9_h~^@Y_&re)
zKWHEhRIY*(Kd8?FEvLX`6!tPTouQVggb7wYmM~|rEC3BEGc05Tmzbbo8+LFhs8|ac
z&futFmSjLyCBOjci7enuVJTr=zy+4!29?F2L{q{HO3)w{IQgiSFfZT*mD*@!-7S`a
z%)By4MGh&?Ajt+?Oo7u5D5(@p2E{PAtO8fk@C0#-1suoBDXAqzphN~vAeyX5$pRFa
zkaCLA5xsB{02fY)d6@;ii4~v%DjF0?pymZAB3KwX7=;)?;}IGxTufC`Xyp)+Hk5ck
z8HD%-?(tD)5TcoxkpVpD&jyYYUvQt19c|!&6I91AGT<B3farh>a-@KV5x^#chN+mr
zgAANR4KgsMpsQoZ;sy<n6l#N8fLwk>SekX<n1(diK;c~E35r;7ZN>wx#lR6*1e$09
zj{p>brst4K0#GG|XrkTX0(HD09el`Gl|RThP%eW+H8?kNf)s&!^Le1?h4kb#82K2b
z7>yWxKs|Y3MAX97qQohv7Yk}eg7elaXq-aAxP&2#C55pU)E)t~Em%{SvYCqHpsi&{
ze-A7Vime(ZafVt(@Bk7cbofXKlCx4+vYCpkK+`**Jhp(Vgu8|j#A*ha$+3`$k)cop
zAq$ZM4K~4KK+{c(4B&8Q^(%@2g#u_`SCcJWt0XRY`ij2&>C0bu=>1pxY5yF`*k1(l
zNP1qI^oO5{oChF{Y*2$)lNS*axA;H<^WY(Pa0o(%`$6r<B2aihVg@`YBMGt-)Ofxn
zRFn@II*HFK%}p)JOis+nsl3Gn9`py7yWr-iCJQ)=-eLonp~bhD3Jt-T6cTOV2n5HS
zcxgdOVo5640mY7aDX=mAa!^cy5|9`p7c;2iF2KyiD90$p#KTynh=@%bx=|)KLHQNr
z7jWEy2I5N?N*HSp@dFwjWJ+PoW-5{a4a0$k>=v+;u+}hwSm5|!0>=WVv}6W}EntDj
zfhHs&GN5=_$>ay=gs@=?w4ynn;G4_9zyKYeDw+op09Q-k&;S$QAmJ=dErEnb2PhCg
z?HvI|i04`UR3Rm0xH^>aeo#_|kC7Tc+el#VN!BoyFlI58FlI3$W6<1mI$H^27E20C
zFC+nj2BTmBlmJe^>?y3-OhuufBnf8OArdRNG0g)?%vrpUge%U_%m}U&<iX8oW=Vzx
ze3+_gn4}p%Dr*?B*ulXFQq2K2JsV*<%mjql=q5w#L{bMf7mKOgH4F>*!2@m#!Jy8F
zKnhznQ_-{<h6RE!QBXu`vilW*@)kJ5q~Fg~2>q@2(;kxNK-s4VR0(SeLGlN<2D1RQ
zCzuK{i@^ED1QfA|EF%o!f+FP>cX4V-Kz?y1s6|u+N(zvI4wNJz$zuUXsRW1s_2{5k
z2%J!E@upUk6eY%|B<2=m=A~oLN}&0t#G(}M%)H#N%tV+?#S=lP2sGr+#UjMW!N~NV
zg^}aG01For7o!@Z9upU{5~CIq7ZVR72($c0&R#f7#GSoBi5ew)xz!@Fmk}s?fm0$V
zrdhyQ37pbEENF@WWioaUe*s4c8zh?{vS1Cv0!~m&6@pYTgH5ujVOYQg7lZ1A=!0cX
zgf39*7A*v|E4k70C_I*mmV&|`5j)^~3yO{+aO{8v7}3ix8KhwHgOrKI{&_i-E~!bS
z=^H?y4{GcPFoQx|ictpCmuBW-l3?OuWcg5~hA7PN8G}2>L2YBum@>FU2dYh&z^w-*
zaKJNzYb&h54T=nKwgYu<ppgL&IA(}U2?Mn4h(^~iEMNx(Iw(LjS^TQhY>5Kk^06oc
z6agHNekwSxfLna<>IIS&z!3lrc5ERoi!Z2*j)8(4RCjPN^09ymPG$~f(7Y)RW0g8_
z!EJ;R-k@9$uR$g-Fn|gVP|F%LwFE0B1!|dVm_UW#0w|pYswJ2u8N|U8MTMN8t{LvB
zsuI>L_7v7$rdpN~kf}`2)&plPYYj^c6KF&~iv!deFEptEFCJ=Tg0$?QqYE6^+hX8$
z0(i6x6danIkOBuB=%51X78|4{F9H`q@S+S-jDd#8G}*zmIHU+-hfL@jK?)z#VJ`G6
zA)cBFD}Nl*N>Yp5@{7tr<<fmn)PN=r*cb&E<rp=X1egRE1sGXSDs>!sQEE9*bbz7-
zT&Wj98*HHdZVK~4=33?&W*3H7<yw{!rV?iOw2BCL{+b08nBZ;@n3cr_3EmnO5m;0S
zf|7R)V+so-6Vxy)-~c6ds3<E)6rAv3y=0I(UV`QcZm}h078jPL78!z)I48K3b&De>
zGq1QHF&R1r2?}>eG^_#@G~ke*31Wc~J8T*RJU0s}OK-6jXXJwxQGh5&z=InKU;^A)
z5KJm6g-m0?!uSm+j6scH&<uzGqZ)W7M1ZkM8j<ud^k_2rX$phZEPz(!#K+&_ijU9D
zPbtkwjgP;@6CYn#nwSHTDbfSE6%^`4fgn~ihyXdgs2RkX1R|z`2(X{PMdo4<cR7ey
z0U}m{h&3Q$6Nmu&1VMoOR0L|Vf$G#^P|3i-z{SL2$DzQ%#UaZP!lA~Y#39VV$sx$W
z$sx}n#v#eW3s#>D>K?-eltHN-6e}Qi7CSL9fCl|oA@x!gENk)CvemGHGb@`ULkeRJ
zXkL|BngKd|$63R;fCV%L1nQDyv86MlpwBKtCUjW98J7(_3B;blp3Vrv>CCkpFgA!z
zXRhT0(J-9OT+0PxgXnZ-&=d%$VG4KQ_FA?Y_65u}>?xq}CdMqL8a7D=P%!{%0YOAl
z*i$$`MFEHe52Nzb@W8}Cd=yojAQ5qfT3)ys@B|T7HGFV2;FTX()$pfq)Uc;;OEN%h
z1e=ek5<Fv#p-!BkR-l9-g%ey^Io1l+fG1KGFxT*cCsY_28EQB|tOd+9d>|U^XD$$H
z0doyMhz5&rgIEigYXm?vScEZ)6BJIM&g%lM8txR(z%DmvULBNgYmjBRYq&vc96`DY
z&lg73Fr;v0Gl8alY8XL78!0>>9iW^J!kWB(kSPgleO^R43!1QiOoM~cA9xrSJa?+e
zi`3!(^`gMTUAMTADl%|!0qGut+mWCGxClJW%~70M5}%S-l6XrH#4=8aFDS|{NG&SK
zOf4<~Wi(BpTdZlI^>1LiAjLIz9%!W=ShN`2)xO0G>5Ih|=VT_Qra-1sLB;qjPVmAH
zkPWvu!HYj2Oz;8_Fq5?aOmXKU&EMOAa<46jumcgGbs|^`C_aQ2L6hyjL8X%!0|NsK
zqYyKwEu_T=ssTV4H1#gQr~-x#j694&P$<VP#mMx9i%)=&<$uvZP|?bDi#4w_x1bUn
zWVe(cv(?~5SbF8DN%<wk@rebQ@foQ(1*t{xX{C9^w|KzAl6pCrd5JmcMQxx4y9}~6
zaNQQ4SR9{`p9>nRyd{h*3mP;4O+uq+feM1h5JAg#z{L>QeMq$-xMTwrYPUFSASJ3D
ds6H+RwemR_I9NFtAx#q|CO$2OdJyDs0|3ugQNaKJ
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..019ec0fc3322ff2667d95fd3933437d829cd2882
GIT binary patch
literal 6584
zcmd1j<>g{vU|=XnO-c2!WMFs<;vi!t1_lNP1_p-WXABGsDGVu$ISe5nnkk1diYbL5
zg(-(QmnDjo5hBKx%g)5W$dJns#R(Q+&f&`Cj^YNhnR0k?d87ECZ2l;Iux^1U!4!rR
zmK>p6;V5CS9BYnHu1J&!n9Y_WnkyD1#>kMup2E?>5G9_%nZnh=5G9eqox;<?5G9$y
zo5I(^5G9qupCZu05G9==m?G4|5G9i$oFdY~5G9);nj+T15G9u)o+8o05G9`?nIhG~
z5T%eJog&l15T%$Rn<Cf35T%qNpQ6yh5T%^L6wIKh_!1NXnvAy?wfr<0Z}Ga7<|RAk
zW#%T9Waj5-GT!1S&d(`JjV~+!Nv5Rc6{p67MT&|w8E>%`7Z#P|CNqMJfMQS>vM?|(
zID^8`j)8%pgrS77hM}3UnbCzIR;-q>hM|U0k|BjLo25u71<bQyfQm6?GZe|BFoAeB
z425hp48aVV%zn=yCUZEz7zZ?&Zm||6=B20JV$Mm;(`35Ek&>DdUy_kpa*Hi9FC{g*
zcqKy-F9QR^uQL6N{M=Oi^!(z2{FKBZFwY3V%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=
z&nqd)&(SZaEXl~v!y-|ZSzMZ!lUbFjUzDGeUlLzjQks^gpP5${pPiYPt5;BYi#ItZ
zF}ENrH77qgv!qf86c&683=9H{Jd7NSJWL#nObslIMf?m549TDXgZPkvft`VYffW{b
z1q=)fHH<Y3Squ^kwM-?93z%w{Y8bMZK{$&=fT4y-fMFq1Ei+gKO&p{Kgpt&-)G*hu
zNHf&3*04fkM8GE0vX!vbur)K*vemE!GpuCv(`38FTo9jniz%<*7IR8|$u0J*#N_yr
zqRd;2Az&A4vfpAXy2V&>i!t>Ub6#ogE#{Qeyj!e^IRzPsMcfPwke~;Le-X&jw|F!2
z%Dgl4a^t-cljB3=VS&NLD8;D3$iXPU$id9QS|o!lFhBtUPDxYH0warQ0Vp6?7Bbc{
z*D!;EqLw9{p_a7-<T*BQS`)2ht6^Qh4oYKLY|;!h3|SmCY?2IFoF$A|Tq#T<4AKlD
z3=0_<84ATq7_+!*SW6hQcv6^qnQB>USV|Zd@YXP7u`Oh*0ox<MAi+?>D!^dQP|H%n
zSHsfGSj$qw63n2<;`j3Z|NsBfAyN3lekJ2A)|8UUf>cfZB0*4KfFl1ETTW(PVov%k
zmV(T@vLX?XI41)G!!4G)l>D??Y>5R0sd*{4*s}67^Gb?uv4WCv;w`3v%v-F9MMa5~
zx7gEi@)JvpjczfgR;EH@p-2p5j5vsp0GY*EP@I~Q9}lts6brWmz^25f<rl^0Waeg;
z6yIXZhQtaeYe1q#5EeB7#i^w!`JQ=YhM?#HIgpEykBNhkg_Dm_gHeD{j){Ybg{epv
zJ(57Fl?|3+L1iZ*#nv!nF=jE9FlVtWU@c(-@jy6>Nr0i5aUr7s10v!|*bxC*B*4JH
zpveIVJCHkyL_t0Tg<_Ec$eYX{4}z?O1OqEtFcfKll!F2Q-LGO8evQva%t`YCc^FhL
za4{;ea4>OjurL+rWAi4wRPz9rYFVHR$&kgokg*n&F(DpeSirK7p_ZkFxrAW>I3+G*
zgk)N9`e6a5ALbJF8s=tjx(#N4c^B@#B1Mq<K_0&)oSc}G>>HBr>RyzXl9`%U5})l`
zqzRH%0ui9nLlf$KPy#^s{1!)gQDTa3Nxmz37|21xAUHEG-LWh`GbJ%EIW-<0`H7&g
z;9+23;A7@s<lvNJ21mLPdT4+`AB4d<GYchWrZ9pEq%5X%h7_hnjOh$1%wUpb5o0=Y
z31b#>3TrP@ElUkU7E2Z=zp*V~2eCmoi&cQ3hB<{zn4yM6gh8AEltjS!nze+ZhP9ay
zRB#0|XtMjI8-u}Rdrf9&jqt%9%3xq9G65wYW-wJ`3<@S>0T3G^D#yUU@Dfz?YH~u7
z8aM*fK^inb1SkR^DG!wBiu6EY-~eYULd$lnIf=O`iMLoXQ&RJ=7Js79Fpp13%q>Vw
z2`(&6EJ`h|f(11fBMXZXqaG-K2^U$Q2Q?@%K#3U~)HTqASHqCSl){+8)XP-MRKk$O
zoWh(AswbF47@(reDWF6M5@V476^fwRffXvp2GUW(kOkwjr!drVq%hQSrZCiUm9W(?
zf$E7Gt{V0lwi=Ea&J>mu)?Ut9?i%i322D1<TWl%$rAaxdMW6sI0#*FCSW=Voi&rw<
zVuQ2`G}(*5VTnkn;7|pZh2T&H<)f86h+w?M7;uZR@D`(c5hxYkVobWl=y;1MC+QYb
zY0@pm46I?w2@TW0fLWk$1VxDeqZlI#6R6Y@U}RweK`CYy<{~%r5Cu5|UacrFGJwOZ
zg`t)yg*k<#1ym}5!VOgCf-97ChFZ21))clDhFbO##u9K13~kKt)N<6YE?`Mv2UQ|9
zYztUYI6$mInG)74P{L-dVOz*l%UQy)0ED?}SZY`nGA(4R<w|F$<pwp1vbez&6?ZL9
z4fg_2reLn&kz~l?0h{7b!jZ*W!(GCW#h1d_%T&u<!(PKs!_v%D3#yp-Q@FC3in2g9
zWeL<U*KntB*YMVGrttK#*7D_Xp^B&Q_OjISr8Cs>gMGl4B?$L{Kn?!_mJ*>FzAV8S
z0ZDMl%*aq!TOwS;-^^IcU&9~FpvmW#%nYgCz$7T=gK#hd1H)%fW~pIZz)-^gu8Emy
zm?c0pB;x{5gM}IHW!4&2Xj!+C<(7DAYCwK*Ca8hwm{yWn<d$Dl4r(8O)6FdwP@A$y
zn1O*|CEG1Fa2-~RNGy=d3Qj1jsYONkMd&3QM|x_Bt7|}h@ph2&I2afhxEMJYg&0{F
zs}!M$22`B7q!uUZ+2rIWC*~B}>B01%v}8ev8<bW-{wxON!xDxCj5Q#iGC}>K$pm&X
z*s+?7MLwWH0$jI(32@toy(Hhipd>SQFDRLU${QZ0Dg~VGfGS356M{5>YFUsoK7(>r
z2?MO^sbQ>PT*wS{%u1G9ti{Qh1(mlr@<BFcR;At&@X5?eO)PTG&nqq|O3chF0XIEX
zG8KU`3^==?2On25T<sB10D^26Vy;rg>12fZB2Zu#fwC63aCiv{zgw)qKEZ(jn#{M@
za#Kq(@>6bcBQzA>VoS;|%}XiPWGadVg<TA&>=uDUF{pV|1ge&cR6tV7AOaKwMIoR@
z0Gkt7BP94iH8?m9ioo@vDo81~%mF3NTdd%w_$`jy%)HE8P@phY++qbcr@#$}m4cAK
z0Jo$3Kw1Mp1So|SfePIs7Z5ibWGPcpeo-ul=L_QTK$2#BaY<rP$t|Xmj9Yv;nR%ej
z12n8}F&Cv4gG;Yl%t<+k=poII7NlE2Da3$*fq{dGgN=)sg^`7kgGq>ui;;(sk5Pz;
zg^`a@gi(Nzi;0U-f>DHtg;j`AjFE$jg|P@!1EVAvP!R`;43PUjlUl@qDi?6E1S#H_
z;l&#>w0M&$VaZ}$z*Yk*-q;s#l&~-0gy(fc@x}x;2U5I&TG3qK;*A?zyxEnoXYtf<
zm#}B?VkzGEz{Oi8$fhiQQ1Mp7059O!YWecG(PdJ2K?NVQh!X%6XDCG+Gx<eakvk~L
zL9NmvP{Xgt8^i@?a4-SN<wc-$^9>~K&%nS?1?mZc$}^NkR(NJfMqogEwl9`a2b9Uc
zMJodX1E{2hXLnHB15~;$gmnPHr4P8n$5g|xkV%{Yn)x-Ei$F;Sk{`fjCo`lt0%i1D
zoS^1ld`W&hdV8=4l-B=&@;RuW<6xFxs?vidOG1t(R8$oOg2DqFY(XGa7>EFe7?=PD
z8z@(?Gcqs~MSz0MkVu;m1qfP+QxplZBMMY=u-sxvEJ`m%6f5A2yOO!65TpcN*sMe@
zY`}#FYB^IB2T~3ybc+y$47fZ(Eo6$~LFy7f1h~iowWEuYKwMCpu_zhD0(%=wfc$<-
z7O!u=fZ_xcqo5867aO=RVC2JE7)UXSF|n|L3j=Nz#-b8z?HyQQP?QZSVe!=G#3pt#
zP^=M7?4Bef_M#k6D1j1;CNs9Kk0&B=7v+MKg2NI_fC3bpl<h#y$_FJTyvZ2JS=36#
z?jV~9Cu1a=iAcsppdp~5B9Lc`K?KR^$kPPmtWuEPa`bcoYSF-Y7N09{^ek$?9a?D5
z0nw{qmB7`jV1ue<N9k3tWArMRQ&?cVij}N>MV25Zg1Zz}AeJ>KnQ}lPToWV!&YR#m
zy$r+!RdC>@OcAKuDzXKMfl~sQ040Z8u-<-P0K6n~jnDQhVPs$^1{I0|pgNVAi;<5J
zsk^{b1nQQd)TPM5R0Im9qDtJ31Lasq^9A11A;EFJJt&TY^<_X!0VXMC7N(+Vd~sMj
zhw5<%>M3IMEjhrVENDGWNbeCdo>^1aVDYTUjug)?|Nj5~|0SrhD<Tl%w*<h$Ac=XI
z1-^+Dptd_PAuI(gUXh~T7iz>3q)=vLVT2?BF;MT3Q3^E1RS$|^m0O}nW4n53`9<a6
z!QES|d8N4pmEfd#OE5RFq#!51BquXTub>jdy2S>inG14CZb|1B<iwZc=jUW+mK5vd
zf)p61=p|O978|GBVu$i?u|Wlx3XE@wAeop58-*_d1rv&8$@w|?Ma8$+pfpQzeooOX
z=HklYTYTUyfF7&|Py}i+-{LGRfb`9aA^8Q=zPQB$Q3)BCECO}t!0lo1I4pRa4?GH1
z1RDGS4@-c1(%`lxxQ;0T6)52B0#24ipio0<;vzC4hfQvNN@-3isBC5^2IUYA1`Z}3
qMj=KX@Mt&(nC4;RVB_HB5aN*J5a*EOP~ec}u;*~#aNv;OkpKY1{d=?k
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc b/python/ur_simple_control/visualize/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=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/python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..5ba205c8b7b413f27c4deb1fe4e9770e874c7c6e
GIT binary patch
literal 3448
zcmd1j<>g{vU|=XnO-a?0U|@I*;vi#Y1_lNP1_p*=8wLi36owSW9EK=HPKFew6y_F&
zD5eya6xJ4oDCQKl6!sQ|D3%nC6wVfgC{}ld6s{ER7KRk=W~L~%6vki%O`ew^bN!N8
zKok`7FfcH%GcYhXgKT)h$iPs-uz<0KaUo+ZO9{gQCNPT?EXKT$v6c-i24b<7Fk~^O
zGo&ysVoYa9VFHuPix|_HOBk|PQdoMKYB@?67O>WEfYhb1rm)s9AmkRXfn2+gDUE3%
zBUl|fL|r;V3dbTwm<cf1Ean=P6wVq}5r!1jbmkh?8Ws?p&X9s?A1By8PLO>NIhZ*p
zwt-Z!*T7Vv+XfTI?JF*@EnFa5YM4O&!r?2BI`$f{IxN0|i8C%@Oku5IOW~|x7hy<e
zPGPNKuVG7PMwnU44YrRPWM4W%3Kt@DP<#ba#a;tbh3+fv8jc!H0frhb0friG0ft}(
zO>RF;=9i2N3=BnZLX#Q7W`c>r1i&glqAMA1v8I$%7Nmj|z67PqBCt3MSOAFtiN5^*
z|Nnm!Ge8Dua@=CdE4am+oL_v4xi~ZL7HeWrQDWsS_OzV*#1dnpTWlG{C5g$|x7dqP
zi!%}nQg5-9fdw>qZZW3ZVlBx?ElIq^SagduF{dCS@fK5Z$t|Yhl3Ps4iMN=F6L0aA
z6eZ>r$5+MY#TOK%mfhkh$}d6iI3cnX@p-q{K@t`5c`F%;WEdD2e%0w`<maa9r{@<J
z<fkMSfq6y<Zcb@lihe;+Vnt@LerZvBab|8oPHKE|eqKpYevW=YWl2VU9u|qR%;M6-
zoXo0J{i6J&{F3<MlG3y^{jB`Vyps6D;`ogG-1ywY61{@TTU;(4CHWrtxxR@d!l10E
z#K6G7#R!2)%o>bxj9M&ejB<=nHZzDVz*Hp1z`%f#pE;59a{~iu`MHKAg%Om`YS=^=
zYFI&OvWBUKMT8-p1(fSRqA83ZK2(MkB$L7fO0zW#=`4_3m(BvqceNZf9Kj4LS^bJY
z1q>v4uz^w+DCMkVE#d&NLGCRA6@W$DATFpl(&Q`>0I>x@gb;`Tg-DSIh$RXl#6W~N
z0|Ub?Ru>Q7#1e1}LI_!qD0gyEYGO$$Sk4F(m=Z{VDaR<q$i>LU$n=+mne8tNOA#of
zF@g}}HgJ*HL}n1CFx4=DgAEjvEYP517h$Mj;AQ{?A6O2=X94kQ*lHL+YH$T&kvz!D
z3JeSkBm@qdFE|7s0V4x40315PkkIkXPsvQnOicl+u|p3fAx0TSG2~EE0VOn04u;kn
z3?MeBA_0ez4+8^34MPb-7Gnyd2tx{!G($E^kpMEkmZ63r1ynh}RfsTTGZd+yi-FVy
zGib8-y#ytimmnL!$w!k3oL+7*6=Z5M7J-un*h^pn>@Tk5oW$IM5KsZ(2l5UmQ3)~f
zF^Vw?F|sfgX@EQcb3BX&6;$B5)r5h8A(bJDF$GkkGNv$rYgFbG7I2Nqn!*OEQB&Ac
zI9eE@*itxCxLO#Z*i*Pcb?PlvFHm+)2BmD6u^=`ptWvE5g$-*BLp(zbYYJmFQxR_o
zV+~U?V+}(*Qyy~-6Ns(>@tA9vYZ&5LYFKI*;#o`B7O>YaEd+^kl&~$}gtEE7;w)fs
zZm>9n%~Qf#!;r;X!;r;P!;r;N!;r<=3^Io=1ytNK`4xeDs>y$g6&y(5z<>lIIHw^p
z87SQsf%7CdyMe;62sO`v^UEzZNKvK9c8jGrH75;ghSdTkIdJ@d32-EF#K&jmWtPOp
zM}eXUlrPyBIhc4D*_hZEL6C<L0%aJh_`u<+2h)%YN^r0M2C+fu1DyC~z_KVq79_x!
zOBfb_f&ju}En&!F0|ibBBPduz7-|@@KpBrAi?xO!iv!H(1j&O+Gmr`p8=?lRgBh#>
zp$=B+F=TOoRMjwKaiL^eZcyu|h9RB@6!hSXtI1o02v=~@MGIVT(FZTpionqUju=pJ
zSftIsz_60BNC!)Ba+emQz)HFjP>6!G@-gx;>M&|B3PAHO&)+IRNLV7NP6idiu;2u-
zK^PpIpc1o&A&UW=R=G<UYZ#$vm7|6+g{g!&i$#P%i~*c#85XcEWME_fd2}T+B#pfU
zCp5MqaO8vX93+{6{jU!4fj-DL%n;vz!vd^Q1|$V0z{y0YD8D4JBsD&-G&i*<GdVFQ
zr?M90J&-Y6OgxMNOgzk05)e;eD#h>{JTE$d@*+`wD*+`AP_hB#Cw5Rm0p}-9kY9@o
zKm{=uZa;zwOGuUg#YYji`U1sb5h!fY%VU112g)-`GU9z*Jlat`D#KJIisDh2I*bef
z3VCo0f^uyMD48=hgPQ$}ews|Tn2Sq_z$vLn4<6Kr@aBk*FG?*aijSWNvK(X(7g7d<
zNcm}U+~SCj&r8frjgP;@6(66QpHi9wV)Mku7nUaGKxB$^LFR*ExyTsA0+p;qW*`>W
zIs^fV=ps!928KwGouHJ?!N9@DA;rPT!NtP`meFLt#hO=|TTltfKXM>bK-F~-IDo;a
q3uFLNh=Pp=nS6`G2I4+DP+3vT$H2hA!NA1CA;2Ne!NVaS#0>xxnX$J3
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4c46e6f66c03ec78ccfa78f5c4ec2a045d0df4e9
GIT binary patch
literal 1481
zcmd1j<>g{vU|=XnO-VImVPJR+;vi!t1_lNP1_p*=69xu`6owSW9EK>y6s8pB7KSLM
z6qXd$7KSM16t)!h7KSL66pj?m7KSL+6s{ER7KSLc6vki%O`ew^^Zb%QCV^NW%*?>R
zz{0@5;0!X^ih+Tlgkb?=4Py<11j9ncTBaJN6h=vg6sBy3BIz2Y6lNqg3yht@8qAQw
z7R;c@?pMUfz`&6HXXVFBzDWu8FG0MQAlpLRG?{KO<rUmwPE0D+WWL2*5TAOBtt7E1
zJ+<T(W9mwVB2ESdhF`_{8Tq-X`sw+_1^Fq7MPQy0f}2yCm!e-#lvt5jtY2CbU!0j+
zkdqppoS#=xl%JztP+5|ZpNB=FEVH;YF(<PsRlg`dDZeDXxTG{KO~14xGpAUupz;=1
zYEe;sQG7{iaS13Oia8h<82A`f7+Dxun2NX=7#L8(3*;M+GH`eafWs?`aRC!7#DW=C
zGWuyU7lD-AV#~?QOUy~X#gdm_l&i^fi#IVjBQv!uH6=bhKQZSP3y1`J972Gi{}wMu
zFg_)-xFj(zITd7mF~~{*#v(zGQ$QgKiZ&1iiL=6@PJw}ep@ty~jI$U87-|_ym=-XX
zFlDhUV69<X$heR(n4yS=fq_Aj?G}4hVsd;*QRXei5QsxriVKTMig+0q7;Z7Aq?Twh
z-C_j?)h$L}h|@r^Qv`OOOmbpQa%oOtNvdyRUS>gQPGVAKPG(7^XI@Hb1wSYefxOPg
zD8yJKf$m^XS_5Hl*lA&La2E3d7La2>X%3Y3YM2(VE@Y5o2xib^@+$)QSCg$sh=GBj
zNEk$bY=XMFxGd!sTVhdqZf2e)bCDQGRvhFVk(A8D^!&WUocR2-_~Nn@5QX6<Ik=yK
za}#rNQj1H#esj%CPt7Y!%mLX|%mz*s0*pM2MKb9A0?B~_2kfsdg8oWjs%5U?C}GNC
zhD1#*OE^QG6bl1LB#W)mn318Dm64%}r-VI)xtWoXA%(G)Ih-MqA%&rqwT?B96+)LY
zmNS$vEr6KA2@17^j5SO(ObfU`Ay&hj!ZL?Bm_d^j5^ms(S)~x<@9h`rp%AJN?CIwo
z>f;#X8R8nEhya?*x43gb@fx2AiqBhu#n1qWPlX1?E$-sXy!6tX#G=fSN=<HTu~8%m
z3VG(tys}%|KACxrIq9yT9CnMfpeQr1q(}r5Iies!3Y;7fu~j4u5|sz(X7!EF%quGb
z<wf)qC=HLTfYhS2{Gwb?M)6H8DauSP1|_*-Q2g*Q3NW%TGBFA;3Ndmp3NUgsF#Qu?
z=3-=GWcyd70<xF=7HeK<Zb2nD2Y~ZAIER5#EjWt7%8)Ea<N*$w-29Z%oK!ndZY&06
RDh>t?W)5}^77i93Rsi=vN$3Co
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d0a69e62d20ea45aceaf28eb99b3149a77c7d3f3
GIT binary patch
literal 4122
zcmd1j<>g{vU|=XnO-YTDWMFs<;vi!t1_lNP1_p-W5(Wl_6owSW9EK>y6s8pB7KSLM
z6qXd$7KSM16t)!h7KSL66pj?m7KSL+6s{ER7KSLc6rL2`7KSMH6uuPx7KSK}6oC}M
z7KSL!6rmL17KSLU6p<9s7KSM96tNWX7KSLE6p0kc7KSL^6sZ*H7KSLk6vki%O_`S<
z_xL3<f+#2kxr&v6fx(%9f#Guj0|P@kLoH(s;{t{nh6Ri%j0+hTGBPs2`AlehW;8wv
z8lM%7&j#U_Fr~0JgUt<Q(B$yDB~+Y|Uz(GWS`=T9m=~XwpP5&3OBgPelbKcm735CM
zNy;yWa(K&&GK)*%4N<v9s9a+R_ZNR=Zb5!gNotBhZhlFAQSnOVTWpC11*v%{w>Z*M
zOMF3sw^$2`GV@ASGTmY=%1_EKxy1%m%TOf9z`*dUML#1yH&s79zqlYjC9w$1GeU54
zO7l|m3yKmeGK=*~i{gtja|?1(<CF9AN{aGx^b0CWGV=4VNR(w3mnP<9R;B8LEsZZO
zDNRe$FHcR%FDZ^sEXa({NX;ooEs9So%`4U`sJz9Ko?7CVlLK~fF*_(h2{AA*urLZB
zK@LV1#v)+`28Lv2Xgq@`76t|eHfUTIPhwzTs9{>b0E**U<{IV&j44duxJG3$!^IFR
z7PuIU#gxLD!kWUC!nTlwk)eip0d_un3OkBClwZP}!U0MRDGb33nw);O*i$l#3vv=G
zGg<!sV8~Qr_|K5}>^}p;%M=C%hL<)V!kU4Bp-MKTD79RnBqLQJIkPA^CsiRo56ml0
zE=o<!(`32Dm7ZGS0x{|qM{#P2bAC>K(JhXYqQr9lvc#MsUIqpRO_p11MTseyrNy_{
zGEy_sGfIm185kIfL_h>6(!nub1Pb9>Tp*p!V7oxl#S#n*3|x#{Ok9jG2#W$9#v)lz
z;Da(3DBwXD6cMnDR;<Cmz)-?a!dS!5%-GE6!Vt?{%T&Wq!z9VDfT@NNM1z93kPQ?h
zD;fPXnQyTcCFZ54-eS&4&AY{2oLUl=nv<WLSyHLVTqMB2z;KJLxF9t(rT7+OCd9>H
zw}YL{R}4+w!C)22pcDg21p<sBj4c0)R6q_zE)h^1Y62?}7+e@)`D&R!DW-%mg|Qh{
zE-*n-O(9DOa}7fcV>1&Y1IW>u%zn3+GxO4lKqhOlfSnwWUz}NznV)xyH7zGUv80F_
z6viOyS3>+yqy+K^D>%=gdqO%hFS8^QoJ}AJ-z~og9t0pCfy!PMMh-?kMjl2!<{~X@
zz5=CxP&rquhwLl%S|(8W2FeGF423M9NP+mHgeipyo-UdFz+S21OUZ;5AqpTTX)+_c
z0Zvdwpr|Fr^{~(Yg>W&<^*l^m%sh-m+SptVOFW-jV6LZeDP03fn4r*MTELXT4o;9J
zu)JCWs!urmZb`yRX?V#UU!0m(oL_WH3ND2xz+rMiNaZ+8Oa!GkM-zpX>S&_S!W}C5
z5)_KRq~Qg9VqOY3j#5(;AWkY?$pVh8TY~ASC9v=hhPak3H7_wKClwrrE1AKCz%6#D
zwp(lv270uLfJ)-L6wka&P=O3FsMsEq8pSY6WL6$V4ki}HB2d|dQUjypHw9SSg7O<D
zBxTkxEnr;801kf^P`DS#feJ1rh*v<BS6E_BX{si3ktHbq@Pn(*_yTBVD=xAEWixPb
z045-1LQ!gBiWjJ!4K7J6Neu+KnFH)*4n_e`-C>8G0zpL?ECm*UIG}o4Aj>Gvtk~GV
z08+`u8{R?}F~Sfr#t<>V5HZCNF)QK(c>@%~MSLI@*e_rL>>&YARKO}luf*j1Xpomc
zQfL(;C<<KBJ%=0xMLM9+<@CEHj_x^VGQ*HC!X;shOTq+~gefiwGpIz7G05|vGNZ^8
z#IgVp;9vj~AP?V?0EGp5vIK=m1}IFhBuzGG()2+O7f?LIBK>m%ENL>*wsBYkGoKya
zSOhl@KuI6eJm7#N@I=E~FqRRVWejJTz*(knmKmI7PD*m}F3nD?12vC9wLWTc5?}<k
zv_jDX4mm-6o-#lIhf;oXfXfe?66O?6P@M)2OfJ7$(9$B&5SqT=0`No)QvpxRFadZX
zh6%tEFH8WIXyJyMLv0~D6@#Lp8x$3wwiiZJz>3&-^yolN$3?!-{4J1WRBVo3c%g`Z
z3ojHAaN&g_0xrBzM8Jg?iijDgI71O3l>R^!6ugZO%DUzYKp8y@M1XQJ&I+U`9OOGL
zaOs6&BRqn|(Yy_hXmkm9griHqBOYA>o&eA#;0Xa;f|LXyiK|#MUkeJB1P}r0z-sc}
zV$CbfEvUT3lve=m1c4jl;D#=^T?lSof$QNSP&HQsst$@kWnU2}9~XgAPZ21Pi$JC!
z)%l1Lmcu4DKczG$)eh9UEtX+mVBlcpVCLZE;OAiD;O5}w5aQtC;N=kG;N=kI;ROJP
C=nUWh
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/drawing.py b/python/ur_simple_control/visualize/robot_stuff/drawing.py
new file mode 100644
index 0000000..d2c85e2
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/drawing_for_anim.py b/python/ur_simple_control/visualize/robot_stuff/drawing_for_anim.py
new file mode 100644
index 0000000..f40ad14
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/follow_curve.py b/python/ur_simple_control/visualize/robot_stuff/follow_curve.py
new file mode 100644
index 0000000..49feb20
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py b/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
new file mode 100644
index 0000000..0a54f80
--- /dev/null
+++ b/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
@@ -0,0 +1,544 @@
+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')#, animated=True)
+ #line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g')#, animated=True)
+ #line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b')#, animated=True)
+ #line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link)#, animated=True)
+ line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r', animated=True)
+ line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g', animated=True)
+ line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b', animated=True)
+ line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link, animated=True)
+
+ 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/python/ur_simple_control/visualize/robot_stuff/inv_kinm.py b/python/ur_simple_control/visualize/robot_stuff/inv_kinm.py
new file mode 100644
index 0000000..2906bfd
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/joint_as_hom_mat.py b/python/ur_simple_control/visualize/robot_stuff/joint_as_hom_mat.py
new file mode 100644
index 0000000..66233e1
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/utils.py b/python/ur_simple_control/visualize/robot_stuff/utils.py
new file mode 100644
index 0000000..e359695
--- /dev/null
+++ b/python/ur_simple_control/visualize/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/python/ur_simple_control/visualize/robot_stuff/webots_api_helper_funs.py b/python/ur_simple_control/visualize/robot_stuff/webots_api_helper_funs.py
new file mode 100644
index 0000000..8299d4b
--- /dev/null
+++ b/python/ur_simple_control/visualize/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
+
--
GitLab