diff --git a/README.md b/README.md
index c72d8e064ed93add0522f0dbf64b867d351ea561..e1fb32522745bee764d29b5ab5ffc1dc9a26492d 100644
--- a/README.md
+++ b/README.md
@@ -74,6 +74,8 @@ Here is a checklist of what to do together with the key remarks.
behind RobotManager wrappers (because other robot don't have rtde_control lel).
i'm also pretty sure there is not a single instant in which i call robot_manager.getQ() without
calling pin.forwardKinematics immediatelly afterward. so think about doing that in robot_manager.getQ().
+- instead of gepetto visualization, just run your python thing because it's the easiest to modify,
+ you don't need to develop anything new and know how to use it already.
- long term: write all code in c++, make python bindings with pybind. have common documentation
and examples
- short term: the long term goal is too big of a time sink right now both lack of experience
diff --git a/python/README.md b/python/README.md
index 9d3d5f035a17e4e123b95787eafcea88b7af14ac..2dcfa448f505a0d793ffa836df25f69a9c7600ba 100644
--- a/python/README.md
+++ b/python/README.md
@@ -9,6 +9,14 @@
- write appropriate asserts over applicable arguments - you don't want to blow something up
due to a misclick while typing out arguments
- try to get reading and setting the payload from code. also figure out why doesn't it zero the f/t sensor
+ --> works, but you need to change ur_rtde code, recompile, and install both c++ and python from there.
+so not terribly convenient. it will get fixed in a future ur_rtde release tho
+- merge existing visualization with current solution. no, it can't real real-time with the robot
+ because it really is that bad, but if you run it in a separate process and update it sporadicly,
+ it will be ok. it already has a bunch of the goodies you want so it's certainly easier to get
+ this to work than to re-do the same functionality from scratch.
+ and you won't really be using it too much anyway let's be real. AND ALSO,
+ if you ditch your shitty integration for pinocchio's it will be faster.
# installation
------------
diff --git a/python/alternative_packaging_BROKEN_atm/pyproject.toml b/python/alternative_packaging_BROKEN_atm/pyproject.toml
deleted file mode 100644
index 1722d4551c3d49868fa51d9e6ac17e7dda06f60f..0000000000000000000000000000000000000000
--- a/python/alternative_packaging_BROKEN_atm/pyproject.toml
+++ /dev/null
@@ -1,31 +0,0 @@
-[build-system]
-requires = ["setuptools"]
-build-backend = "setuptools.build_meta"
-
-[project]
-name = "ur_simple_control"
-version = "0.0.1"
-# TODO: fill out with what you can. pip install pinocchio didn't seem to work
-# so it should probably be done through conda
-# which means it can still be added as a requirement here?
-# TODO try it out
-dependencies = [
- "ur_rtde",
-]
-authors = [
- {name = "Marko Guberina", email = "marko.guberina@control.lth.se"},
-]
-maintainers = [
- {name = "Marko Guberina", email = "marko.guberina@control.lth.se"}
-]
-description = "Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations."
-readme = "README.md"
-license = {file = "LICENSE.txt"}
-keywords = ["manipulator", "control algorithm"]
-
-[project.urls]
-Homepage = "https://gitlab.control.lth.se/marko-g/ur_simple_control"
-Documentation = "https://gitlab.control.lth.se/marko-g/ur_simple_control"
-Repository = "https://gitlab.control.lth.se/marko-g/ur_simple_control"
-"Bug Tracker" = "https://gitlab.control.lth.se/marko-g/ur_simple_control"
-Changelog = "https://gitlab.control.lth.se/marko-g/ur_simple_control"
diff --git a/python/alternative_packaging_BROKEN_atm/setup.cfg b/python/alternative_packaging_BROKEN_atm/setup.cfg
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/python/examples/clik_new.py b/python/examples/clik.py
similarity index 100%
rename from python/examples/clik_new.py
rename to python/examples/clik.py
diff --git a/python/examples/fts.png b/python/examples/data/fts.png
similarity index 100%
rename from python/examples/fts.png
rename to python/examples/data/fts.png
diff --git a/python/examples/joint_trajectory.csv b/python/examples/data/joint_trajectory.csv
similarity index 100%
rename from python/examples/joint_trajectory.csv
rename to python/examples/data/joint_trajectory.csv
diff --git a/python/examples/path_in_pixels.csv b/python/examples/data/path_in_pixels.csv
similarity index 100%
rename from python/examples/path_in_pixels.csv
rename to python/examples/data/path_in_pixels.csv
diff --git a/python/examples/fixes_for_demo.md b/python/examples/fixes_for_demo.md
deleted file mode 100644
index 6a80169b6c6d89dd46f8b44f67efca0afb5851e1..0000000000000000000000000000000000000000
--- a/python/examples/fixes_for_demo.md
+++ /dev/null
@@ -1,16 +0,0 @@
-THERE WAS A NORM IN THE ERROR TERM IN MOVEJ THAT'S WHY IT KEPT GOING FORWARD
-GOD DAMN IT
-thank you jesus for showing me that one, good lord thank you
-1. i'm pretty sure why it goes so far with movej is because you gave it a tight error.
- well, give it a lax error then. atm there is a single instance where you're running this command,
- so it's fine, you cares. bottom line is, be above the starting point, and then go down with
- dmp which has the force feedback and will ease into it.
-2. throw the low-pass filter away, rock the moving average, that's what worked.
-3. if it will give you trouble, just remove it. write on the bord, that's precise enough.
-4. put in new the new translation and rot mat into old code.
- replace all (BUT PATH) 2's with 1's to go from the z-axis into the y-axis.
-
-one of these fixes the problem instantly.
-fuck the tuning for the most part, it's mostly ok.
-feel free to play around with it AFTER IT WORKS.
-but whatever is in there now is not what makes the difference.
diff --git a/python/examples/gepetto_ex.py b/python/examples/gepetto_ex.py
index 3e5978759cd8fa6882fd269ebf0288c0d6830b3b..3d89c53db0afdb6ede2469397574cdd8474c7991 100644
--- a/python/examples/gepetto_ex.py
+++ b/python/examples/gepetto_ex.py
@@ -1,3 +1,8 @@
+# TODO:
+# - get python gepetto viz to work
+# - put that into robot manager
+# - delete this file afterward
+
import pinocchio as pin
import numpy as np
import sys
diff --git a/python/examples/lil_test.py b/python/examples/lil_test.py
deleted file mode 100644
index aacf81da6869c74d90604442c356fe5c3a3d6aa9..0000000000000000000000000000000000000000
--- a/python/examples/lil_test.py
+++ /dev/null
@@ -1,343 +0,0 @@
-import pinocchio as pin
-import numpy as np
-import matplotlib.pyplot as plt
-import copy
-import argparse
-import time
-from functools import partial
-from ur_simple_control.util.get_model import get_model
-from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.util.draw_path import drawPath
-from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
-# TODO merge these clik files as well, they don't deserve to be separate
-# TODO but first you need to clean up clik.py as specified there
-from ur_simple_control.clik.clik_point_to_point import getClikController, moveL, moveUntilContact
-from ur_simple_control.clik.clik_trajectory_following import map2DPathTo3DPlane, clikCartesianPathIntoJointPath
-from ur_simple_control.managers import ControlLoopManager, RobotManager
-from ur_simple_control.util.calib_board_hacks import calibratePlane, getSpeedInDirectionOfN
-from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.basics.basics import moveJ
-
-#######################################################################
-# arguments #
-#######################################################################
-
-def calibrateFT(robot):
- ft_readings = []
- for i in range(2000):
- start = time.time()
- q = robot.rtde_receive.getActualQ()
- ft = robot.rtde_receive.getActualTCPForce()
- tau = robot.rtde_control.getJointTorques()
- current = robot.rtde_receive.getActualCurrent()
- #print("current", current)
- #print("getActualTCPForce", ft)
- #print("tau", tau)
- ft_readings.append(ft)
- end = time.time()
- diff = end - start
- if diff < robot.dt:
- time.sleep(robot.dt - diff)
-
- ft_readings = np.array(ft_readings)
- avg = np.average(ft_readings, axis=0)
- print("average ft time", avg)
- return avg
-
-def getArgs():
- #######################################################################
- # generic arguments #
- #######################################################################
- parser = argparse.ArgumentParser(description='Make a drawing on screen,\
- watch the robot do it on the whiteboard.',
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- # TODO this one won't really work but let's leave it here for the future
- parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator. \
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio.\
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize with gepetto, but \
- NOTE: not implemented yet", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
- parser.add_argument('--acceleration', type=float, \
- help="robot's joints acceleration. scalar positive constant, \
- max 1.7, and default 0.4. \
- BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
- TODO: check what this means", default=0.3)
- parser.add_argument('--speed-slider', type=float,\
- help="cap robot's speed with the speed slider \
- to something between 0 and 1, 1.0 by default because for dmp. \
- BE CAREFUL WITH THIS.", default=1.0)
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
- #######################################################################
- # your controller specific arguments #
- #######################################################################
- # not applicable here, but leaving it in the case it becomes applicable
- # it's also in the robot manager even though it shouldn't be
- parser.add_argument('--past-window-size', type=int, \
- help="how many timesteps of past data you want to save", default=5)
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with. NOTE: not used here", \
- default=1e-3)
- # TODO: test the interaction of this and the overall demo
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tiknohov regularization.\
- This is used when generating the joint trajectory from the drawing.", \
- default=1e-2)
- # TODO add the rest
- parser.add_argument('--clik-controller', type=str, \
- help="select which click algorithm you want", \
- default='dampedPseudoinverse', \
- choices=['dampedPseudoinverse', 'jacobianTranspose'])
- # maybe you want to scale the control signal
- parser.add_argument('--controller-speed-scaling', type=float, \
- default='1.0', help='not actually_used atm')
- #############################
- # dmp specific arguments #
- #############################
- parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
- help="whether you want to use temporal coupling", default=True)
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
- parser.add_argument('--kv', type=float, \
- help="damping in impedance control", \
- default=0.001)
- parser.add_argument('--tau0', type=float, \
- help="total time needed for trajectory. if you use temporal coupling,\
- you can still follow the path even if it's too fast", \
- default=5)
- parser.add_argument('--gamma-nominal', type=float, \
- help="positive constant for tuning temporal coupling: the higher,\
- the fast the return rate to nominal tau", \
- default=1.0)
- parser.add_argument('--gamma-a', type=float, \
- help="positive constant for tuning temporal coupling, potential term", \
- default=0.5)
- parser.add_argument('--eps-tc', type=float, \
- help="temporal coupling term, should be small", \
- default=0.001)
- parser.add_argument('--alpha', type=float, \
- help="force feedback proportional coefficient", \
- default=0.01)
- # TODO add low pass filtering and make it's parameters arguments too
- #######################################################################
- # task specific arguments #
- #######################################################################
- # TODO measure this for the new board
- parser.add_argument('--board-width', type=float, \
- help="width of the board (in meters) the robot will write on", \
- default=0.3)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- default=0.3)
- parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
- help="whether you want to do calibration", default=False)
- parser.add_argument('--draw-new', action=argparse.BooleanOptionalAction, \
- help="whether draw a new picture, or use the saved path path_in_pixels.csv", default=True)
- parser.add_argument('--pick_up_marker', action=argparse.BooleanOptionalAction, \
- help="""
- whether the robot should pick up the marker.
- NOTE: THIS IS FROM A PREDEFINED LOCATION.
- """, default=True)
- parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
- help="""
- whether you want to do find marker offset (recalculate TCP
- based on the marker""", default=False)
- parser.add_argument('--n-calibration-tests', type=int, \
- help="number of calibration tests you want to run", default=10)
- parser.add_argument('--clik-goal-error', type=float, \
- help="the clik error you are happy with", default=1e-2)
- parser.add_argument('--max-init-clik-iterations', type=int, \
- help="number of max clik iterations to get to the first point", default=10000)
- parser.add_argument('--max-running-clik-iterations', type=int, \
- help="number of max clik iterations between path points", default=1000)
-
- args = parser.parse_args()
- if args.gripper and args.simulation:
- raise NotImplementedError('Did not figure out how to put the gripper in \
- the simulation yet, sorry :/ . You can have only 1 these flags right now')
- return args
-
-
-# go and pick up the marker
-def getMarker(q_init):
- pass
-
-
-#######################################################################
-# control loop #
-#######################################################################
-
-# feedforward velocity, feedback position and force for impedance
-def controller():
- pass
-
-# TODO:
-# regarding saving data you have 2 options:
-# 1) explicitely return what you want to save - you can't magically read local variables
-# 2) make controlLoop a class and then save handle the saving behind the scenes -
-# now you these variables are saved in a class so they're not local variables
-# option 1 is clearly more hands-on and thus worse
-# option 2 introduces a third big class and makes everything convoluted.
-# for now, we go for option 1) because it's simpler to implement and deal with.
-# but in the future, implementing 2) should be tried. you really have to try
-# to do it cleanly to see how good/bad it is.
-# in that case you'd most likely want to inherit ControlLoopManager and go from there.
-# you'd still need to specify what you're saving with self.that_variable so no matter
-# there's no running away from that in any case.
-# it's 1) for now, it's the only non-idealy-clean part of this solution, and it's ok really.
-# TODO but also look into something fancy like some decorator or something and try
-# to find option 3)
-
-# control loop to be passed to ControlLoopManager
-def controlLoopPointImpedance(wrench_offset, q_init, controller, robot, i, past_data):
- breakFlag = False
- # TODO rename this into something less confusing
- save_past_dict = {}
- log_item = {}
- q = robot.getQ()
- # TODO look into UR code/api for estimating the same
- # based on currents in the joints.
- # it's probably worse, but maybe some sensor fusion-type thing
- # is actually better, who knows.
- # also you probably want to do the fusion of that onto tau (got from J.T @ wrench)
- # evil hack because wrench is not zeros (why? - no idea whatsoever)
- wrench = robot.getWrench()
- log_item['wrench_raw'] = wrench.reshape((6,))
- wrench = wrench - wrench_offset
- # deepcopy for good coding practise (and correctness here)
- save_past_dict['wrench'] = copy.deepcopy(wrench)
- # rolling average
- #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
- 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]))
- #Z = np.diag(np.array([1.0, 1.0, 0.1, 1.0, 1.0, 1.0]))
- #Z = np.diag(np.ones(6))
- copy_wrench = copy.deepcopy(wrench)
-# wrench[1] =
- #Z = np.diag(np.array([0.1, 0.1, 1.0, 0.1, 0.1, 0.1]))
- Z = np.diag(np.array(np.ones(6)))
- #Z = np.diag(np.array([0.0, 1.0, 0.0, 0.0, 0.0, 0.0]))
- #Z = np.diag(np.array([1.0, 0.6, 1.0, 0.5, 0.5, 0.5]))
- # MAP WRENCH TO TCP FRAME
-# print("before", wrench)
- # good idea, bad execution 1
- # this one makes most sense
- # wrench = robot.getMtool().toDualActionMatrix() @ wrench
- # if it aint that then it could be
- wrench = robot.getMtool().toDualActionMatrix().T @ wrench
- # and if it ain't that then it could be
- #wrench = robot.getMtool().inverse().toDualActionMatrix() @ wrench
- # this one is correct if i'm readying the book right
- #wrench = robot.getMtool().inverse().toDualActionMatrix().T @ wrench
-
- if i % 100 == 0:
- print("after: ", end="")
- for w in wrench:
- if w < 0:
- print(np.round(w, 3), end=", ")
- else:
- print(np.round(w, 3), end=', ')
- print("")
- wrench = Z @ wrench
-# print("after", wrench)
-# if i % 100 == 0:
-# for w in wrench:
-# if w < 0:
-# print(np.round(w, 3), end=", ")
-# else:
-# print(np.round(w, 3), end=', ')
-# print("")
- #pin.forwardKinematics(robot.model, robot.data, q)
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
- dq = robot.getQd()[:6].reshape((6,1))
- # get joint
- tau = J.T @ wrench
- tau = tau[:6].reshape((6,1))
- # compute control law:
- # - feedback the position
- #print(q_init[:6])
- #print(q[:6])
- #print(args.alpha * tau)
- vel_cmd = args.kp * (q_init[:6].reshape((6,1)) - q[:6].reshape((6,1))) + args.alpha * tau - args.kv * dq
- #print(vel_cmd)
- robot.sendQd(vel_cmd)
-
- # immediatelly stop if something weird happened (some non-convergence)
- if np.isnan(vel_cmd[0]):
- breakFlag = True
-
- # log what you said you'd log
- # TODO fix the q6 situation (hide this)
- log_item['qs'] = q[:6].reshape((6,))
- log_item['dqs'] = dq.reshape((6,))
- log_item['wrench_used'] = wrench.reshape((6,))
-
- return breakFlag, save_past_dict, log_item
-
-if __name__ == "__main__":
- #######################################################################
- # software setup #
- #######################################################################
- args = getArgs()
- clikController = getClikController(args)
- robot = RobotManager(args)
- #print("ur_tcp", robot.rtde_receive.getActualTCPPose())
- #print("my_tcp", robot.getMtool().translation, pin.rpy.matrixToRpy(robot.getMtool().rotation))
- #exit()
-
- # calibrate FT first
- wrench_offset = calibrateFT(robot)
-
- # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
- # you already found an API in rtde_control for this, just put it in initialization
- # under using/not-using gripper parameters
- # ALSO NOTE: to use this you need to change the version inclusions in
- # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
- # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
- # here you give what you're saving in the rolling past window
- # it's initial value.
- # controlLoopManager will populate the queue with these initial values
- save_past_dict = {
- 'wrench' : np.zeros(6),
- }
- # here you give it it's initial value
- log_dict = {
- 'qs' : np.zeros((args.max_iterations, 6)),
- 'dqs' : np.zeros((args.max_iterations, 6)),
- 'wrench_raw' : np.zeros((args.max_iterations, 6)),
- 'wrench_used' : np.zeros((args.max_iterations, 6)),
- }
- q_init = robot.getQ()
-
- controlLoop = partial(controlLoopPointImpedance, wrench_offset, q_init, controller, robot)
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
-
- #moveJ(args, robot, dmp.pos.reshape((6,)))
- # and now we can actually run
- loop_manager.run()
-
- for i in range(300):
- vel_cmd = np.zeros(6)
- self.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
- #plotFromDict(log_dict, args)
- # plot results
- plotFromDict(log_dict, args)
- robot.stopHandler(None, None)
- # TODO: add some math to analyze path precision
-
-
-
-
diff --git a/python/examples/clik_old.py b/python/examples/old_or_experimental/clik_old.py
similarity index 100%
rename from python/examples/clik_old.py
rename to python/examples/old_or_experimental/clik_old.py
diff --git a/python/examples/force_mode_api.py b/python/examples/old_or_experimental/force_mode_api.py
similarity index 100%
rename from python/examples/force_mode_api.py
rename to python/examples/old_or_experimental/force_mode_api.py
diff --git a/python/examples/forcemode_example.py b/python/examples/old_or_experimental/forcemode_example.py
similarity index 100%
rename from python/examples/forcemode_example.py
rename to python/examples/old_or_experimental/forcemode_example.py
diff --git a/python/examples/test_traj_w_movej.py b/python/examples/old_or_experimental/test_traj_w_movej.py
similarity index 100%
rename from python/examples/test_traj_w_movej.py
rename to python/examples/old_or_experimental/test_traj_w_movej.py
diff --git a/python/examples/test_traj_w_speedj.py b/python/examples/old_or_experimental/test_traj_w_speedj.py
similarity index 100%
rename from python/examples/test_traj_w_speedj.py
rename to python/examples/old_or_experimental/test_traj_w_speedj.py
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index a084ace3ebdacf4dbba9ee341439093540bbfe50..461d155d192cf4d32be0723186875bde276e7c8e 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.001
+ beta = 0.1
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/fts.png b/python/fts.png
deleted file mode 100644
index 84adc854bf5f7081849a0b865471aa193eff2f33..0000000000000000000000000000000000000000
Binary files a/python/fts.png and /dev/null differ
diff --git a/python/initial_python_solution/.manipulator_visual_motion_analyzer.py.swp b/python/initial_python_solution/.manipulator_visual_motion_analyzer.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..12b2cf4aa8c3ea2f063d13812214aa29f1ac0189
Binary files /dev/null and b/python/initial_python_solution/.manipulator_visual_motion_analyzer.py.swp differ
diff --git a/python/initial_python_solution/blit_test.py b/python/initial_python_solution/blit_test.py
new file mode 100644
index 0000000000000000000000000000000000000000..d0a97298e9a823b5f94348b10c782f696def9b47
--- /dev/null
+++ b/python/initial_python_solution/blit_test.py
@@ -0,0 +1,45 @@
+import matplotlib.pyplot as plt
+import numpy as np
+
+x = np.linspace(0, 2 * np.pi, 100)
+
+fig, ax = plt.subplots()
+
+# animated=True tells matplotlib to only draw the artist when we
+# explicitly request it
+(ln,) = ax.plot(x, np.sin(x), animated=True)
+
+# make sure the window is raised, but the script keeps going
+plt.show(block=False)
+
+# stop to admire our empty window axes and ensure it is rendered at
+# least once.
+#
+# We need to fully draw the figure at its final size on the screen
+# before we continue on so that :
+# a) we have the correctly sized and drawn background to grab
+# b) we have a cached renderer so that ``ax.draw_artist`` works
+# so we spin the event loop to let the backend process any pending operations
+plt.pause(0.1)
+
+# get copy of entire figure (everything inside fig.bbox) sans animated artist
+bg = fig.canvas.copy_from_bbox(fig.bbox)
+# draw the animated artist, this uses a cached renderer
+ax.draw_artist(ln)
+# show the result to the screen, this pushes the updated RGBA buffer from the
+# renderer to the GUI framework so you can see it
+fig.canvas.blit(fig.bbox)
+
+for j in range(100):
+ # reset the background back in the canvas state, screen unchanged
+ fig.canvas.restore_region(bg)
+ # update the artist, neither the canvas state nor the screen have changed
+ ln.set_ydata(np.sin(x + (j / 100) * np.pi))
+ # re-render the artist, updating the canvas state, but not the screen
+ ax.draw_artist(ln)
+ # copy the image to the GUI state, but screen might not be changed yet
+ fig.canvas.blit(fig.bbox)
+ # flush any pending GUI events, re-painting the screen if needed
+ fig.canvas.flush_events()
+ # you can put a pause in if you want to slow things down
+ # plt.pause(.1)
diff --git a/python/initial_python_solution/main.py b/python/initial_python_solution/main.py
index 9a5c499a9765393f3d5bf1bcbdc86a1e3af666cd..19b36264102f5a3a6e5d31bc079d1cee1815c609 100644
--- a/python/initial_python_solution/main.py
+++ b/python/initial_python_solution/main.py
@@ -6,13 +6,16 @@ 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("192.168.1.102")
+rtde_c = RTDEControl("127.0.0.1")
-rtde_r = RTDEReceive("192.168.1.102")
+#rtde_r = RTDEReceive("192.168.1.102")
+rtde_r = RTDEReceive("127.0.0.1")
q = rtde_r.getActualQ()
# Parameters
@@ -21,9 +24,9 @@ dt = 1.0/500 # 2ms
ik_env = InverseKinematicsEnv()
ik_env.damping = 25
-ik_env.goal[0] = 0.21
-ik_env.goal[1] = 0.38
-ik_env.goal[2] = 0.5
+ik_env.goal[0] = -0.21
+ik_env.goal[1] = -0.38
+ik_env.goal[2] = 0.2
ik_env.render()
@@ -38,6 +41,7 @@ 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()
@@ -58,6 +62,9 @@ while True:
# 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)
diff --git a/python/initial_python_solution/manipulator_visual_motion_analyzer.py b/python/initial_python_solution/manipulator_visual_motion_analyzer.py
index d15e61d80ffc2127980461aa9797db2920576bc0..3cea2701b7776da660e13f6492dca66e2dc9aeb3 100644
--- a/python/initial_python_solution/manipulator_visual_motion_analyzer.py
+++ b/python/initial_python_solution/manipulator_visual_motion_analyzer.py
@@ -19,6 +19,9 @@ from make_run import makeRun
import numpy as np
import sys
+# don't want to refactor yet, but obv
+# TODO: refactor, have normal names for things
+import time as ttime
sys.path.append('../geometry_matplotlib')
#from common import exit_on_x, make_rotation_matrix3D
@@ -83,8 +86,10 @@ ik_env.data.append(makeRun(controller2, ik_env, n_iters, 1))
screensize = pyautogui.size()
SCREEN_WIDTH = screensize.width
SCREEN_HEIGHT = screensize.height
-SCALING_FACTOR_HEIGHT = 0.8
-SCALING_FACTOR_WIDTH = 0.5
+#SCALING_FACTOR_HEIGHT = 0.8
+#SCALING_FACTOR_WIDTH = 0.5
+SCALING_FACTOR_HEIGHT = 0.3
+SCALING_FACTOR_WIDTH = 0.3
#######################################################################
# YARA'S PLOT #
@@ -288,6 +293,8 @@ notebook_right.grid(row=0, column=1, sticky='ew')
# tkinterize these plots
canvas_manipulator = FigureCanvasTkAgg(fig_manipulator, master=frame_manipulator)
+# NEW
+background_manipulator = canvas_manipulator.copy_from_bbox(fig_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)
@@ -341,6 +348,8 @@ toolbar_imu.grid(column=0, row=2)
def update_points(new_val):
# ee plot
###########################################################################
+ start = ttime.time()
+ canvas_manipulator.restore_region(background_manipulator)
index = int(np.floor(float(new_val)))
# Update 3D reconstruction plot
@@ -396,11 +405,17 @@ def update_points(new_val):
ik_env.p_e_point_plots[robot_index].set_data([ik_env.data[robot_index]['p_es'][index][0]], [ik_env.data[robot_index]['p_es'][index][1]])
ik_env.p_e_point_plots[robot_index].set_3d_properties([ik_env.data[robot_index]['p_es'][index][2]])
canvas_ee.draw()
- canvas_manipulator.draw()
+ #canvas_manipulator.draw()
+ # NEW
+ # 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):
diff --git a/python/initial_python_solution/robot_stuff/.InverseKinematics.py.swp b/python/initial_python_solution/robot_stuff/.InverseKinematics.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..a9ec7ce2e024e479f6e20b5e8b5460644f0e31c2
Binary files /dev/null and b/python/initial_python_solution/robot_stuff/.InverseKinematics.py.swp differ
diff --git a/python/initial_python_solution/robot_stuff/InverseKinematics.py b/python/initial_python_solution/robot_stuff/InverseKinematics.py
index 71c50f5f107f46cddff2c88e1b9a0c7f50eb4dff..8cbe712bd58bf4ef3b8e7dd4251d6192d8464bcc 100644
--- a/python/initial_python_solution/robot_stuff/InverseKinematics.py
+++ b/python/initial_python_solution/robot_stuff/InverseKinematics.py
@@ -23,6 +23,7 @@ class InverseKinematicsEnv:
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
@@ -158,7 +159,10 @@ class InverseKinematicsEnv:
self.robot.drawStateAnim()
self.ax.set_title(str(self.n_of_tries_for_point) + 'th iteration toward goal')
drawPoint(self.ax, self.goal, 'red', 'o')
- self.fig.canvas.draw()
+ # if no draw it is kinda faster
+ #self.fig.canvas.draw()
+ # this is even faster
+ self.fig.canvas.update()
self.fig.canvas.flush_events()
diff --git a/python/initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc b/python/initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
index cac3d1c8e224c623351da026775d35ef4b973cbb..47e56460692494af368d10f7e35571c312e88590 100644
Binary files a/python/initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc and b/python/initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index bf343fb9824b543b7be07c2f7b832527f91aa3e2..24de3cbd1f605849db1670e76ac3f2d16f4d86a4 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/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 7d999ad8057d0d469193fda6411d1d7bebe45b68..82a3796c32c3794fe89d1fe2e5a6baf7df4193bc 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -41,6 +41,10 @@ def moveJControlLoop(q_desired, robot, i, past_data):
robot.sendQd(qd)
return breakFlag, {}, {}
+# TODO:
+# fix this by tuning or whatever else.
+# MOVEL works just fine, so apply whatever's missing for there here
+# and that's it.
def moveJ(args, robot, q_desired):
assert type(q_desired) == np.ndarray
controlLoop = partial(moveJControlLoop, q_desired, robot)
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 b9fde457d3263e808aa5850664e9db5bd513f0cf..dd38d4802f19ea27099acff0a3f394579a099c10 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 afe07d315707b12348f0ed3726a816b9a3e8208b..0d00128461b12a79c598dac57d357e8a2c65afb3 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -6,8 +6,6 @@ from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
import time
-
-
"""
Every imaginable magic number, flag and setting is put in here.
This way the rest of the code is clean.
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index cf25427a54257f96ae8bd0e14069bae3483b8ff6..99b082c78a14545b8acabcedb3cac47525b4fc5f 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -139,6 +139,7 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
joint_trajectory = np.hstack((t, qs))
# TODO handle saving more consistently/intentionally
# (although this definitely works right now and isn't bad, just mid)
+ # os.makedir -p a data dir and save there, this is ugly
np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
return joint_trajectory
diff --git a/python/ur_simple_control/dmp/run_dmp.py b/python/ur_simple_control/dmp/run_dmp.py
deleted file mode 100644
index 7c9f7c4204748bc9b124d199b67304178e24a1c0..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/dmp/run_dmp.py
+++ /dev/null
@@ -1,277 +0,0 @@
-# TODO:
-# 1. clean up these instantiations in a separate util file
-# and just do a line-liner importing -> need to make this a package to get rid
-# of the relative-path imports
-# 2. delete the unnecessary comments
-# 3. figure out how to scale the dmp velocity when creating it -> DONE
-# 4. parametrize everything, put argparse with defaults in a separate file
-# like in tianshou examples. make magic numbers arguments!
-# also add helpfull error messages based on this, ex. if interfaces couldn't connect
-# with the simulation argument on, write out "check whether you started the simulator"
-# 5. remove all unused code:
-# - for stuff that could work, make a separate file
-# - just remove stuff that's a remnant of past tries
-# 6. put visualization into a separate file for visualization
-# 7. put an actual low-pass filter over the force-torque sensor
-# 8. add some code to pick up the marker from a prespecified location
-# 9. add the (optional) decomposition of force feedback so that
-# you get hybrid motion-force control
-# 10. write documentation as you go along
-# 11. put gripper class in util or something,
-# package it and then have it one place instead of copying it everywhere
-# 12. make this a mainfile, put everything that could be turned into a function elsewhere
-
-from create_dmp import DMP
-from rtde_control import RTDEControlInterface
-from rtde_receive import RTDEReceiveInterface
-from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
-from temporal_coupling import NoTC, TCVelAccConstrained
-import pinocchio as pin
-import numpy as np
-import os
-import time
-import signal
-import matplotlib.pyplot as plt
-import sys
-import copy
-sys.path.insert(0, '../../util')
-from give_me_the_calibrated_model import get_model
-
-# create models
-# TODO make a argument for loading/not loading and running gepetto visualization
-urdf_path_relative = "../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
-urdf_path_absolute = os.path.abspath(urdf_path_relative)
-mesh_dir = "../../robot_descriptions/"
-mesh_dir_absolute = os.path.abspath(mesh_dir)
-model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
-
-# create and connect robot communication interfaces
-rtde_control = RTDEControlInterface("192.168.1.102")
-rtde_receive = RTDEReceiveInterface("192.168.1.102")
-rtde_io = RTDEIOInterface("192.168.1.102")
-# on scale from 0 to 1 set the maximum allowed speed on the robot.
-# it's a global thing on the robot
-# TODO make this an argument
-speed_slider = 0.7
-rtde_io.setSpeedSlider(speed_slider)
-
-# TODO add a use gripper argument
-# run if marker isn't gripped
-#gripper = RobotiqGripper()
-#gripper.connect("192.168.1.102", 63352)
-#gripper.activate()
-#time.sleep(3)
-#gripper.move(255,100,100)
-#time.sleep(3)
-
-# TODO create a simulation argument!
-#rtde_control = RTDEControl("127.0.0.1")
-#rtde_receive = RTDEReceive("127.0.0.1")
-
-# TODO make N_iter and argument
-N_ITER = 10000
-# TODO gather all vector memory allocation in one place
-# allocate memory for vectors (use numpy arrays instead of default python linked lists)
-qs = np.zeros((N_ITER, 6))
-dmp_poss = np.zeros((N_ITER, 6))
-dqs = np.zeros((N_ITER, 6))
-dmp_vels = np.zeros((N_ITER, 6))
-
-# TODO move to util file
-# catch SIGINT
-def handler(signum, frame):
- print('sending 100 speedjs full of zeros')
- for i in range(100):
- vel_cmd = np.zeros(6)
- rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
-# TODO make a separate visualization file and put this in a function there
- print('also plotting')
- t = np.arange(N_ITER)
- ax_q = plt.subplot(221)
- ax_dq = plt.subplot(222)
- ax_dmp_q = plt.subplot(223)
- ax_dmp_dq = plt.subplot(224)
- for i in range(5):
- ax_q.plot(t, qs[:,i], color='blue')
- ax_dq.plot(t, dqs[:,i], color='red')
- ax_dmp_q.plot(t, dmp_poss[:,i], color='green')
- ax_dmp_dq.plot(t, dmp_vels[:,i], color='orange')
- # for labels
- ax_q.plot(t, qs[:,5], color='blue', label='q')
- ax_q.legend()
- ax_dq.plot(t, dqs[:,5], color='red', label='dq')
- ax_dq.legend()
- ax_dmp_q.plot(t, dmp_poss[:,5], color='green', label='dmp_q')
- ax_dmp_q.legend()
- ax_dmp_dq.plot(t, dmp_vels[:,5], color='orange', label='dmp_dq')
- ax_dmp_dq.legend()
- plt.show()
- exit()
-# if not rtde_control.isProgramRunning():
-# exit()
-
-# catch SIGINT
-signal.signal(signal.SIGINT, handler)
-
-#######################################################################
-# initalization #
-#######################################################################
-
-# load trajectory, create DMP based on it
-dmp = DMP()
-# get up from the board
-current_pose = rtde_receive.getActualTCPPose()
-current_pose[2] = current_pose[2] + 0.03
-rtde_control.moveL(current_pose)
-# move to initial pose
-# this is a blocking call
-rtde_control.moveJ(dmp.pos.reshape((6,)))
-
-# TODO make all these arguments
-TEMPORAL_COUPLING = True
-update_rate = 500
-# TODO calculate this when setting default arguments
-dt = 1.0 / update_rate
-JOINT_ID = 6
-
-# ========================================================================
-# TODO: either write a separate file where this is used
-# ---> TODO write a separate file where you're testing this out
-# or just delete it
-task_frame = [0, 0, 0, 0, 0, 0]
-# these are in {0,1} and select which task frame direction compliance is active in
-# just be soft everywhere
-selection_vector = [1, 1, 1, 1, 1, 1]
-# the wrench applied to the environment:
-# position is adjusted to achieve the specified wrench
-# let's pretend speedjs are this and see what happens (idk honestly)
-wrench = [0, 0, 0, 0, 0, 0]
-ftype = 2
-# limits for:
-# - compliant axes: highest tcp velocities allowed on compliant axes
-# - non-compliant axes: maximum tcp position error compared to the program (which prg,
-# and where is this set?)
-# why these values?
-limits = [2, 2, 2, 2, 2, 2]
-# ========================================================================
-
-
-
-# parameters from yaml config file in albin's repo
-# TODO: make these arguments
-kp = 2
-acceleration = 1.7
-#acceleration = 0.5
-
-if not TEMPORAL_COUPLING:
- tc = NoTC()
-else:
- # TODO learn the math already
- # TODO MAKE THESE ARGUMENTS
- tau0 = 5 # total time required for trajectory
- gamma_nominal = 1.0 # positive constant for tuning temporal coupling,
- # the higher, the fast the return rate to nominal tau
- gamma_a = 0.5 # positive constant for tuning temporal coupling,
- # potential term
- eps = 0.001 # temporal coupling term, should be small
- # TODO test whether this works (it should, but test it)
- v_max = np.ones(6) * 2 * speed_slider
- a_max = np.ones(6) * 1.7
- tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
-
-# TODO make this an argument
-alpha = 0.003
-wrench_avg = np.zeros((5,6))
-
-
-#######################################################################
-# control loop #
-#######################################################################
-
-
-# TODO make a wrapper function for timing
-# make the rest of the for a function and put it in the wrapper
-# then run that in the for
-print("starting the trajectory")
-for i in range(N_ITER):
- start = time.time()
- # dmp
- dmp.step(dt)
- # temporal coupling
- tau = dmp.tau + tc.update(dmp, dt) * dt
- dmp.set_tau(tau)
- q = rtde_receive.getActualQ()
- # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
- # you already found an API in rtde_control for this, just put it in initialization
- # under using/not-using gripper parameters
- # TODO: document the mathematics with one-line comments
- wrench = np.array(rtde_receive.getActualTCPForce())
- # rolling average
- # TODO: try doing a low-pass filter instead
- wrench_avg[i % 5] = wrench
- wrench = np.average(wrench_avg, axis=0)
- # pinocchio model is 8-dof because it treats the gripper
- # as two prismatic joints
- q6 = np.array(copy.deepcopy(q))
- q.append(0.0)
- q.append(0.0)
- q = np.array(q)
- pin.forwardKinematics(model, data, q)
- # calculate joint torques based on the force-torque sensor
- # TODO look into UR code/api for estimating the same
- # based on currents in the joints.
- # it's probably worse, but maybe some sensor fusion-type thing
- # is actually better, who knows
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- dq = np.array(rtde_receive.getActualQd()).reshape((6,1))
- tau = J.T @ wrench
- tau = tau[:6].reshape((6,1))
- # compute control law:
- # - feedforward the velocity and the force reading
- # - feedback the position
- vel_cmd = dmp.vel + kp * (dmp.pos - q6.reshape((6,1))) - alpha * tau
- vel_cmd = vel_cmd.reshape((6,))
- # just in case, clip the velocity to max values
- vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
- # and immediatelly stop if something weird happened (some non-convergence)
- if np.isnan(vel_cmd[0]):
- break
- rtde_control.speedJ(vel_cmd, acceleration, dt)
- # TODO: move to existing force mode API testing file
- # this is very stupind, but it was quick to implement
- #vel_cmd8 = list(vel_cmd)
- #vel_cmd8.append(0.0)
- #vel_cmd8.append(0.0)
- #vel_cmd8 = np.array(vel_cmd8)
- #vel_tcp = J @ vel_cmd8
- #vel_tcp = vel_tcp * 10
- #rtde_control.forceMode(task_frame, selection_vector, vel_tcp, ftype, limits)
-
- # save values for the plot
- # TODO cut off at the last time instant?
- qs[i] = q6.reshape((6,))
- dmp_poss[i] = dmp.pos.reshape((6,))
- dqs[i] = dq.reshape((6,))
- dmp_vels[i] = dmp.vel.reshape((6,))
- # TODO make start-end times a function wrapper or something,
- # so that it is in every controller for-loop by default
- # (or leave it here, whatever)
- end = time.time()
- diff = end - start
- if dt < diff:
- print("missed deadline by", diff - dt)
- continue
- else:
- time.sleep(dt - diff)
- # TODO do this with something sensible
- if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
- break
-
-# call the handler to stop movement
-handler(None, None)
-
- #print(vel_cmd[0][0])
-
-
-
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 72d493f4e85f3f88fcbcac10446e99259ab05291..05f3ec926c5913e839ab0446529ceef5763a607d 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -15,7 +15,6 @@ from rtde_io import RTDEIOInterface
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import copy
import signal
-# TODO make the package work
from ur_simple_control.util.get_model import get_model
from collections import deque
@@ -44,6 +43,11 @@ The point of these managers is to handle:
current state
-------------
Everything is UR specific.
+In the process of removing everything that isn't a speedJ command
+from the interface though.
+After that point, it will be immediatelly trasferable if you can
+give me a robot with a speedJ command
+and a correct urdf model of it (so that I can load it into pinocchio).
long term vision
-------------------
@@ -61,38 +65,46 @@ It's a class because it keeps non-directly-control-loop-related parameters
like max_iterations, what data to save etc.
NOTE: you give this the ready-made control loop.
if it has arguments, bake them in with functools.partial.
+Handles short-term data saving and logging.
+Details on this are given below.
+
+Short term data saving:
+ - it's a dictionaries of deques (initialized here), because deque is the most convenient class
+ for removing the first element and appending a last (it is just a linked list under the hood of course).
+ - it's a dictionary for modularity's sake, because this way you can save whatever you want
+ - and it will just work based on dictionary keys.
+ - it is the user's resposibility to make sure they're providing correct data.
+ - --> TODO but make an assert for the keys at least
+ - in the c++ imlementation, make the user write their own struct or something.
+ - since this is python, you need to give me initial values to infer types.
+ - you need to provide initial values to populate the deque to start anyway.
+
+Logging data (for analysis and plotting):
+ - it can only be handled here because the control loop itself only cares about the present/
+ a small time-window around it.
+ - saves it all in a dictionary of ndarrays (initialized here), returns that after a run
+ TODO: it's provided by the user now, make it actually initialize here!!!
+ - you need to specify which keys you're using to do the initialization
+ - later, the controlLoop needs to return what's to be save in a small temporary dict.
+ - NOTE: this is of course a somewhat redundant solution, but it's the simplest
+ and most flexible way of doing it.
+ it probably will be done some other way down the line, but it works and is not
+ a priority right now
+
+Other info:
+- relies on RobotManager to handle all the magic numbers
+ that are not truly only control loop related
+
+
"""
class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
self.max_iterations = args.max_iterations
- # i need rtde_control do to rtde_control
- # TODO this thing might be re-initializing the robot
+ # NOTE the robot manager is all over the place,
+ # there might be a better design approach to that
self.robot_manager = robot_manager
self.controlLoop = controlLoop
self.args = args
- # predefined ur magic numbers
- self.update_rate = 500
- self.dt = 1 / self.update_rate
- # TODO handle data saving here.
- # it can only be handled here because the control loop only cares about the present/
- # a small time-window around it.
- # of course have this under the save_plots flag or something similar
- # save it all in a dictionary of ndarrays and return that
- # these are all the same (mostly).
- # if they're not consider inheriting and specializing this or sth similar,
- # we'll see when we get to it
-
- # it's a dictionaries of deque because deque is the most convenient class to have for removing the first
- # element and appending a last (it is just a linked list under the hood of course).
- # it's a dictionary for modularity's sake, because this way you can save whatever you want
- # and it will just work based on dictionary keys.
- # it is the user's resposibility to make sure they're providing correct data.
- # --> but make an assert for the keys at least
- # in the c++ imlementation, make the user write their own struct or something.
- # TODO: figure out how to populate it initially
- # you have the length from the arguments, but you also need the keys.
- # since those won't be changed, it doesn't make sense to have them as program arguments.
- # --> make them arguments of this class, there is no better solution
self.past_data = {}
# save_past_dict has to have the key and 1 example of what you're saving
# so that it's type can be inferred (but we're in python so types don't really work).
@@ -144,11 +156,11 @@ class ControlLoopManager:
break
end = time.time()
diff = end - start
- if self.dt < diff:
- print("missed deadline by", diff - self.dt)
+ if self.robot_manager.dt < diff:
+ print("missed deadline by", diff - self.robot_manager.dt)
continue
else:
- time.sleep(self.dt - diff)
+ time.sleep(self.robot_manager.dt - diff)
# TODO: provide a debug flag for this
# if i < self.max_iterations -1:
# print("success in", i, "iterations!")
@@ -174,6 +186,11 @@ robotmanager:
actually run at 500Hz and not more.
- this is probably not the most new-user friendly solution,
but it's designed for fastest idea to implementation rate.
+- NOTE and TODO: rely on pinocchio as much as possible.
+ do NOT copy data that's already present in pin.model and pin.data,
+ it's just confusing. this lib already heavily relies on pinocchio,
+ let's just lean into that all the way and not invent hot water.
+- TODO: write out default arguments needed
"""
class RobotManager:
# just pass all of the arguments here and store them as is
@@ -196,6 +213,7 @@ class RobotManager:
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
get_model(args.visualize)
+ # TODO: fix this
if args.visualize:
raise NotImplementedError('Paths in the urdf or something else need to \
be fixed to use this first. Sorry. Coming soon.')
@@ -261,6 +279,7 @@ class RobotManager:
self.rtde_io.setSpeedSlider(args.speed_slider)
self.max_iterations = args.max_iterations
# TODO: these are almost certainly higher
+ # NOTE and TODO: speed slider is evil, put it to 1, handle the rest yourself.
# maybe you want to have them high and cap the rest with speed slider?
# idk really, but it's better to have this low and burried for safety and robot longevity reasons
self.max_qdd = 1.7 * args.speed_slider
@@ -285,6 +304,34 @@ class RobotManager:
self.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
exit()
+ """
+ step
+ ----
+ - NOTE and TODO: not used ATM
+ - the idea is to update everything that should be updated
+ on a step-by-step basis
+ - the actual problem this is solving is that you're not calling
+ forwardKinematics, an expensive call, more than once per step.
+ - within the TODO is to make all (necessary) variable private
+ so that you can rest assured that everything is handled the way
+ it's supposed to be handled. then have getters for these
+ private variables which return deepcopies of whatever you need.
+ that way the computations done in the control loop
+ can't mess up other things. this is important if you want
+ to switch between controllers during operation and have a completely
+ painless transition between them.
+ TODO: make the getQ, getQd and the rest here do the actual communication,
+ and make these functions private.
+ then have the deepcopy getters public.
+ also TODO: make ifs for the simulation etc.
+ this is less ifs overall right.
+ """
+ def step(self):
+ self.getQ()
+ self.getQd()
+ self.wrench = np.array(self.rtde_receive.getActualTCPForce())
+ pin.forwardKinematics(self.model, self.data, self.q)
+ self.Mtool = self.data.oMi[self.JOINT_ID]
"""
setSpeedSlider
@@ -410,6 +457,7 @@ class RobotManager:
# readability is a somewhat subjective quality after all
qd = np.array(qd)
self.qd = qd
+ # TODO make sure this makes sense
else:
return self.qd
return qd
@@ -464,7 +512,7 @@ class RobotManager:
TODO get the visual thing you did in ivc project with sliders also.
it's just text input for now because it's totally usable, just not superb.
but also you do want to have both options. obviously you go for the sliders
- in the case you're visualizing, makes no sense otherwise
+ in the case you're visualizing, makes no sense otherwise.
"""
def defineGoalPoint(self):
q = self.getQ()
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index 565cc85c74e5580a485052c17069fe777cd20f1a..2850cb14d4726a62def37e709cf6bb52d3b5cf17 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 62537f7122cb2bdb8a733bab151cf89bc95b5506..4a32e98a4303921a0fc9e50ea8ce51f2105ea85f 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -43,12 +43,14 @@ def get_model(visualize):
# TODO fix paths via python packaging, get this to work
if visualize:
- raise NotImplementedError('Paths in the urdf or something else need to be fixed to use this first. Sorry. Coming soon.')
- module = None
+ #raise NotImplementedError('Paths in the urdf or something else need to be fixed to use this first. Sorry. Coming soon.')
+ model = None
collision_model = None
visual_model = None
+ model = pin.buildModelFromUrdf(urdf_path_absolute)
#model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute)
#visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL)
+ visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, package_dirs=mesh_dir_absolute)
#collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION)
#data = pin.Data(model)
else: