diff --git a/python/examples/clik.py b/python/examples/clik.py
index b6cde26961a23ff9e30bd27a9267b9f48b69803b..933b9bf209c293cd9066bc5a5e469afce2f1109a 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -1,8 +1,12 @@
+import numpy as np
import argparse
from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
# TODO merge the 2 clik files
from ur_simple_control.clik.clik_point_to_point import getClikController, controlLoopClik
+# TODO write this in managers and automate name generation
+import pickle
+
"""
Every imaginable magic number, flag and setting is put in here.
This way the rest of the code is clean.
@@ -65,9 +69,25 @@ if __name__ == "__main__":
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
log_dict = {
- 'qs' : np.zeros((args.max_iterations, 6)),
- 'dqs' : np.zeros((args.max_iterations, 6)),
+ 'qs' : np.zeros((args.max_iterations, robot.model.nq)),
+ 'dqs' : np.zeros((args.max_iterations, robot.model.nq)),
}
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_dict)
- loop_manager.run()
+ # TODO create a logging function in util.
+ # something that generates a file name from something in args.
+ # also save args there
+ log_dict = loop_manager.run()
+ # save the logged data
+ file_path = "./data/clik_run_001.pickle"
+ # pretty sure you need to open it binary for pickling
+ log_file = open(file_path, 'wb')
+ pickle.dump(log_dict, log_file)
+ log_file.close()
+ # but also save the arguments
+ file_path = "./data/clik_run_001_args.pickle"
+ # pretty sure you need to open it binary for pickling
+ log_file = open(file_path, 'wb')
+ pickle.dump(args, log_file)
+ log_file.close()
+
diff --git a/python/examples/data/clik_run_001.pickle b/python/examples/data/clik_run_001.pickle
new file mode 100644
index 0000000000000000000000000000000000000000..22dba69ab354c9446dc0c1977f138a6fbb4aa299
Binary files /dev/null and b/python/examples/data/clik_run_001.pickle differ
diff --git a/python/examples/data/clik_run_001_args.pickle b/python/examples/data/clik_run_001_args.pickle
new file mode 100644
index 0000000000000000000000000000000000000000..9d20c5ca9feb7d5adf4978d8de0677386d30d4ab
Binary files /dev/null and b/python/examples/data/clik_run_001_args.pickle differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 24de3cbd1f605849db1670e76ac3f2d16f4d86a4..f140aa92fbed2b3fe15daff025b45847443394bb 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
index dd38d4802f19ea27099acff0a3f394579a099c10..f770d71cc54659c570bd13f21e6d8b0bca454d94 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc and b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index a0f5ed28336f2c03bc6863e0beb5386839296e1b..971d4d96290bf73f002e52de4f6ed955312aba73 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -158,11 +158,11 @@ def controlLoopClik(robot, clik_controller, i, past_data):
# print("angular error = ", pin.log6(SEerror).angular)
# print(" error = ", err_vector.transpose())
- log_item['qs'] = q[:6].reshape((6,))
+ log_item['qs'] = q.reshape((robot.model.nq,))
# get actual velocity, not the commanded one.
# although that would be fun to compare i guess
# TODO: have both, but call the compute control signal like it should be
- log_item['dqs'] = robot.getQd()[:6].reshape((6,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nq,))
# we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 05f3ec926c5913e839ab0446529ceef5763a607d..0a0eeb23d98e35556280a14f289d2f494db40586 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -170,6 +170,8 @@ class ControlLoopManager:
# if not self.args.pinocchio_only:
# self.robot_manager.stopHandler(None, None)
+ return self.log_dict
+
"""
robotmanager:
---------------
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
index cc59155c44e721a1377bb4db4c29fbecfe833313..607c0fa9684ec739810129676b0ad85f254d1d75 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc and b/python/ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc differ
diff --git a/python/ur_simple_control/visualize/make_run.py b/python/ur_simple_control/visualize/make_run.py
index 378d700020cca2187953753d745ed7e414ca0183..04ebe058e8f5c0b5cc9264d4f7acd7ade0a9fb3f 100644
--- a/python/ur_simple_control/visualize/make_run.py
+++ b/python/ur_simple_control/visualize/make_run.py
@@ -3,6 +3,7 @@ from robot_stuff.drawing import *
from robot_stuff.inv_kinm import *
from robot_stuff.utils import *
from ur_simple_control.managers import RobotManager
+import pinocchio as pin
import numpy as np
@@ -60,7 +61,7 @@ def loadRun(args, robot, log_data):
# anything extra can be copied here and then plotted in an extra tab or something
assert "qs" in log_data.keys()
n_iters = log_data["qs"].shape[0]
- recompute_checher = {
+ recompute_checker = {
"qs" : False,
"dqs" : False,
"manip_ell_eigenvals" : False,
@@ -72,16 +73,16 @@ def loadRun(args, robot, log_data):
}
# we construct what's not in log_dict for a nice default window
if "dqs" not in log_data.keys():
- log_data["dqs"] = np.zeros((n_iters, robot.n_joints)),
+ log_data["dqs"] = np.zeros((n_iters, robot.n_joints))
recompute_checker["dqs"] = True
if "manip_ell_eigenvals" not in log_data.keys():
log_data["manip_ell_eigenvals"] = np.zeros((n_iters, 3))
recompute_checker["manip_ell_eigenvals"] = True
if "manip_elip_svd_rots" not in log_data.keys():
- log_data["manip_elip_svd_rots"] = np.zeros((n_iters, 3, 3)),
+ log_data["manip_elip_svd_rots"] = np.zeros((n_iters, 3, 3))
recompute_checker["manip_elip_svd_rots"] = True
if "p_es" not in log_data.keys():
- log_data["p_es"] = np.zeros((n_iters, 3)),
+ log_data["p_es"] = np.zeros((n_iters, 3))
recompute_checker["p_es"] = True
if "vs" not in log_data.keys():
log_data["vs"] = np.zeros((n_iters, 6)) # all links linear velocities # NOT USED
@@ -101,25 +102,29 @@ def loadRun(args, robot, log_data):
# cut of angular velocities 'cos i don't need them for the manip ellipsoid
# but TODO: check that it's the top ones and not the bottom ones
J_lin = J[:3, :6]
- if recompute_checher['vs']:
- data['vs'][i] = J[:6] @ log_data['dqs'][i]
+ if recompute_checker['vs']:
+ log_data['vs'][i] = J[:6] @ log_data['dqs'][i]
# TODO fix if you want it, whatever atm man
- if recompute_checher['p_es']:
- #data['p_es'][i] = robot.getMtool.translation
+ # TODO you're doubling fkine, dont
+ if recompute_checker['p_es']:
+ log_data['p_es'][i] = robot.getMtool().translation
M = J_lin @ J_lin.T
manip_index = np.sqrt(np.linalg.det(M))
_, diag_svd, rot = np.linalg.svd(M)
- if recompute_checher['manip_indeces']:
- data['manip_indeces'][i] = manip_index
+ if recompute_checker['manip_indeces']:
+ log_data['manip_indeces'][i] = manip_index
# TODO check this: idk if this or the square roots are eigenvalues
- if recompute_checher['manip_ell_eigenvals']:
- data["manip_ell_eigenvals"][i] = np.sqrt(diag_svd)
- if recompute_checher['manip_elip_svd_rots']:
- data["manip_elip_svd_rots"][i] = rot
+ if recompute_checker['manip_ell_eigenvals']:
+ log_data["manip_ell_eigenvals"][i] = np.sqrt(diag_svd)
+ if recompute_checker['manip_elip_svd_rots']:
+ log_data["manip_elip_svd_rots"][i] = rot
# maybe plot just this idk, here it is
smallest_eigenval = diag_svd[diag_svd.argmin()]
- dist_to_goal = np.linalg.norm(ik_env.robots[robot_index].p_e - ik_env.goal)
# TODO: make these general error terms
- data['dists_to_goal'][i] = dist_to_goal
- return data
+ # also TODO: put this in the autogenerate plots tab
+ if recompute_checker['dists_to_goal']:
+ #dist_to_goal = np.linalg.norm(ik_env.robots[robot_index].p_e - ik_env.goal)
+ #log_data['dists_to_goal'][i] = dist_to_goal
+ pass
+ return log_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
index e9188a0b74543ac473e6f975f1fd748cce5cb800..aa401f468e0e56aee4e7822da510b5bffef5eac5 100644
--- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -17,7 +17,8 @@ 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
+from ur_simple_control.visualize.make_run import makeRun, loadRun
+from ur_simple_control.managers import RobotManager
import numpy as np
# it is the best solution for a particular problem
@@ -28,6 +29,7 @@ from multiprocessing import Queue
# but again, who cares, all of this is ugly as hell anyway ('cos it's front-end))
import threading
import time
+import pickle
@@ -124,7 +126,7 @@ ManipulatorVisualMotionAnalyzer
# this list is to be updated only if you load a new run.
# or even skip this and reload everything, who cares.
class ManipulatorVisualMotionAnalyzer:
- def __init__(self, root, queue, real_time_flag, **kwargs):
+ def __init__(self, root, queue, data, real_time_flag, **kwargs):
# need to put this in the main program,
# so you need to pass it here to use it
self.root = root
@@ -168,7 +170,10 @@ class ManipulatorVisualMotionAnalyzer:
########################
# TODO: remove from here,
# we won't be generating runs here later
- self.n_iters = 200
+ if data == None:
+ self.n_iters = 200
+ else:
+ self.n_iters = data['qs'].shape[0]
# the word time is used for timing
self.t = np.arange(self.n_iters)
# local kinematics integrator
@@ -190,8 +195,15 @@ class ManipulatorVisualMotionAnalyzer:
# 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
+ # TODO: make loading of multiple datas possible
#self.ik_env.robots.append(Robot_raw(robot_name="no_sim"))
- self.ik_env.data.append(makeRun(self.controller1, self.ik_env, self.n_iters, 0))
+ # TODO this data has 0 bussiness being defined in ik_env dude
+ # it was probably there to get object permanence before this was a class,
+ # but still dawg
+ if data == None:
+ self.ik_env.data.append(makeRun(self.controller1, self.ik_env, self.n_iters, 0))
+ else:
+ self.ik_env.data.append(data)
#self.ik_env.data.append(makeRun(self.controller2, self.ik_env, self.n_iters, 1))
@@ -705,7 +717,9 @@ class ManipulatorVisualMotionAnalyzer:
self.ik_env.ax.set_title(str(index) + 'th iteration toward goal')
# animate 3d manipulator
for robot_index, robot in enumerate(self.ik_env.robots):
- self.ik_env.robots[robot_index].setJoints(self.ik_env.data[robot_index]["qs"][index])
+ # TODO: fix actually (put dh parameters of current robot in)
+ #self.ik_env.robots[robot_index].setJoints(self.ik_env.data[robot_index]["qs"][index])
+ self.ik_env.robots[robot_index].setJoints(self.ik_env.data[robot_index]["qs"][index][:6])
self.ik_env.robots[robot_index].drawStateAnim()
# for link in robot.lines:
# for line in link:
@@ -859,7 +873,18 @@ class ManipulatorVisualMotionAnalyzer:
if __name__ == "__main__":
queue = Queue()
root = Tk()
- gui = ManipulatorVisualMotionAnalyzer(root, queue, False, data=None)
+ # TODO: change to something different obviously
+ # or add a button to load or something, idc
+ log_data_file = open("/home/gospodar/lund/praxis/ur_simple_control/python/examples/data/clik_run_001.pickle", 'rb')
+ args_file = open("/home/gospodar/lund/praxis/ur_simple_control/python/examples/data/clik_run_001_args.pickle", 'rb')
+ log_data = pickle.load(log_data_file)
+ args = pickle.load(args_file)
+ # i need pinocchio etc, this is how i can get it with 0 additional hassle
+ args.simulation = True
+ args.pinocchio_only = True
+ robot = RobotManager(args)
+ log_data = loadRun(args, robot, log_data)
+ gui = ManipulatorVisualMotionAnalyzer(root, queue, log_data, False)
# have mainloop 'cos from tkinter import *
mainloop()