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
Binary files /dev/null and b/python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc differ
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 0000000000000000000000000000000000000000..ab4e0ac1d82966d5d3e8fc5f4f18a7b0c95f6051
--- /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 0000000000000000000000000000000000000000..44eed8ec8d3fca41fd835d50294c9ac7d9522732
--- /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 0000000000000000000000000000000000000000..7d1b3fa4cefb06d1be34cc16b5c4ae8b395e902a
--- /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 0000000000000000000000000000000000000000..57b26c9fc0c38042e96ea97d7321c01f779bdd84
--- /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 0000000000000000000000000000000000000000..3411eae78232e0d97dfd535a99263ea674d5c408
--- /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 0000000000000000000000000000000000000000..69b01f64fc37b52ebcc200bcfcf9a05078b747de
--- /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 0000000000000000000000000000000000000000..6487a200a40cfbe40a79615772832b0931d02089
--- /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 0000000000000000000000000000000000000000..8c60750ade82902789ace9705c2001d310c32273
--- /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 0000000000000000000000000000000000000000..991315d641fbffcf960ba9a33c0d71d0c4b3c829
--- /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 0000000000000000000000000000000000000000..09af7229e9e354153fc913f1f410af7cd8b41881
--- /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 0000000000000000000000000000000000000000..19b36264102f5a3a6e5d31bc079d1cee1815c609
--- /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 0000000000000000000000000000000000000000..240c7fd2440b06a8e325e790b5c2e32b8323fcb9
--- /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 0000000000000000000000000000000000000000..11fb982c8d388f5d0d44f2e45df0f167e2d34e82
--- /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 0000000000000000000000000000000000000000..9cd579d94725f7e951d9a585bbc5869f95604d50
--- /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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-311.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc differ
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
Binary files /dev/null and b/python/ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc differ
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 0000000000000000000000000000000000000000..d2c85e2ea353c7e0a14bce8ea1e7077dab0adf57
--- /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 0000000000000000000000000000000000000000..f40ad14a8ba80cdcbc5a4e424cf3878671c770a9
--- /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 0000000000000000000000000000000000000000..49feb200e0b6e71e98551ddb5307fcee46c7a95d
--- /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 0000000000000000000000000000000000000000..0a54f807779e36f9c913ea72e29e149c5dfd516e
--- /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 0000000000000000000000000000000000000000..2906bfd60219df5c7150a1d3d26425a7b1f71318
--- /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 0000000000000000000000000000000000000000..66233e10d9ab1a511580539ffb00b6af3a9f18ce
--- /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 0000000000000000000000000000000000000000..e3596956983c4070fef67f18f0eb9f776d236203
--- /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 0000000000000000000000000000000000000000..8299d4bc6bb4c6439992d00da518c05bda66b0c0
--- /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
+