diff --git a/python/convenience_tool_box/ft_readings.py b/python/convenience_tool_box/ft_readings.py index 65f21ae3002e9bbff28162f392ae10d548a47a03..a43f9ef772e38d1a0622e25dd972e568ff6151a3 100644 --- a/python/convenience_tool_box/ft_readings.py +++ b/python/convenience_tool_box/ft_readings.py @@ -22,7 +22,7 @@ robot = RobotManager(args) ft_readings = [] dt = 1/500 #while True: -for i in range(15000): +for i in range(5000): start = time.time() q = robot.rtde_receive.getActualQ() ft = robot.rtde_receive.getActualTCPForce() diff --git a/python/convenience_tool_box/fts.png b/python/convenience_tool_box/fts.png index 4e0feaefebb198021b86297773ea356f31801b7a..4195055f7686019be819db83f4ba74c68829d9a9 100644 Binary files a/python/convenience_tool_box/fts.png and b/python/convenience_tool_box/fts.png differ diff --git a/python/examples/clik.py b/python/examples/clik.py index 6d3b1082d9ddcedb200b633534ca1822023e6e04..b6cde26961a23ff9e30bd27a9267b9f48b69803b 100644 --- a/python/examples/clik.py +++ b/python/examples/clik.py @@ -64,6 +64,10 @@ if __name__ == "__main__": Mgoal = robot.defineGoalPoint() 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)), + } # we're not using any past data or logging, hence the empty arguments - loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {}) + loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_dict) loop_manager.run() diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py index 461d155d192cf4d32be0723186875bde276e7c8e..8b140f2d77f799964fd396897a60bdec203a42e5 100644 --- a/python/examples/point_impedance_control.py +++ b/python/examples/point_impedance_control.py @@ -219,7 +219,7 @@ def controlLoopPointImpedance(wrench_offset, q_init, controller, robot, i, past_ #wrench = np.average(np.array(past_data['wrench']), axis=0) # first-order low pass filtering instead # beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1] - beta = 0.1 + beta = 0.01 wrench = beta * wrench + (1 - beta) * past_data['wrench'][-1] #Z = np.diag(np.array([0.6, 0.6, 1.0, 0.5, 0.5, 0.5])) #Z = np.diag(np.array([0.6, 1.0, 0.6, 0.5, 0.5, 0.5])) diff --git a/python/initial_python_solution/manipulator_visual_motion_analyzer.py b/python/initial_python_solution/manipulator_visual_motion_analyzer.py index 690020863bf13c694cd6623dfc58c285701eb561..c220018f8396d3f88a93e7ec34cc1e3bea439d4c 100644 --- a/python/initial_python_solution/manipulator_visual_motion_analyzer.py +++ b/python/initial_python_solution/manipulator_visual_motion_analyzer.py @@ -21,11 +21,9 @@ import numpy as np import sys # 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 -sys.path.append('../geometry_matplotlib') -#from common import exit_on_x, make_rotation_matrix3D -#from plot3D import plot3D_camera, plot3D, set_equal_axis3D, camera_data_for_plot3D DPI = 80 SLIDER_SIZE = 200 diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc index bf08e547774804db451086aa3542eb488668758a..39479797c796ac2cf74a596f87a7c0fc705d6b0a 100644 Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc differ diff --git a/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-310.pyc index 291588f9bd393e2ee55972ae03feea53f3844d50..322e8243ac4f9a77b5dd3edd6ae80436ae5961e4 100644 Binary files a/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-310.pyc and b/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.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 0d00128461b12a79c598dac57d357e8a2c65afb3..a0f5ed28336f2c03bc6863e0beb5386839296e1b 100644 --- a/python/ur_simple_control/clik/clik_point_to_point.py +++ b/python/ur_simple_control/clik/clik_point_to_point.py @@ -132,7 +132,7 @@ point-to-point motion control loop. """ def controlLoopClik(robot, clik_controller, i, past_data): breakFlag = False - save_past_dict = {} + log_item = {} # know where you are, i.e. do forward kinematics q = robot.getQ() pin.forwardKinematics(robot.model, robot.data, q) @@ -148,6 +148,7 @@ def controlLoopClik(robot, clik_controller, i, past_data): # just plug and play different ones qd = clik_controller(J, err_vector) robot.sendQd(qd) + # TODO: only print under a debug flag, it's just clutter otherwise # we do the printing here because controlLoopManager should be general. # and these prints are click specific (although i'm not 100% happy with the arrangement) @@ -156,9 +157,15 @@ def controlLoopClik(robot, clik_controller, i, past_data): # print("linear error = ", pin.log6(SEerror).linear) # print("angular error = ", pin.log6(SEerror).angular) # print(" error = ", err_vector.transpose()) - # we're not saving or loggin here, but need to respect the API, - # hence the empty dicts - return breakFlag, {}, {} + + log_item['qs'] = q[:6].reshape((6,)) + # 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,)) + # we're not saving here, but need to respect the API, + # hence the empty dict + return breakFlag, {}, log_item """ @@ -170,7 +177,6 @@ point-to-point motion control loop. """ def moveUntilContactControlLoop(speed, robot, clik_controller, i, past_data): breakFlag = False - save_past_dict = {} # know where you are, i.e. do forward kinematics q = robot.getQ() pin.forwardKinematics(robot.model, robot.data, q) diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc index 241bd9e24178eaa5bc912899e4c8fd24857bc3bf..381e01577542314df3b46fd5df48a3467ae9ca1f 100644 Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc differ diff --git a/python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp b/python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp deleted file mode 100644 index ba65be11a0473d207f62e43b83c0564e7d9518bf..0000000000000000000000000000000000000000 Binary files a/python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp and /dev/null differ diff --git a/python/ur_simple_control/visualize/.visualize.py.swp b/python/ur_simple_control/visualize/.visualize.py.swp deleted file mode 100644 index 85c39c3b20e714208f92a4c7bc7a2afcfe88cc4e..0000000000000000000000000000000000000000 Binary files a/python/ur_simple_control/visualize/.visualize.py.swp and /dev/null differ diff --git a/python/ur_simple_control/visualize/make_run.py b/python/ur_simple_control/visualize/make_run.py index 240c7fd2440b06a8e325e790b5c2e32b8323fcb9..378d700020cca2187953753d745ed7e414ca0183 100644 --- a/python/ur_simple_control/visualize/make_run.py +++ b/python/ur_simple_control/visualize/make_run.py @@ -2,6 +2,7 @@ from robot_stuff.InverseKinematics import InverseKinematicsEnv from robot_stuff.drawing import * from robot_stuff.inv_kinm import * from robot_stuff.utils import * +from ur_simple_control.managers import RobotManager import numpy as np @@ -13,7 +14,7 @@ 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)), + "dqs" : 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)), @@ -23,14 +24,14 @@ def makeRun(controller, ik_env, n_iters, robot_index): } 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) + dqs = controller(ik_env.robots[robot_index], ik_env.goal) + ik_env.simpleStep(dqs, 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 + data['dqs'][i] = dqs # NOTE: this is probably not correct, but who cares - data['vs'][i] = ik_env.robots[robot_index].jacobian @ q_dots + data['vs'][i] = ik_env.robots[robot_index].jacobian @ dqs 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 @@ -43,3 +44,82 @@ def makeRun(controller, ik_env, n_iters, robot_index): data['dists_to_goal'][i] = dist_to_goal data['p_es'][i] = ik_env.robots[robot_index].p_e return data + +""" +loadRun +-------- +- args and robot as standard practise +- log_data is a dict of data you wanna plot +- some keys are expected: + - qs +- and nothing is really done with the other ones + --> TODO put other ones in a new tab +""" +def loadRun(args, robot, log_data): + # some of these you need, while the others you can compute here + # 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 = { + "qs" : False, + "dqs" : False, + "manip_ell_eigenvals" : False, + "manip_elip_svd_rots" : False, + "p_es" : False, + "vs" : False, + "dists_to_goal" : False, + "manip_indeces" : False + } + # 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)), + 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)), + recompute_checker["manip_elip_svd_rots"] = True + if "p_es" not in log_data.keys(): + 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 + recompute_checker["vs"] = True + if "dists_to_goal" not in log_data.keys(): + log_data["dists_to_goal"] = np.zeros(n_iters) # this should be general error terms + recompute_checker["dists_to_goal"] = True + if "manip_indeces" not in log_data.keys(): + log_data["manip_indeces"] = np.zeros(n_iters) + recompute_checker["manip_indeces"] = True + + for i in range(n_iters): + # NOTE: this is probably not correct, but who cares + pin.forwardKinematics(robot.model, robot.data, log_data['qs'][i]) + J = pin.computeJointJacobian(robot.model, robot.data, + log_data['qs'][i], robot.JOINT_ID) + # 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] + # TODO fix if you want it, whatever atm man + if recompute_checher['p_es']: + #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 + # 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 + # 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 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 fe8a6d1f0ce4f9eed36ecc5544ba7d61e4c8af9f..e9188a0b74543ac473e6f975f1fd748cce5cb800 100644 --- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py +++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py @@ -1,5 +1,7 @@ # override ugly tk widgets with system-based ttk ones # change the import * thing if it's better for your ide +# TODO: remove last thing after you switch ellipse off +# TODO: set reasonable y-axis limits for side plots from tkinter import * from tkinter.ttk import * @@ -25,9 +27,6 @@ from multiprocessing import Queue # for local thread which manages local queue (can be made better, # but again, who cares, all of this is ugly as hell anyway ('cos it's front-end)) import threading -# 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 @@ -117,15 +116,13 @@ ManipulatorVisualMotionAnalyzer - 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 +# shove artists 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. # 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, real_time_flag, **kwargs): # need to put this in the main program, @@ -183,19 +180,19 @@ class ManipulatorVisualMotionAnalyzer: # 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") + #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.robots.append(Robot_raw(robot_name="no_sim")) self.ik_env.data.append(makeRun(self.controller1, self.ik_env, self.n_iters, 0)) - self.ik_env.data.append(makeRun(self.controller2, self.ik_env, self.n_iters, 1)) + #self.ik_env.data.append(makeRun(self.controller2, self.ik_env, self.n_iters, 1)) # ugly front end code is ugly.