diff --git a/Dockerfile_to_build_upon b/Dockerfile_to_build_upon
new file mode 100644
index 0000000000000000000000000000000000000000..4456cf521a4ae2079654b4d3370189370676b9b9
--- /dev/null
+++ b/Dockerfile_to_build_upon
@@ -0,0 +1 @@
+FROM docker.control.lth.se/marko-g/ur_simple_control:latest
diff --git a/TODOS_2024_09_19 b/TODOS_2024_09_19
index 1c12eb3f18c7901410e674c81bcac4a10a63cf70..ccc314f16944f25eec85e570f8e7c9b3bbe0d13f 100644
--- a/TODOS_2024_09_19
+++ b/TODOS_2024_09_19
@@ -1,3 +1,26 @@
+goal 1: usability, verifiability
+----------------------------------
+1. have default arguments, you're adding/adapting new essential ones often, and copy-pasting
+ them around examples is idiotic and has to end immediatelly -> MOSTLY DONE
+2. fix the logging (as in save logs in an automated way + a parameter to check whether you want a new one or name it or sth)
+3. make logs importable to manipulator visualizer or wherever
+ to compare real and simulated runs specifically.
+ currently we genuinely don't know what the difference is,
+ and we have no way of judging how well references are tracked.
+ this is obviously essential - we're not eyeballing stuff,
+ we're verifying.
+4. BONUS to 3.: think of some basic metrics to calculate along
+ trajectories (print out some info on the performance of point-to-point and traj-following
+ runs, including comparing to same simulate runs etc. plots should be
+ fine to start, but having running rms or something sounds like a good idea).
+ also make sure x-axis are labelled correctly (wall-clock time)
+5. write some tests just too see that:
+ a) various parameter combinations work
+ b) controllers converge in situations they should converge in
+ c) most basic unit tests on functions
+ d) preferably some of this is runnable on the real robot,
+ or at least sim with --start-from-current pose flag
+
goal 2: clean up the code
---------------------------
1. test to see everything works
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 85d9db2798046aaa44adece1b5bd1ae546de2462..c5a8c58865919327900ef61af5e4fd1b8169a924 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -2,11 +2,9 @@ import numpy as np
import time
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, moveL, compliantMoveL
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
# TODO write this in managers and automate name generation
-from ur_simple_control.util.logging_utils import saveLog
"""
@@ -25,76 +23,18 @@ to the script you will be writing. This is kept here
only as a convenient reference point.
"""
def get_args():
- parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
- of various kinds. Make sure you know what the goal is before you run!',
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator", default=False)
- parser.add_argument('--robot-ip', type=str,
- help="robot's ip address (only needed if running on the real robot)", \
- default="192.168.1.102")
- parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio", default=False)
- parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
- parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', type=str, \
- help="gripper you're using (no gripper is the default)",
- default="none", choices=['none', 'robotiq', 'onrobot'])
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with", default=1e-2)
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
- 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, 0.5 by default \
- BE CAREFUL WITH THIS.", default=0.5)
- parser.add_argument("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
- help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
- very useful and convenient when running simulation before running on real", \
- default=False)
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tikhonov regularization", default=1e-3)
- parser.add_argument('--minimum-detectable-force-norm', type=float, \
- help="we need to disregard noise to converge despite filtering. \
- a quick fix is to zero all forces of norm below this argument threshold.",
- default=3.0)
- # TODO add the rest
- parser.add_argument('--clik-controller', type=str, \
- help="select which click algorithm you want", \
- default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP'])
- # maybe you want to scale the control signal
- parser.add_argument('--controller-speed-scaling', type=float, \
- default='1.0', help='not actually_used atm')
- parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
- help="print some debug info", default=False)
- parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
- help="save log data folder in whatever directory you called this in. if it doesn't exists there, it's saved to /tmp/data", default=False)
- parser.add_argument('--alpha', type=float, \
- help="force feedback proportional coefficient", \
- default=0.01)
- parser.add_argument('--beta', type=float, \
- help="low-pass filter beta parameter", \
- default=0.01)
- parser.add_argument('--past-window-size', type=int, \
- help="how many timesteps of past data you want to save", default=5)
-
+ parser = getMinimalArgParser()
+ parser.description = 'Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!'
+ parser = getClikArgs(parser)
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
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
Mgoal = robot.defineGoalPointCLI()
- log_dict, final_iteration = compliantMoveL(args, robot, Mgoal)
+ compliantMoveL(args, robot, Mgoal)
robot.closeGripper()
time.sleep(2.0)
robot.openGripper()
@@ -108,6 +48,6 @@ if __name__ == "__main__":
robot.killManipulatorVisualizer()
if args.save_log:
- saveLog(log_dict, final_iteration, args)
+ robot.log_manager.saveLog()
#loop_manager.stopHandler(None, None)
diff --git a/python/examples/compare_logs.py b/python/examples/compare_logs.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/examples/data/clik_comparison_0.pickle b/python/examples/data/clik_comparison_0.pickle
new file mode 100644
index 0000000000000000000000000000000000000000..32e22f561c64f552da9018fdfb46aec3f3b0ca90
Binary files /dev/null and b/python/examples/data/clik_comparison_0.pickle differ
diff --git a/python/examples/data/clik_comparison_1.pickle b/python/examples/data/clik_comparison_1.pickle
new file mode 100644
index 0000000000000000000000000000000000000000..0ceb878e17283c30b70f44b5c1e00c57f3c633c7
Binary files /dev/null and b/python/examples/data/clik_comparison_1.pickle differ
diff --git a/python/examples/data/latest_run_0 b/python/examples/data/latest_run_0
new file mode 100644
index 0000000000000000000000000000000000000000..161ff8a1bba90f9074b9539cdc38a3aa1bdd49e3
Binary files /dev/null and b/python/examples/data/latest_run_0 differ
diff --git a/python/examples/data/test2_0.pickle b/python/examples/data/test2_0.pickle
new file mode 100644
index 0000000000000000000000000000000000000000..fc57d202f31f4278b74dddecd55f61deb411beac
Binary files /dev/null and b/python/examples/data/test2_0.pickle differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 12314158fba8c230c9c96800db01e5f4137833b9..51f3c262eefad9207c80ec87783c4d5c2826e9a5 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -14,13 +14,11 @@ import time
from functools import partial
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, controlLoopClik, compliantMoveL
-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.dmp.dmp import getDMPArgs, DMP, NoTC,TCVelAccConstrained
+from ur_simple_control.clik.clik import getClikArgs, getClikController, moveL, moveUntilContact, controlLoopClik, compliantMoveL, clikCartesianPathIntoJointPath
+from ur_simple_control.util.map2DPathTo3DPlane import map2DPathTo3DPlane
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.util.calib_board_hacks import getBoardCalibrationArgs, calibratePlane, getSpeedInDirectionOfN
from ur_simple_control.basics.basics import moveJ
#######################################################################
@@ -31,127 +29,18 @@ 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('--robot-ip', type=str,
- help="robot's ip address (only needed if running on the real robot)", \
- default="192.168.1.102")
- 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-manipulator', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
- parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', type=str, \
- help="gripper you're using (no gripper is the default)",
- default="none", choices=['none', 'robotiq', 'onrobot'])
- 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)
- # TODO: make this optional
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
- parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
- help="print some debug info", default=False)
- #######################################################################
- # 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)
- parser.add_argument("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
- help="if connected to the robot, read the current pose and set it as the initial pose for the robot.\
- very useful and convenient when running simulation before running on real", \
- default=False)
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tikhonov regularization.\
- This is used when generating the joint trajectory from the drawing.", \
- default=1e-2)
- parser.add_argument('--minimum-detectable-force-norm', type=float, \
- help="we need to disregard noise to converge despite filtering. \
- a quick fix is to zero all forces of norm below this argument threshold.",
- default=3.0)
- # 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')
- parser.add_argument('--contact-detecting-force', type=float, \
- default='1.3', help='the force used to detect contact (collision) in the moveUntilContact function')
- #############################
- # dmp specific arguments #
- #############################
- parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
- help="whether you want to use temporal coupling", default=True)
+ parser = getMinimalArgParser()
+ parser = getClikArgs(parser)
+ parser = getDMPArgs(parser)
+ parser = getBoardCalibrationArgs(parser)
parser.add_argument('--kp', type=float, \
help="proportial control constant for position errors", \
default=1.0)
- 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", \
+ parser.add_argument('--kv', type=float, \
+ help="damping in impedance control", \
default=0.001)
- parser.add_argument('--alpha', type=float, \
- help="force feedback proportional coefficient", \
- default=0.007)
- #default=0.05)
- parser.add_argument('--beta', type=float, \
- help="low-pass filter beta parameter", \
- 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.5)
- default=0.25)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- #default=0.35)
- default=0.25)
- parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
- help="are you wiping the board (default is no because you're writing)", \
- default=False)
- # TODO: add axis direction too!!!!!!!!!!!!!!!!!
- # and change the various offsets accordingly
- parser.add_argument('--board-axis', type=str, \
- choices=['z', 'y'], \
- help="(world) axis (direction) in which you want to search for the board", \
- default="z")
- parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
- help="whether you want to do calibration", default=False)
+ parser.description = 'Make a drawing on screen,\
+ watch the robot do it on the whiteboard.'
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, \
@@ -163,14 +52,9 @@ def getArgs():
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)
+ parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
+ help="are you wiping the board (default is no because you're writing)", \
+ default=False)
args = parser.parse_args()
if args.gripper and args.simulation:
raise NotImplementedError('Did not figure out how to put the gripper in \
@@ -225,12 +109,10 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
robot.Mgoal = TgoalUP.copy()
compliantMoveL(args, robot, TgoalUP.copy())
#moveL(args, robot, TgoalUP.copy())
- #log_dict, final_iteration = loop_manager.run()
print("going down to marker")
robot.Mgoal = Tgoal.copy()
compliantMoveL(args, robot, Tgoal.copy())
#moveL(args, robot, Tgoal.copy())
- #log_dict, final_iteration = loop_manager.run()
print("picking up marker")
robot.closeGripper()
time.sleep(2)
@@ -238,7 +120,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
robot.Mgoal = TgoalUP.copy()
compliantMoveL(args, robot, TgoalUP.copy())
#moveL(args, robot, TgoalUP.copy())
- #log_dict, final_iteration = loop_manager.run()
print("going back")
# TODO: this HAS to be a movej
# in fact all of them should be
@@ -246,7 +127,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#compliantMoveL(args, robot, Tinit.copy())
moveJ(args, robot, q_init)
#moveL(args, robot, Tinit.copy())
- #log_dict, final_iteration = loop_manager.run()
print("got back")
"""
@@ -596,7 +476,7 @@ if __name__ == "__main__":
controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
# and now we can actually run
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
#loop_manager.stopHandler(None, None)
mtool = robot.getT_w_e()
print("move a bit back")
@@ -609,12 +489,9 @@ if __name__ == "__main__":
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
- #plotFromDict(log_dict, args)
loop_manager.stopHandler(None, None)
#robot.stopHandler(None, None)
#robot.stopHandler(None, None)
- # plot results
- #plotFromDict(log_dict, args)
# TODO: add some math to analyze path precision
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index bc9ca0b92fa4d364f9d15e8237f8bd7088e9ac6d..4cb8ed50a18b76585680c581358239ad6ce96602 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,75 +1,80 @@
-0.51607,0.68193
-0.51829,0.68193
-0.51829,0.68193
-0.52052,0.68041
-0.52052,0.68041
-0.52052,0.67889
-0.52496,0.67738
-0.52718,0.67586
-0.52940,0.67434
-0.53828,0.66826
-0.54050,0.66674
-0.54272,0.66522
-0.54494,0.66370
-0.55605,0.65155
-0.55827,0.65003
-0.56715,0.64396
-0.56715,0.64244
-0.56715,0.64244
-0.56715,0.64092
-0.56715,0.63940
-0.56715,0.63788
-0.56715,0.63636
-0.56715,0.63636
-0.56715,0.63029
-0.55827,0.62117
-0.55827,0.61966
-0.54939,0.61358
-0.52940,0.59080
-0.52496,0.58624
-0.51829,0.58168
-0.48498,0.55890
-0.47388,0.55434
-0.46721,0.55130
-0.44945,0.54371
-0.41836,0.53459
-0.41169,0.53308
-0.40503,0.53308
-0.38726,0.52548
-0.38726,0.52548
-0.38060,0.52548
-0.37394,0.52548
-0.37394,0.52396
+0.54717,0.65459
+0.54494,0.65459
+0.53828,0.65459
+0.53384,0.65459
+0.52718,0.65459
+0.52052,0.65459
+0.51829,0.65459
+0.51163,0.65307
+0.50941,0.65307
+0.49386,0.64548
+0.48054,0.64244
+0.47388,0.64092
+0.43390,0.62421
+0.42724,0.62117
+0.40281,0.60902
+0.39393,0.60295
+0.38504,0.59687
+0.38060,0.59231
+0.35395,0.57257
+0.35395,0.57257
+0.35173,0.56953
+0.34729,0.56042
+0.34507,0.55890
+0.34507,0.55586
+0.34063,0.54523
+0.34063,0.54371
+0.34063,0.54219
+0.34063,0.54067
+0.34063,0.53915
+0.34063,0.53763
+0.34285,0.53459
+0.34285,0.53308
+0.34729,0.53156
+0.35617,0.52852
+0.35839,0.52700
0.37172,0.52396
-0.36950,0.52396
-0.36950,0.52396
-0.36950,0.52548
-0.36950,0.52548
-0.36950,0.53004
-0.37172,0.53156
-0.37616,0.54219
-0.38060,0.54675
-0.38504,0.55586
-0.38948,0.56497
-0.40503,0.59991
-0.40947,0.60447
-0.41836,0.61358
-0.43612,0.64852
-0.43834,0.65307
-0.44278,0.66370
-0.46499,0.68801
-0.46944,0.69712
-0.47388,0.70168
-0.47832,0.71231
-0.49164,0.72598
-0.49831,0.73661
-0.50719,0.74269
-0.51607,0.75332
-0.52052,0.75788
-0.52274,0.75940
-0.52496,0.76092
-0.52496,0.76244
-0.52718,0.76244
-0.52718,0.76396
-0.52718,0.76396
-0.52718,0.76396
+0.37394,0.52092
+0.38060,0.51940
+0.40725,0.51029
+0.40947,0.50877
+0.41613,0.50725
+0.41836,0.50725
+0.45611,0.49662
+0.45833,0.49662
+0.46499,0.49662
+0.47832,0.49358
+0.48498,0.49358
+0.48720,0.49358
+0.49164,0.49358
+0.49164,0.49358
+0.49386,0.49358
+0.49609,0.49358
+0.49609,0.49510
+0.49609,0.49662
+0.50275,0.50725
+0.50497,0.50877
+0.50497,0.51333
+0.50941,0.52244
+0.51385,0.53004
+0.51829,0.54067
+0.52718,0.57409
+0.52940,0.58320
+0.53606,0.59383
+0.55161,0.62573
+0.55605,0.63636
+0.56049,0.64548
+0.56493,0.65003
+0.58492,0.67282
+0.58936,0.68345
+0.59158,0.68801
+0.61601,0.71383
+0.61823,0.71535
+0.61823,0.72142
+0.62045,0.72294
+0.62045,0.72294
+0.62045,0.72446
+0.62045,0.72598
+0.62045,0.72598
+0.62045,0.72750
+0.62045,0.72750
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index e09487350efbfea204dc9b262177625db2344a6f..1a2728ff68c363efbd13b2685028a74c00ba95ae 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -1,6 +1,5 @@
import pinocchio as pin
import numpy as np
-import matplotlib.pyplot as plt
import copy
import argparse
import time
@@ -8,14 +7,9 @@ from functools import partial
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.clik.clik import getClikArgs, getClikController, moveL, moveUntilContact
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.basics.basics import moveJ
-import matplotlib
#######################################################################
# arguments #
@@ -25,137 +19,18 @@ 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('--robot-ip', type=str,
- help="robot's ip address", \
- default="192.168.1.102")
- 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-manipulator', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
- parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', type=str, \
- help="gripper you're using (no gripper is the default)",
- default="none", choices=['none', 'robotiq', 'onrobot'])
- 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=500000)
- #######################################################################
- # 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('--cartesian-space-impedance', action=argparse.BooleanOptionalAction, \
- help="is the impedance computed and added in cartesian or in joint space", default=False)
- 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("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
- help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
- very useful and convenient when running simulation before running on real", \
- default=False)
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tikhonov 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', 'invKinmQP'])
- # 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 = getMinimalArgParser()
+ parser = getClikArgs(parser)
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.05)
- parser.add_argument('--beta', type=float, \
- help="low-pass filter beta parameter", \
- default=0.01)
- #######################################################################
- # 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)
- parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
- help="print some debug info", default=False)
+ parser.add_argument('--cartesian-space-impedance', action=argparse.BooleanOptionalAction, \
+ help="is the impedance computed and added in cartesian or in joint space", default=False)
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
@@ -285,7 +160,6 @@ if __name__ == "__main__":
# software setup #
#######################################################################
- #matplotlib.use('tkagg')
args = getArgs()
if args.debug_prints:
print("you will get a lot of stuff printed out, as requested")
@@ -323,7 +197,7 @@ if __name__ == "__main__":
#moveJ(args, robot, dmp.pos.reshape((6,)))
# and now we can actually run
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
#plotFromDict(log_dict, args)
# plotting is now initiated in stophandler because then we get the plot
diff --git a/python/examples/test_gripper.py b/python/examples/test_gripper.py
index 2b1102dd09a4a8f0fa30504ff2a69826b3364bd3..2493fa583530e03a42f8711448251e5fc13fa85a 100644
--- a/python/examples/test_gripper.py
+++ b/python/examples/test_gripper.py
@@ -211,6 +211,6 @@ if __name__ == "__main__":
# }
# # we're not using any past data or logging, hence the empty arguments
# loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_dict)
-# log_dict, final_iteration = loop_manager.run()
+# loop_manager.run()
# saveLog(log_dict, final_iteration, args)
diff --git a/python/examples/test_movej.py b/python/examples/test_movej.py
index c00d3683f8534117080ae5dfce6f59c27c70b534..be853026c99557a5533ae34e0542f8a42fa640d2 100644
--- a/python/examples/test_movej.py
+++ b/python/examples/test_movej.py
@@ -192,7 +192,7 @@ if __name__ == "__main__":
q_init[5] += 0.1
moveJ(args, robot, q_init)
# and now we can actually run
-# log_dict, final_iteration = loop_manager.run()
+# loop_manager.run()
#plotFromDict(log_dict, args)
# plotting is now initiated in stophandler because then we get the plot
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index ac78c9ecfbfb2b36f7db8dfd8b9ca6621c026c2d..49cdc1ff94ca1a8be35034d322263985a15a4d32 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index d81beb36ff7ec21e6520d05bfab108ca4915d6ed..4f424098d8ba7b728f84a2dc541580ae9562eec8 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -56,7 +56,7 @@ def moveJ(args, robot, q_desired):
controlLoop = partial(moveJControlLoop, q_desired, robot)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
# TODO: remove, this isn't doing anything
#time.sleep(0.01)
if args.debug_prints:
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik.py
similarity index 69%
rename from python/ur_simple_control/clik/clik_point_to_point.py
rename to python/ur_simple_control/clik/clik.py
index 83ab327900f78d78898072c0ca5c86934b3b426d..a422e1667bd1bebe55cce2ed816a8542a106ddca 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -6,75 +6,58 @@ from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
import time
from qpsolvers import solve_qp
+import argparse
-def get_args():
+def getClikArgs(parser):
"""
- Every imaginable magic number, flag and setting is put in here.
+ getClikArgs
+ ------------
+ Every clik-related magic number, flag and setting is put in here.
This way the rest of the code is clean.
+ Force filtering is considered a core part of these control loops
+ because it's not necessarily the same as elsewhere.
If you want to know what these various arguments do, just grep
though the code to find them (but replace '-' with '_' in multi-word arguments).
All the sane defaults are here, and you can change any number of them as an argument
when running your program. If you want to change a drastic amount of them, and
not have to run a super long commands, just copy-paste this function and put your
own parameters as default ones.
- NOTE1: the args object obtained from args = parser.get_args()
- is pushed all around the rest of the code (because it's tidy), so don't change their names.
- NOTE2: that you need to copy-paste and add aguments you need
- to the script you will be writing. This is kept here
- only as a convenient reference point.
"""
- parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
- of various kinds. Make sure you know what the goal is before you run!',
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator", default=False)
- parser.add_argument('--robot-ip', type=str,
- help="robot's ip address", \
- default="192.168.1.102")
- parser.add_argument('--debug_prints', action=argparse.BooleanOptionalAction,
- help="print some info for debugging", default=False)
- parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio", default=False)
- parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
- parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
+ #parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
+ # of various kinds. Make sure you know what the goal is before you run!',
+ # formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+ ####################
+ # clik arguments #
+ ####################
parser.add_argument('--goal-error', type=float, \
help="the final position error you are happy with", default=1e-2)
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
- 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, 0.5 by default \
- BE CAREFUL WITH THIS.", default=0.5)
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tikhonov regularization", default=1e-3)
# 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')
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP'])
+
+ ###########################################
+ # force sensing and feedback parameters #
+ ###########################################
parser.add_argument('--alpha', type=float, \
help="force feedback proportional coefficient", \
- #default=0.01)
- default=0.05)
+ default=0.01)
parser.add_argument('--beta', type=float, \
help="low-pass filter beta parameter", \
default=0.01)
- 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
+ ###############################
+ # path following parameters #
+ ###############################
+ 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)
+
+ return parser
#######################################################################
# controllers #
@@ -245,7 +228,7 @@ def moveUntilContact(args, robot, speed):
controlLoop = partial(moveUntilContactControlLoop, args.contact_detecting_force, speed, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
# TODO: remove, this isn't doing anything
time.sleep(0.01)
print("Collision detected!!")
@@ -268,7 +251,7 @@ def moveL(args, robot, goal_point):
'dqs' : np.zeros(robot.model.nq),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
# TODO: remove, this isn't doing anything
time.sleep(0.01)
print("MoveL done: convergence achieved, reached destionation!")
@@ -354,11 +337,119 @@ def compliantMoveL(args, robot, goal_point):
'wrench': np.zeros(6),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
# TODO: remove, this isn't doing anything
time.sleep(0.01)
print("compliantMoveL done: convergence achieved, reached destionation!")
- return log_dict, final_iteration
+
+def clikCartesianPathIntoJointPath(path, args, robot, \
+ clikController, q_init, R, p):
+ """
+ clikCartesianPathIntoJointPath
+ ------------------------------
+ functionality
+ ------------
+ Follows a provided Cartesian path,
+ creates a joint space trajectory for it.
+
+ return
+ ------
+ - joint_space_trajectory to follow the given path.
+
+ arguments
+ ----------
+ - path:
+ --> cartesian path given in task frame
+ TODO: write asserts for these arguments
+ - args:
+ --> all the magic numbers used here better be here
+ - clikController:
+ --> clik controller you're using
+ - robot:
+ --> RobotManager instance (needed for forward kinematics etc)
+ TODO: maybe it's better design to expect path in body frame idk
+ the good thing here is you're forced to think about frames.
+ TODO: make this a kwarg with a neural transform as default.
+ - transf_body_task_frame:
+ --> A transformation from the body frame to task frame.
+ --> It is assumed that the path was provided in the task frame
+ because 9/10 times that's more convenient,
+ and you can just pass a neural transform if you're not using it.
+ - q_init:
+ --> starting point.
+ --> you can movej to it before executing the trajectory,
+ so this makes perfect sense to ensure correctness
+ """
+
+ transf_body_to_task_frame = pin.SE3(R, p)
+ q = copy.deepcopy(q_init)
+
+ for i in range(len(path)):
+ path[i] = transf_body_to_task_frame.act(path[i])
+ # TODO remove print, write a test for this instead
+ if args.debug_prints:
+ print(path)
+
+ # TODO: finish this
+ # - pass clik alg as argument
+ # - remove magic numbers
+ # - give output in the right format
+ # skip inital pos tho
+ #q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
+ #q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
+ # this is just init_q right now
+ # TODO: make this a flag or something for readability's sake
+ n_iter = args.max_init_clik_iterations
+ # we don't know how many there will be, so a linked list is
+ # clearly the best data structure here (instert is o(1) still,
+ # and we aren't pressed on time when turning it into an array later)
+ qs = []
+ for goal in path:
+ Mgoal = pin.SE3(R, goal)
+ for i in range(n_iter):
+ # TODO maybe hide pin call with RobotManager call
+ pin.forwardKinematics(robot.model, robot.data, q)
+ SEerror = robot.data.oMi[robot.JOINT_ID].actInv(Mgoal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < args.goal_error:
+ if not n_iter == args.max_init_clik_iterations:
+ if args.debug_prints:
+ print("converged in", i)
+ break
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ qd = clikController(J, err_vector)
+ # we're integrating this fully offline of course
+ q = pin.integrate(robot.model, q, qd * robot.dt)
+ if (not n_iter == args.max_init_clik_iterations) and (i % 10 == 0):
+ qs.append(q[:6])
+
+ # just skipping the first run with one stone
+ if n_iter == args.max_init_clik_iterations:
+ n_iter = args.max_running_clik_iterations
+ else:
+ if i == args.max_running_clik_iterations - 1:
+ print("DID NOT CONVERGE -- exiting")
+ # nothing is moving
+ # and i'm not even using a manager here
+ # so no need, right?
+ #ControlLoopManager.stopHandler(None, None, None)
+ exit()
+
+ ##############################################
+ # save the obtained joint-space trajectory #
+ ##############################################
+ qs = np.array(qs)
+ # we're putting a dmp over this so we already have the timing ready
+ # TODO: make this general, you don't want to depend on other random
+ # arguments (make this one traj_time, then put tau0 = traj_time there
+ t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs),1))
+ 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
+
if __name__ == "__main__":
args = get_args()
@@ -368,4 +459,4 @@ if __name__ == "__main__":
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
- log_dict, final_iteration = loop_manager.run()
+ loop_manager.run()
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
deleted file mode 100644
index 96908e43201fee762a344848d0aac3c193121aeb..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ /dev/null
@@ -1,155 +0,0 @@
-# your entry point is a
-# np.array of shape (N_POINTS, 2)
-# this files results in a timed joint path
-
-# STEP 1: map the pixels to a 3D plane
-# STEP 2: clik that path
-# STEP 3: put result into csv (but also save it in a convenient class/array here)
-
-import numpy as np
-import pinocchio as pin
-import copy
-from ur_simple_control.managers import ControlLoopManager
-
-#######################################################################
-# map the pixels to a 3D plane #
-#######################################################################
-def map2DPathTo3DPlane(path_2D, width, height):
- """
- map2DPathTo3DPlane
- --------------------
- TODO: THINK AND FINALIZE THE FRAME
- TODO: WRITE PRINT ABOUT THE FRAME TO THE USER
- assumptions:
- - origin in top-left corner (natual for western latin script writing)
- - x goes right (from TCP)
- - z will go away from the board
- - y just completes the right-hand frame
- TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
- NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
- once it will actually be needed elsewhere
- Returns a 3D path appropriately scaled, and placed into the first quadrant
- of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
- """
- z = np.zeros((len(path_2D),1))
- path_3D = np.hstack((path_2D,z))
- # scale the path to m
- path_3D[:,0] = path_3D[:,0] * width
- path_3D[:,1] = path_3D[:,1] * height
- # in the new coordinate system we're going in the -y direction
- # TODO this is a demo specific hack,
- # make it general for a future release
- path_3D[:,1] = -1 * path_3D[:,1] + height
- return path_3D
-
-
-def clikCartesianPathIntoJointPath(path, args, robot, \
- clikController, q_init, R, p):
- """
- clikCartesianPathIntoJointPath
- ------------------------------
- functionality
- ------------
- Follows a provided Cartesian path,
- creates a joint space trajectory for it.
-
- return
- ------
- - joint_space_trajectory to follow the given path.
-
- arguments
- ----------
- - path:
- --> cartesian path given in task frame
- TODO: write asserts for these arguments
- - args:
- --> all the magic numbers used here better be here
- - clikController:
- --> clik controller you're using
- - robot:
- --> RobotManager instance (needed for forward kinematics etc)
- TODO: maybe it's better design to expect path in body frame idk
- the good thing here is you're forced to think about frames.
- TODO: make this a kwarg with a neural transform as default.
- - transf_body_task_frame:
- --> A transformation from the body frame to task frame.
- --> It is assumed that the path was provided in the task frame
- because 9/10 times that's more convenient,
- and you can just pass a neural transform if you're not using it.
- - q_init:
- --> starting point.
- --> you can movej to it before executing the trajectory,
- so this makes perfect sense to ensure correctness
- """
-
- transf_body_to_task_frame = pin.SE3(R, p)
- q = copy.deepcopy(q_init)
-
- for i in range(len(path)):
- path[i] = transf_body_to_task_frame.act(path[i])
- # TODO remove print, write a test for this instead
- if args.debug_prints:
- print(path)
-
- # TODO: finish this
- # - pass clik alg as argument
- # - remove magic numbers
- # - give output in the right format
- # skip inital pos tho
- #q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
- #q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
- # this is just init_q right now
- # TODO: make this a flag or something for readability's sake
- n_iter = args.max_init_clik_iterations
- # we don't know how many there will be, so a linked list is
- # clearly the best data structure here (instert is o(1) still,
- # and we aren't pressed on time when turning it into an array later)
- qs = []
- for goal in path:
- Mgoal = pin.SE3(R, goal)
- for i in range(n_iter):
- # TODO maybe hide pin call with RobotManager call
- pin.forwardKinematics(robot.model, robot.data, q)
- SEerror = robot.data.oMi[robot.JOINT_ID].actInv(Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < args.clik_goal_error:
- if not n_iter == args.max_init_clik_iterations:
- if args.debug_prints:
- print("converged in", i)
- break
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
- qd = clikController(J, err_vector)
- # we're integrating this fully offline of course
- q = pin.integrate(robot.model, q, qd * robot.dt)
- if (not n_iter == args.max_init_clik_iterations) and (i % 10 == 0):
- qs.append(q[:6])
-
- # just skipping the first run with one stone
- if n_iter == args.max_init_clik_iterations:
- n_iter = args.max_running_clik_iterations
- else:
- if i == args.max_running_clik_iterations - 1:
- print("DID NOT CONVERGE -- exiting")
- # nothing is moving
- # and i'm not even using a manager here
- # so no need, right?
- #ControlLoopManager.stopHandler(None, None, None)
- exit()
-
- ##############################################
- # save the obtained joint-space trajectory #
- ##############################################
- qs = np.array(qs)
- # we're putting a dmp over this so we already have the timing ready
- # TODO: make this general, you don't want to depend on other random
- # arguments (make this one traj_time, then put tau0 = traj_time there
- t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs),1))
- 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/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index c88d4965a1288a73f8fc9345baa64af0aeeca153..01c73c9ba60d4bd126027f16c31dbe867c811c25 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc differ
diff --git a/python/ur_simple_control/dmp/dmp.py b/python/ur_simple_control/dmp/dmp.py
index 16f6f154a15cc75ccc6b2f0c83062d020ddecd2c..348cb0ac12eebefc45154825a423bf25451af4c3 100644
--- a/python/ur_simple_control/dmp/dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -1,4 +1,5 @@
import numpy as np
+import argparse
# TODO:
# 1. change the dimensions so that they make sense,
# i.e. shape = (N_points, dimension_of_points)
@@ -12,6 +13,34 @@ import numpy as np
# these work fine, but it could be good to play around with them just to
# see their effect. normally people set them so that you get critical damping
# as the uncostrained system
+
+def getDMPArgs(parser):
+ #############################
+ # 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('--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)
+ #default=0.05)
+ return parser
+
+
class DMP:
def __init__(self, trajectory, k=100, d=20, a_s=1, n_bfs=100):
# TODO load the trajectory here
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index b8d1f2d0040d9efd1f380d90589a53ec04846098..e25f0cded131c2efe8ee96fbee6f754543283de4 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -20,7 +20,9 @@ import signal
from ur_simple_control.util.get_model import get_model
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
+from ur_simple_control.util.logging_utils import LogManager
from multiprocessing import Process, Queue
+import argparse
"""
general notes
@@ -58,6 +60,83 @@ There is an interface to a physics simulator.
Functions for torque controlled robots exist.
"""
+def getMinimalArgParser():
+ """
+ getDefaultEssentialArgs
+ ------------------------
+ returns a parser containing:
+ - essential arguments (run in real or in sim)
+ - parameters for (compliant)moveJ
+ - parameters for (compliant)moveL
+ """
+ parser = argparse.ArgumentParser(description="Run something with \
+ Simple Manipulator Control", \
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ #################################################
+ # general arguments: connection, plotting etc #
+ #################################################
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
+ help="whether you are running the UR simulator", default=False)
+ parser.add_argument('--robot-ip', type=str,
+ help="robot's ip address (only needed if running on the real robot)", \
+ default="192.168.1.102")
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction, \
+ help="whether you want to just integrate with pinocchio", default=False)
+ parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction, \
+ help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
+ parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction, \
+ help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
+ parser.add_argument('--gripper', type=str, \
+ help="gripper you're using (no gripper is the default)", \
+ default="none", choices=['none', 'robotiq', 'onrobot'])
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--speed-slider', type=float,\
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.", default=0.5)
+ parser.add_argument("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
+ very useful and convenient when running simulation before running on real", \
+ 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('--debug-prints', action=argparse.BooleanOptionalAction, \
+ help="print some debug info", default=False)
+ parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
+ help="whether you want to save the log of the run. it saves \
+ what you pass to ControlLoopManager. check other parameters for saving directory and log name.", default=False)
+ parser.add_argument('--save-dir', type=str, \
+ help="path to where you store your logs. default is ./data, but if that directory doesn't exist, then /tmp/data is created and used.", \
+ default='./data')
+ parser.add_argument('--run-name', type=str, \
+ help="name the whole run/experiment (name of log file). note that indexing of runs is automatic and under a different argument.", \
+ default='none')
+ parser.add_argument('--index-runs', action=argparse.BooleanOptionalAction, \
+ help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).", default=True)
+ parser.add_argument('--past-window-size', type=int, \
+ help="how many timesteps of past data you want to save", default=5)
+ # maybe you want to scale the control signal (TODO: NOT HERE)
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ ########################################
+ # environment interaction parameters #
+ ########################################
+ parser.add_argument('--contact-detecting-force', type=float, \
+ default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+ parser.add_argument('--minimum-detectable-force-norm', type=float, \
+ help="we need to disregard noise to converge despite filtering. \
+ a quick fix is to zero all forces of norm below this argument threshold.",
+ default=3.0)
+ # TODO make this work without parsing (or make it possible to parse two times)
+ #if (args.gripper != "none") 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 parser
+
+
class ControlLoopManager:
"""
ControlLoopManager
@@ -235,13 +314,8 @@ class ControlLoopManager:
if self.args.debug_prints:
print("terminated real_time_plotter_process")
- # now turn the logs into numpy arrays
- for key in self.log_dict:
- if self.args.debug_prints:
- print("turning log files into numpy arrays")
- self.log_dict[key] = np.array(self.log_dict[key])
-
- return self.log_dict, self.final_iteration
+ if self.args.save_log:
+ self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
def stopHandler(self, signum, frame):
"""
@@ -296,7 +370,7 @@ class ControlLoopManager:
# if self.args.debug_prints:
# print("joined manipulator_visualizer_process")
- # need to turn logs into ndarrays here too
+ # let's plot the loop you stopped
for key in self.log_dict:
self.log_dict[key] = np.array(self.log_dict[key])
plotFromDict(self.log_dict, self.final_iteration, self.args)
@@ -342,9 +416,8 @@ class RobotManager:
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
get_model()
- # we're using meshcat exclusively.
- # there are no good options,
- # but this does work and isn't a dead project
+ # start visualize manipulator process if selected.
+ # has to be started here because it lives throughout the whole run
if args.visualize_manipulator:
self.manipulator_visualizer_queue = Queue()
if args.debug_prints:
@@ -352,7 +425,6 @@ class RobotManager:
print("ROBOT_MANAGER: i am creating and starting the manipulator visualizer process")
self.manipulator_visualizer_process = Process(target=manipulatorVisualizer,
args=(self.args, self.model, self.collision_model, self.visual_model, self.manipulator_visualizer_queue, ))
- # give real-time plotter some time to set itself up
self.manipulator_visualizer_process.start()
if args.debug_prints:
print("ROBOT_MANAGER: manipulator_visualizer_process started")
@@ -360,6 +432,10 @@ class RobotManager:
if args.debug_prints:
print("ROBOT_MANAGER: i managed to put initializing q to manipulator_visualizer_queue")
+ # create log manager if we're saving logs
+ if args.save_log:
+ self.log_manager = LogManager(args)
+
# ur specific magic numbers
# NOTE: all of this is ur-specific, and needs to be if-ed if other robots are added.
# TODO: this is 8 in pinocchio and that's what you actually use
@@ -845,3 +921,4 @@ class RobotManager:
self.manipulator_visualizer_process.terminate()
if self.args.debug_prints:
print("terminated manipulator_visualizer_process")
+
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
index f3cb936ccba20349b062a79f7f3ea0bac9108256..e346abfc3404c5e04c001cf4c8c41732952a3921 100644
Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/draw_path.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/draw_path.cpython-312.pyc
index 71e35875b7d8a6176729a24085e21d5e5618c2b2..d52205a7faf1718a3c5d1dfcb332277f2b09b2ed 100644
Binary files a/python/ur_simple_control/util/__pycache__/draw_path.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/draw_path.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/freedrive.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/freedrive.cpython-312.pyc
index dce20d71598646ad9f177e5cf7aa45a82ee6ab06..d5de4577242d504ac09051918b46a3d816d45fd1 100644
Binary files a/python/ur_simple_control/util/__pycache__/freedrive.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/freedrive.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index 1557c6d021d3fe0b591b7bf92ae68a47a0777933..f47591d995636eee4dd219f3004ed75c1e0574b0 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 63bce31c624dd63094349a35bfc6b340e4995231..d53d64851338f3a49892fe0f8a7fe3b2ce11ef08 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -7,9 +7,31 @@ import time
import copy
from ur_simple_control.managers import RobotManager
from ur_simple_control.util.freedrive import freedrive
-from ur_simple_control.clik.clik_point_to_point import moveL, moveUntilContact
+from ur_simple_control.clik.clik import moveL, moveUntilContact
# used to deal with freedrive's infinite while loop
import threading
+import argparse
+
+def getBoardCalibrationArgs(parser):
+ parser.add_argument('--board-width', type=float, \
+ help="width of the board (in meters) the robot will write on", \
+ #default=0.5)
+ default=0.25)
+ parser.add_argument('--board-height', type=float, \
+ help="height of the board (in meters) the robot will write on", \
+ #default=0.35)
+ default=0.25)
+ # TODO: add axis direction too!!!!!!!!!!!!!!!!!
+ # and change the various offsets accordingly
+ parser.add_argument('--board-axis', type=str, \
+ choices=['z', 'y'], \
+ help="(world) axis (direction) in which you want to search for the board", \
+ default="z")
+ parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
+ help="whether you want to do calibration", default=False)
+ parser.add_argument('--n-calibration-tests', type=int, \
+ help="number of calibration tests you want to run", default=10)
+ return parser
"""
general
diff --git a/python/ur_simple_control/util/default_args.py b/python/ur_simple_control/util/default_args.py
deleted file mode 100644
index 218de8545f1b2c8b13ef45653f48418ac1633911..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/util/default_args.py
+++ /dev/null
@@ -1,14 +0,0 @@
-import pinocchio as pin
-import numpy as np
-import argparse
-
-
-"""
-TODO: have 2 versions:
- 1. only the minimal set of argumets (those needed to load robot
- and run anything)
- 2. every single one you have os far
-"""
-def getDefaultArgs():
- raise NotImplementedError("sorry, this hasn't been implemented yet")
-
diff --git a/python/ur_simple_control/util/draw_path.py b/python/ur_simple_control/util/draw_path.py
index b06db178645e9bdce5a13178429b022089c150a6..29cf10423471560c052ceb02704d0ffebae40ada 100644
--- a/python/ur_simple_control/util/draw_path.py
+++ b/python/ur_simple_control/util/draw_path.py
@@ -18,7 +18,6 @@ import matplotlib.pyplot as plt
# Thus it is the correct tool for the job and there's no need
# to reimplement it from mouse events.
from matplotlib.widgets import LassoSelector
-from ur_simple_control.clik.clik_point_to_point import get_args
# Path is the most generic matplotlib class.
# It has some convenient functions to handle the draw line,
# i.e. collection of points, but just refer to matplotlib
diff --git a/python/ur_simple_control/util/freedrive.py b/python/ur_simple_control/util/freedrive.py
index e5a55d82b6ca7be309397b45ed1811c2d2253d74..6403437c05a373e787af6e73b17ec40d9bbad8a3 100644
--- a/python/ur_simple_control/util/freedrive.py
+++ b/python/ur_simple_control/util/freedrive.py
@@ -2,12 +2,11 @@ import pinocchio as pin
import numpy as np
import time
import signal
-from ur_simple_control.managers import RobotManager
+from ur_simple_control.managers import RobotManager, getMinimalArgParser
# TODO: put sane default arguments needed just for managers
# into a separate file, call it default arguments.
# ideally you also only need to add your additional ones
# to this list.
-from ur_simple_control.clik.clik_point_to_point import get_args
def handler(signum, frame):
@@ -29,7 +28,8 @@ def freedrive(robot):
time.sleep(0.005)
if __name__ == "__main__":
- args = get_args()
+ parser = getMinimalArgParser()
+ args = parser.parse_args()
robot = RobotManager(args)
signal.signal(signal.SIGINT, handler)
freedrive(robot)
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 99d4b3e8878c1095d2ea9d7677c870e510130991..3e80eef1d08852b88281c2e73cc8b6f61b5bb0b4 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -1,58 +1,115 @@
import pickle
import numpy as np
import os
+import subprocess
+import re
-def saveLog(log_dict, final_iteration, args):
- # shave off the zeros, noone needs 'em
- for key in log_dict:
- log_dict[key] = log_dict[key][:final_iteration]
- # TODO make generic:
- # - generate name based on args + add index
- # - you need to run shell ls to get the index,
- # there's at least one chalmers project with code for that
- if os.path.exists('./data'):
- run_file_path = "./data/clik_run_001.pickle"
- args_file_path = "./data/clik_run_001_args.pickle"
- else:
- os.makedirs('/tmp/data', exist_ok=True)
- run_file_path = "/tmp/data/clik_run_001.pickle"
- args_file_path = "/tmp/data/clik_run_001_args.pickle"
- # save the logged data
- # you need to open it binary for pickling
- log_file = open(run_file_path, 'wb')
- pickle.dump(log_dict, log_file)
- log_file.close()
- # but also save the arguments
- # pretty sure you need to open it binary for pickling
- log_file = open(args_file_path, 'wb')
- pickle.dump(args, log_file)
- log_file.close()
-
-
-def cleanUpRun(log_dict, final_iteration, n_iterations_you_want):
- # shave off the zeros at the end
- for key in log_dict:
- log_dict[key] = log_dict[key][:final_iteration]
- # and now keep only every nth iteration
- # because you don't want to plot too much
- if final_iteration > n_iterations_you_want:
- nth_to_keep = final_iteration // n_iterations_you_want
- bool_array = [i % nth_to_keep == 0 for i in range(final_iteration)]
- # actual final number
- # True is turned to 0, False to 0, praised by python and its ways
- n_iters = np.sum(bool_array)
- for key in log_dict:
- log_dict[key] = log_dict[key][bool_array]
-
- return log_dict
-
-
-def loadRunForAnalysis(log_data_file_name, args_file_name):
- log_data_file = open(log_data_file_name, 'rb')
- args_file = open(args_file_name, 'rb')
- log_data = pickle.load(log_data_file)
- args = pickle.load(args_file)
- # if you're analyzing, you're not running anything on the real robot
- args.simulation = True
- args.pinocchio_only = True
- return log_data, args
+
+class LogManager:
+ """
+ LogManager
+ ----------
+ The simplest possible way to combine logs of different
+ control loops - store them separately.
+ Comes with some functions to clean and combine logs
+ of different control loops (TODO).
+ - input: log_dicts from ControlLoopManager
+ - output: saves this whole class as pickle -
+ data and arguments included
+ """
+ def __init__(self, args):
+ if args is None:
+ return
+ self.args = args
+ self.loop_logs = {}
+ self.loop_number = 0
+ # name the run
+ self.run_name = 'latest_run'
+ if self.args.run_name != 'none':
+ self.run_name = self.args.run_name
+ # assign save directory
+ if args.save_dir != "./data":
+ if os.path.exists(self.args.save_dir):
+ self.save_dir = self.args.save_dir
+ else:
+ if os.path.exists("./data"):
+ self.save_dir = "./data"
+ else:
+ os.makedirs('/tmp/data', exist_ok=True)
+ self.save_dir = '/tmp/data'
+
+ # if indexing (same run, multiple times, want to save all)
+ # update the name with the free index
+ if args.index_runs:
+ index = self.findLatestIndex()
+ self.run_name = self.run_name + "_" + str(index) + ".pickle"
+
+ self.save_file_path = os.path.join(self.save_dir, self.run_name)
+
+
+ def storeControlLoopRun(self, log_dict, loop_name, final_iteration):
+ loop_name = str(self.loop_number) + '_' + loop_name
+ self.loop_number += 1
+ self.loop_logs[loop_name] = log_dict
+
+
+ def saveLog(self, cleanUpRun=False):
+ """
+ saveLog
+ -------
+ transforms the logs obtained from control loops
+ into numpy arrays and pickles the whole of LogManager
+ (including the data and the args).
+ Uses default pickling.
+ """
+ # finally transfer to numpy (after nothing is running anymore)
+ for control_loop_name in self.loop_logs:
+ for key in self.loop_logs[control_loop_name]:
+ self.loop_logs[control_loop_name][key] = np.array(self.loop_logs[control_loop_name][key])
+ if cleanUpRun:
+ self.cleanUpRun()
+ log_file = open(self.save_file_path, 'wb')
+ pickle.dump(self.__dict__, log_file)
+ log_file.close()
+
+ def loadLog(self, save_file_path):
+ """
+ loadLog
+ -------
+ unpickles a log, which is the whole of LogManager
+ (including the data and the args).
+ Uses default (un)pickling.
+ """
+ log_file = open(save_file_path, 'rb')
+ tmp_dict = pickle.load(log_file)
+ log_file.close()
+ self.__dict__.clear()
+ self.__dict__.update(tmp_dict)
+
+
+ def findLatestIndex(self):
+ """
+ findLatestIndex
+ ---------------
+ reads save_dir, searches for run_name,
+ finds the highest index within the file whose names match run_name.
+ NOTE: better to not have other files in the data dir,
+ this isn't written to work in every circumstances,
+ it assumes a directory with Simple Manipulator Control log files only
+ """
+ child = subprocess.Popen(['ls', self.save_dir], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+ files_in_datadir = child.stdout.read().decode('utf-8').split("\n")
+ regex_name = re.compile(self.run_name + ".*")
+ regex_index = re.compile("[0-9]+")
+ highest_index = -1
+ for file_name in files_in_datadir:
+ rez_name = regex_name.search(file_name)
+ if rez_name != None:
+ rez_index = regex_index.findall(file_name)
+ if len(rez_index) > 0:
+ this_file_name_index = int(rez_index[-1])
+ if this_file_name_index > highest_index:
+ highest_index = this_file_name_index
+
+ index = highest_index + 1
+ return index
diff --git a/python/ur_simple_control/util/map2DPathTo3DPlane.py b/python/ur_simple_control/util/map2DPathTo3DPlane.py
new file mode 100644
index 0000000000000000000000000000000000000000..a0d9f78b3f870b612ad099ce543787c112480505
--- /dev/null
+++ b/python/ur_simple_control/util/map2DPathTo3DPlane.py
@@ -0,0 +1,34 @@
+import numpy as np
+
+#######################################################################
+# map the pixels to a 3D plane #
+#######################################################################
+def map2DPathTo3DPlane(path_2D, width, height):
+ """
+ map2DPathTo3DPlane
+ --------------------
+ TODO: THINK AND FINALIZE THE FRAME
+ TODO: WRITE PRINT ABOUT THE FRAME TO THE USER
+ assumptions:
+ - origin in top-left corner (natual for western latin script writing)
+ - x goes right (from TCP)
+ - z will go away from the board
+ - y just completes the right-hand frame
+ TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
+ NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
+ once it will actually be needed elsewhere
+ Returns a 3D path appropriately scaled, and placed into the first quadrant
+ of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
+ """
+ z = np.zeros((len(path_2D),1))
+ path_3D = np.hstack((path_2D,z))
+ # scale the path to m
+ path_3D[:,0] = path_3D[:,0] * width
+ path_3D[:,1] = path_3D[:,1] * height
+ # in the new coordinate system we're going in the -y direction
+ # TODO this is a demo specific hack,
+ # make it general for a future release
+ path_3D[:,1] = -1 * path_3D[:,1] + height
+ return path_3D
+
+
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 8a1adf2f8db4ef94087d08a06981bcfb331508e7..8fcb405e56bc736d66c541ba4caf5b5ff6f02c36 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
new file mode 100644
index 0000000000000000000000000000000000000000..3dd8012350d72e63bb632eeebc78f9c33e8aecea
--- /dev/null
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -0,0 +1,113 @@
+import numpy as np
+import argparse
+import os
+import time
+from itertools import zip_longest
+from multiprocessing import Process, Queue
+from ur_simple_control.managers import getMinimalArgParser, RobotManager
+from ur_simple_control.util.logging_utils import LogManager
+from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer
+
+def getLogComparisonArgs():
+ parser = getMinimalArgParser()
+# parser = getClikArgs(parser)
+# parser = getDMPArgs(parser)
+# parser = getBoardCalibrationArgs(parser)
+ parser.add_argument('--log-file1', type=str, \
+ help="first log file to load for comparison")
+ parser.add_argument('--log-file2', type=str, \
+ help="first log file to load for comparison")
+ args = parser.parse_args()
+ # these are obligatory
+ args.visualize_manipulator = False
+ args.pinocchio_only = True
+ args.simulation = False
+ return args
+
+class ManipulatorComparisonManager:
+ def __init__(self, args):
+ self.args = args
+ # these are obligatory
+ args.visualize_manipulator = False
+ args.pinocchio_only = True
+ args.simulation = False
+
+ self.robot1 = RobotManager(args)
+ self.robot2 = RobotManager(args)
+
+ # no two loops will have the same amount of timesteps.
+ # we need to store the last available step for both robots.
+ self.lastq1 = np.zeros(self.robot1.model.nq)
+ self.lastq2 = np.zeros(self.robot2.model.nq)
+
+ if os.path.exists(args.log_file1):
+ self.logm1 = LogManager(None)
+ self.logm1.loadLog(args.log_file1)
+ else:
+ print("you did not give me a valid path, exiting")
+ exit()
+ if os.path.exists(args.log_file2):
+ self.logm2 = LogManager(None)
+ self.logm2.loadLog(args.log_file2)
+ else:
+ print("you did not give me a valid path, exiting")
+ exit()
+
+ self.manipulator_visualizer_queue = Queue()
+
+ # we are assuming both robots are the same,
+ # but this does not necessarily have to be true
+ self.manipulator_visualizer_process = Process(target=manipulatorComparisonVisualizer,
+ args=(args, self.robot1.model, self.robot1.collision_model,
+ self.robot2.visual_model, self.manipulator_visualizer_queue, ))
+ if self.args.debug_prints:
+ print("MANIPULATOR_COMPARISON_VISUALIZER: manipulator_comparison_visualizer_process started")
+ self.manipulator_visualizer_process.start()
+ # wait for meshcat to start
+ self.manipulator_visualizer_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq)))
+ if self.args.debug_prints:
+ print("COMPARE_LOGS_MAIN: i managed to put initializing (q1, q2) to manipulator_comparison_visualizer_queue")
+ # wait until it's ready (otherwise you miss half the sim potentially)
+ # 5 seconds should be more than enough,
+ # and i don't want to complicate this code by complicating IPC
+ time.sleep(5)
+
+ # NOTE: this uses slightly fancy python to make it bareable to code
+ # NOTE: dict keys are GUARANTEED to be in insert order from python 3.7 onward
+ def visualizeManipulatorRuns(self):
+ for control_loop1, control_loop2 in zip_longest(self.logm1.loop_logs, self.logm2.loop_logs):
+ print(f'run {self.logm1.args.run_name}, controller: {control_loop1}')
+ print(f'run {self.logm2.args.run_name}, controller: {control_loop2}')
+ # relying on python's default thing.toBool()
+ if not control_loop1:
+ q1 = self.lastq1
+ for q2 in self.logm2.loop_logs[control_loop2]['qs']:
+ self.manipulator_visualizer_queue.put_nowait((q1, q2))
+ if not control_loop2:
+ q2 = self.lastq2
+ for q1 in self.logm1.loop_logs[control_loop1]['qs']:
+ self.manipulator_visualizer_queue.put_nowait((q1, q2))
+ if control_loop1 and control_loop2:
+ for q1, q2 in zip_longest(self.logm1.loop_logs[control_loop1]['qs'], \
+ self.logm2.loop_logs[control_loop2]['qs']):
+ if not (q1 is None):
+ self.lastq1 = q1
+ if not (q2 is None):
+ self.lastq2 = q2
+ print(self.lastq1)
+ print(self.lastq2)
+ self.manipulator_visualizer_queue.put_nowait((self.lastq1, self.lastq2))
+
+
+
+if __name__ == "__main__":
+ args = getLogComparisonArgs()
+ visualizer = ManipulatorComparisonManager(args)
+ visualizer.visualizeManipulatorRuns()
+ time.sleep(100)
+ visualizer.manipulator_visualizer_queue.put("befree")
+ print("main done")
+ time.sleep(0.1)
+ visualizer.manipulator_visualizer_process.terminate()
+ if args.debug_prints:
+ print("terminated manipulator_visualizer_process")
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 280709b4b2a5feab1e5225782403661bc60dacec..61182930c12379c14d7521597232f02cf33ca755 100644
--- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -68,23 +68,23 @@ def getController(controller_name):
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):
+ """
+ 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
+ """
def __init__(self, queue, localQueue, _check):
super(GetCommandThread, self).__init__()
self.queue = queue
@@ -110,15 +110,6 @@ class GetCommandThread(threading.Thread):
# 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
-"""
# 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.
@@ -127,6 +118,15 @@ 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:
+ """
+ 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
+ """
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
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 801013b6d49c69f251055e50e80386920fa7acc7..93f4a1347b63b71d99f7f3fc85156c4ff7dd01ea 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -190,7 +190,6 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
if q == "befree":
if args.debug_prints:
print("MANIPULATORVISUALIZER: got befree, manipulatorVisualizer out")
- # TODO: find a way to actually close it, i don't want a bilion dangling sockets
viz.viewer.window.server_proc.kill()
viz.viewer.window.server_proc.wait()
break
@@ -198,8 +197,63 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
except KeyboardInterrupt:
if args.debug_prints:
print("MANIPULATORVISUALIZER: caught KeyboardInterrupt, i'm out")
- # TODO: find a way to actually close it, i don't want a bilion dangling sockets
- # and a random explosion caused by them
- #viz.viewer.close()
viz.viewer.window.server_proc.kill()
viz.viewer.window.server_proc.wait()
+
+# could be merged with the above function.
+# but they're different enough in usage to warrent a different function,
+# instead of polluting the above one with ifs
+def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, queue):
+ # for whatever reason the hand-e files don't have/
+ # meshcat can't read scaling information.
+ # so we scale manually
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ for geom in collision_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ viz = MeshcatVisualizer(model, collision_model, visual_model)
+ viz.initViewer(open=True)
+ # load the first one
+ viz.loadViewerModel()
+ # maybe needed
+ #viz.displayVisuals(True)
+
+ # other robot display
+ viz2 = MeshcatVisualizer(model, collision_model, visual_model)
+ viz2.initViewer(viz.viewer)
+ # i don't know if rootNodeName does anything apart from being different
+ viz2.loadViewerModel(rootNodeName="pinocchio2")
+ # initialize
+ q1, q2 = queue.get()
+ viz.display(q1)
+ viz2.display(q2)
+
+ print("MANIPULATOR_COMPARISON_VISUALIZER: FULLY ONLINE")
+ try:
+ while True:
+ q = queue.get()
+ if type(q) == str:
+ print("got str q")
+ if q == "befree":
+ if args.debug_prints:
+ print("MANIPULATOR_COMPARISON_VISUALIZER: got befree, manipulatorComparisonVisualizer out")
+ viz.viewer.window.server_proc.kill()
+ viz.viewer.window.server_proc.wait()
+ break
+ q1, q2 = q
+ viz.display(q1)
+ viz2.display(q2)
+ except KeyboardInterrupt:
+ if args.debug_prints:
+ print("MANIPULATOR_COMPARISON_VISUALIZER: caught KeyboardInterrupt, i'm out")
+ viz.viewer.window.server_proc.kill()
+ viz.viewer.window.server_proc.wait()
+