diff --git a/.gitignore b/.gitignore
index 4b08d60d9276447654672ad42b1b1277a9bd3a04..af1c19092c6423e9143b1efb01c16bd7c21eeb7d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,4 +5,4 @@ build/
**__pycache__
.vscode/
**.pickle
-*csv
+*.csv
diff --git a/python/examples/data/clik_run_001.pickle b/python/examples/data/clik_run_001.pickle
deleted file mode 100644
index a00c271660647b3458b7f932971396e12d08ac22..0000000000000000000000000000000000000000
Binary files a/python/examples/data/clik_run_001.pickle and /dev/null differ
diff --git a/python/examples/data/clik_run_001_args.pickle b/python/examples/data/clik_run_001_args.pickle
deleted file mode 100644
index aed77b21c0ed22d104f9b656933c8df387e1f497..0000000000000000000000000000000000000000
Binary files a/python/examples/data/clik_run_001_args.pickle and /dev/null differ
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
index 747204f13bea6585dc9fde6f2f60978ab70560cc..4680ab807b8ec0484caccccb3060d4052566c3bb 100644
Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ
diff --git a/python/examples/data/test2_0.pickle b/python/examples/data/test2_0.pickle
deleted file mode 100644
index fc57d202f31f4278b74dddecd55f61deb411beac..0000000000000000000000000000000000000000
Binary files a/python/examples/data/test2_0.pickle and /dev/null differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 05d53990d35dc103fc0de8322f6bb6b84fa170ec..a9240db63994c88ea41ad992b91a50761f70e460 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -178,7 +178,8 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
# first-order low pass filtering instead
# beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
- wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
wrench = Z @ wrench
#J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
@@ -347,7 +348,7 @@ if __name__ == "__main__":
path_pose.translation = path_points_3D[i]
path.append(plane_pose.act(path_pose))
- if args.viz_path:
+ if args.viz_test_path:
print(
"""
look at the viz now! we're constructing a trajectory for the drawing.
@@ -356,7 +357,7 @@ if __name__ == "__main__":
)
joint_trajectory = clikCartesianPathIntoJointPath(args, robot, path, \
clikController, q_init, plane_pose)
- if args.viz_path:
+ if args.viz_test_path:
answer = input("did the movement of the manipulator look reasonable? [Y/n]")
if not (answer == "Y" or answer == "y"):
print("well if it doesn't look reasonable i'll just exit!")
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 553d3888af1730d39cba390893138d0c52293bd7..90d134cd621bb90a419f8a13d0345bd798f314b0 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,25 +1,88 @@
-0.00000,1.11039,-1.23013,-1.62170,-1.02108,1.93437,-0.29909
-0.41667,1.11035,-1.23011,-1.62174,-1.02106,1.93439,-0.29911
-0.83333,1.11011,-1.23000,-1.62196,-1.02098,1.93454,-0.29927
-1.25000,1.10978,-1.22987,-1.62223,-1.02087,1.93475,-0.29950
-1.66667,1.10939,-1.22974,-1.62252,-1.02075,1.93499,-0.29976
-2.08333,1.10786,-1.22945,-1.62340,-1.02036,1.93598,-0.30082
-2.50000,1.10587,-1.22930,-1.62426,-1.01994,1.93730,-0.30219
-2.91667,1.10163,-1.22958,-1.62535,-1.01929,1.94016,-0.30513
-3.33333,1.07429,-1.24078,-1.62039,-1.01907,1.95955,-0.32456
-3.75000,1.06133,-1.24717,-1.61681,-1.01942,1.96883,-0.33391
-4.16667,1.04725,-1.25502,-1.61189,-1.02018,1.97898,-0.34418
-4.58333,1.01307,-1.27717,-1.59639,-1.02341,2.00376,-0.36958
-5.00000,1.00018,-1.28613,-1.58982,-1.02499,2.01307,-0.37931
-5.41667,0.99126,-1.29307,-1.58416,-1.02654,2.01951,-0.38611
-5.83333,0.98462,-1.29829,-1.57992,-1.02772,2.02430,-0.39120
-6.25000,0.94619,-1.32935,-1.55485,-1.03495,2.05185,-0.42114
-6.66667,0.93695,-1.33717,-1.54837,-1.03696,2.05844,-0.42847
-7.08333,0.91212,-1.36116,-1.52643,-1.04427,2.07601,-0.44844
-7.50000,0.90656,-1.36660,-1.52144,-1.04599,2.07993,-0.45297
-7.91667,0.89635,-1.37651,-1.51249,-1.04907,2.08710,-0.46132
-8.33333,0.88664,-1.38707,-1.50215,-1.05277,2.09389,-0.46934
-8.75000,0.88579,-1.38800,-1.50123,-1.05311,2.09448,-0.47005
-9.16667,0.88494,-1.38893,-1.50032,-1.05344,2.09508,-0.47076
-9.58333,0.88490,-1.38897,-1.50028,-1.05345,2.09510,-0.47079
-10.00000,0.88486,-1.38901,-1.50024,-1.05347,2.09513,-0.47082
+0.00000,1.20178,-1.41060,-1.33105,-1.11327,1.86782,-0.23666
+0.11494,1.20175,-1.41059,-1.33108,-1.11326,1.86783,-0.23668
+0.22989,1.20172,-1.41058,-1.33110,-1.11324,1.86784,-0.23669
+0.34483,1.20170,-1.41056,-1.33113,-1.11323,1.86785,-0.23671
+0.45977,1.20139,-1.41056,-1.33118,-1.11316,1.86801,-0.23691
+0.57471,1.14847,-1.43710,-1.30858,-1.11705,1.90490,-0.27292
+0.68966,1.11306,-1.45819,-1.29040,-1.12150,1.93063,-0.29768
+0.80460,1.06526,-1.49092,-1.26088,-1.13029,1.96580,-0.33204
+0.91954,1.02978,-1.51889,-1.23371,-1.13958,1.99182,-0.35824
+1.03448,0.94706,-1.59438,-1.15607,-1.16900,2.05165,-0.42188
+1.14943,0.86796,-1.68166,-1.05970,-1.20921,2.10733,-0.48659
+1.26437,0.86399,-1.68647,-1.05417,-1.21164,2.11008,-0.48996
+1.37931,0.86106,-1.69011,-1.04994,-1.21349,2.11211,-0.49245
+1.49425,0.85906,-1.69251,-1.04722,-1.21469,2.11349,-0.49415
+1.60920,0.85799,-1.69374,-1.04587,-1.21528,2.11423,-0.49506
+1.72414,0.85595,-1.69602,-1.04341,-1.21634,2.11564,-0.49680
+1.83908,0.85373,-1.69832,-1.04108,-1.21734,2.11716,-0.49870
+1.95402,0.84988,-1.70160,-1.03843,-1.21841,2.11981,-0.50199
+2.06897,0.84767,-1.70208,-1.03954,-1.21780,2.12132,-0.50386
+2.18391,0.84693,-1.68337,-1.07525,-1.20110,2.12180,-0.50436
+2.29885,0.84908,-1.66678,-1.10429,-1.18766,2.12029,-0.50240
+2.41379,0.86246,-1.62010,-1.17900,-1.15368,2.11101,-0.49074
+2.52874,0.88257,-1.56432,-1.26407,-1.11577,2.09697,-0.47356
+2.64368,0.89362,-1.53460,-1.30887,-1.09609,2.08920,-0.46423
+2.75862,0.91309,-1.46684,-1.41464,-1.05020,2.07538,-0.44791
+2.87356,0.91885,-1.45127,-1.43772,-1.04043,2.07128,-0.44315
+2.98851,0.93546,-1.39590,-1.52183,-1.00530,2.05938,-0.42951
+3.10345,0.93875,-1.38488,-1.53846,-0.99845,2.05701,-0.42683
+3.21839,0.94620,-1.35924,-1.57698,-0.98280,2.05164,-0.42078
+3.33333,0.95208,-1.34205,-1.60196,-0.97286,2.04741,-0.41606
+3.44828,0.95494,-1.33386,-1.61376,-0.96821,2.04535,-0.41377
+3.56322,0.95706,-1.32792,-1.62227,-0.96488,2.04381,-0.41207
+3.67816,0.95831,-1.32475,-1.62672,-0.96316,2.04291,-0.41108
+3.79310,0.95895,-1.32319,-1.62889,-0.96232,2.04245,-0.41057
+3.90805,0.95964,-1.32157,-1.63112,-0.96147,2.04195,-0.41003
+4.02299,0.96035,-1.31995,-1.63333,-0.96062,2.04144,-0.40946
+4.13793,0.96084,-1.31894,-1.63468,-0.96011,2.04109,-0.40908
+4.25287,0.96166,-1.31727,-1.63691,-0.95927,2.04050,-0.40843
+4.36782,0.96164,-1.31727,-1.63691,-0.95926,2.04051,-0.40844
+4.48276,0.96161,-1.31730,-1.63688,-0.95927,2.04053,-0.40847
+4.59770,0.91620,-1.37147,-1.57978,-0.97878,2.07285,-0.44460
+4.71264,0.90509,-1.38474,-1.56583,-0.98377,2.08069,-0.45364
+4.82759,0.87952,-1.41497,-1.53452,-0.99516,2.09858,-0.47475
+4.94253,0.85807,-1.44115,-1.50713,-1.00546,2.11345,-0.49280
+5.05747,0.84749,-1.45377,-1.49423,-1.01037,2.12073,-0.50183
+5.17241,0.82776,-1.47792,-1.46936,-1.01997,2.13422,-0.51887
+5.28736,0.81754,-1.49050,-1.45648,-1.02502,2.14116,-0.52781
+5.40230,0.80945,-1.50051,-1.44624,-1.02907,2.14662,-0.53494
+5.51724,0.80645,-1.50427,-1.44238,-1.03061,2.14864,-0.53761
+5.63218,0.80446,-1.50676,-1.43982,-1.03163,2.14998,-0.53937
+5.74713,0.80346,-1.50801,-1.43853,-1.03215,2.15065,-0.54026
+5.86207,0.80343,-1.50805,-1.43849,-1.03216,2.15067,-0.54029
+5.97701,0.80247,-1.50927,-1.43724,-1.03266,2.15132,-0.54115
+6.09195,0.80147,-1.51052,-1.43595,-1.03318,2.15199,-0.54203
+6.20690,0.79945,-1.51296,-1.43352,-1.03415,2.15334,-0.54383
+6.32184,0.79880,-1.51327,-1.43359,-1.03409,2.15378,-0.54441
+6.43678,0.79553,-1.51067,-1.44127,-1.03058,2.15595,-0.54729
+6.55172,0.79358,-1.49913,-1.46348,-1.02084,2.15721,-0.54895
+6.66667,0.79698,-1.47294,-1.50627,-1.00252,2.15489,-0.54576
+6.78161,0.81662,-1.40963,-1.59790,-0.96469,2.14157,-0.52809
+6.89655,0.82336,-1.39122,-1.62339,-0.95443,2.13698,-0.52211
+7.01149,0.84132,-1.33826,-1.69706,-0.92545,2.12464,-0.50634
+7.12644,0.84923,-1.31680,-1.72605,-0.91437,2.11918,-0.49948
+7.24138,0.85622,-1.29952,-1.74871,-0.90591,2.11435,-0.49347
+7.35632,0.86926,-1.26257,-1.79815,-0.88778,2.10528,-0.48234
+7.47126,0.87211,-1.25342,-1.81061,-0.88325,2.10328,-0.47991
+7.58621,0.87519,-1.24446,-1.82252,-0.87899,2.10113,-0.47729
+7.70115,0.87855,-1.23535,-1.83439,-0.87482,2.09878,-0.47446
+7.81609,0.88134,-1.22837,-1.84328,-0.87175,2.09683,-0.47212
+7.93103,0.88219,-1.22619,-1.84607,-0.87079,2.09623,-0.47141
+8.04598,0.88354,-1.22294,-1.85015,-0.86939,2.09528,-0.47027
+8.16092,0.88356,-1.22290,-1.85021,-0.86937,2.09527,-0.47026
+8.27586,0.88424,-1.22132,-1.85217,-0.86871,2.09480,-0.46969
+8.39080,0.88447,-1.22074,-1.85291,-0.86845,2.09463,-0.46949
+8.50575,0.88469,-1.22016,-1.85366,-0.86820,2.09448,-0.46931
+8.62069,0.88469,-1.22013,-1.85370,-0.86818,2.09448,-0.46931
+8.73563,0.85976,-1.23839,-1.84493,-0.86906,2.11171,-0.49009
+8.85057,0.82320,-1.27328,-1.81991,-0.87542,2.13667,-0.52139
+8.96552,0.77157,-1.33116,-1.77224,-0.89020,2.17119,-0.56738
+9.08046,0.75924,-1.34581,-1.75974,-0.89441,2.17930,-0.57869
+9.19540,0.74291,-1.36572,-1.74257,-0.90030,2.18994,-0.59386
+9.31034,0.73094,-1.38007,-1.73055,-0.90448,2.19767,-0.60513
+9.42529,0.71819,-1.39573,-1.71726,-0.90919,2.20584,-0.61726
+9.54023,0.71613,-1.39833,-1.71501,-0.91000,2.20716,-0.61924
+9.65517,0.71510,-1.39963,-1.71388,-0.91041,2.20781,-0.62023
+9.77011,0.71407,-1.40094,-1.71274,-0.91083,2.20847,-0.62122
+9.88506,0.71304,-1.40225,-1.71160,-0.91124,2.20912,-0.62221
+10.00000,0.71301,-1.40229,-1.71156,-0.91126,2.20914,-0.62224
diff --git a/python/examples/log_analysis_example.py b/python/examples/log_analysis_example.py
new file mode 100644
index 0000000000000000000000000000000000000000..e540fea5dfe8e19df9e1809ecbf4930d4044e577
--- /dev/null
+++ b/python/examples/log_analysis_example.py
@@ -0,0 +1,14 @@
+import pinocchio as pin
+import numpy as np
+import argparse
+from ur_simple_control.managers import getMinimalArgParser
+from ur_simple_control.util.logging_utils import LogManager
+
+
+if __name__ == "__main__":
+ log_manager = LogManager(None)
+ log_manager.loadLog('./data/ft_readings_9.pickle')
+ # pickling does not save this attribute somehow????
+ #log_manager.are_logs_vectorized_flag = False
+ log_manager.plotAllControlLoops()
+
diff --git a/python/examples/test_gripper.py b/python/examples/old_or_experimental/test_gripper.py
similarity index 100%
rename from python/examples/test_gripper.py
rename to python/examples/old_or_experimental/test_gripper.py
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index a547354a96600b510c8cddbd40dd12f16f8a1d77..d26413b033264908c12ecab921c508243c1a8e96 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,25 +1,88 @@
-0.16462,0.49814
-0.16462,0.49814
-0.16610,0.49814
-0.16759,0.49814
-0.16907,0.49814
-0.17351,0.49814
-0.17796,0.49814
-0.18536,0.49814
-0.22092,0.50270
-0.23573,0.50270
-0.25203,0.50270
-0.29351,0.50270
-0.30980,0.50270
-0.32165,0.50573
-0.33054,0.50573
-0.38387,0.50573
-0.39721,0.50573
-0.43572,0.51181
-0.44461,0.51181
-0.46091,0.51181
-0.47720,0.51637
-0.47869,0.51637
-0.48017,0.51637
-0.48017,0.51637
-0.48017,0.51637
+0.17444,0.82632
+0.17039,0.82632
+0.16837,0.82632
+0.16635,0.82632
+0.17848,0.83020
+0.25736,0.83408
+0.30590,0.83149
+0.37466,0.82891
+0.42926,0.82891
+0.56881,0.82632
+0.72050,0.81986
+0.72859,0.81857
+0.73465,0.81857
+0.73870,0.81728
+0.74072,0.81599
+0.74477,0.81469
+0.74881,0.81211
+0.75488,0.80565
+0.75083,0.79014
+0.70027,0.76042
+0.68005,0.74879
+0.62746,0.71777
+0.56477,0.67772
+0.53241,0.65316
+0.46769,0.58209
+0.44949,0.56917
+0.39691,0.50714
+0.38679,0.49422
+0.36252,0.46450
+0.34432,0.44640
+0.33623,0.43736
+0.33016,0.43090
+0.32612,0.42831
+0.32410,0.42702
+0.32208,0.42573
+0.32005,0.42444
+0.31803,0.42444
+0.31601,0.42314
+0.34837,0.43865
+0.36859,0.44770
+0.45151,0.47613
+0.47174,0.47871
+0.51825,0.48517
+0.55870,0.49034
+0.57893,0.49034
+0.61735,0.49292
+0.63758,0.49292
+0.65376,0.49292
+0.65982,0.49292
+0.66387,0.49292
+0.66589,0.49292
+0.66589,0.49292
+0.66791,0.49292
+0.66994,0.49292
+0.67398,0.49163
+0.66791,0.47096
+0.64567,0.44899
+0.62140,0.43219
+0.58702,0.40634
+0.51016,0.35207
+0.48792,0.33656
+0.42926,0.28358
+0.40500,0.26291
+0.38477,0.24740
+0.34634,0.20734
+0.33825,0.19571
+0.32814,0.18666
+0.31803,0.17762
+0.30994,0.17116
+0.30792,0.16857
+0.30387,0.16599
+0.30387,0.16599
+0.30185,0.16470
+0.30185,0.16340
+0.30185,0.16211
+0.31601,0.16340
+0.38882,0.16986
+0.44342,0.17503
+0.53039,0.18020
+0.55263,0.18020
+0.58297,0.18020
+0.60522,0.17762
+0.62949,0.17633
+0.63353,0.17633
+0.63555,0.17633
+0.63758,0.17633
+0.63960,0.17633
+0.63960,0.17633
diff --git a/python/examples/planar_dragging_via_top_contact_force.py b/python/examples/planar_dragging_via_top_contact_force.py
deleted file mode 100644
index 8e7a82458a292c5549fea9dd9999b36376d9f688..0000000000000000000000000000000000000000
--- a/python/examples/planar_dragging_via_top_contact_force.py
+++ /dev/null
@@ -1,248 +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.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 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 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'])
- # 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('--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.007)
- # 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)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- default=0.35)
- 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
-
-
-
-
-#######################################################################
-# control loop #
-#######################################################################
-
-# feedforward velocity, feedback position and force for impedance
-def controller():
- pass
-
-
-# control loop to be passed to ControlLoopManager
-def controlLoopPlanarDragging(dmp, tc, controller, robot, i, past_data):
- breakFlag = False
- # TODO rename this into something less confusing
- save_past_dict = {}
- log_item = {}
- dmp.step(robot.dt) # dmp step
- # temporal coupling step
- tau = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
- dmp.set_tau(tau)
- q = robot.getQ()
- Z = np.diag(np.ones(6))
-
- wrench = robot.getWrench()
- wrench = np.average(np.array(past_data['wrench']), axis=0)
-
- # first-order low pass filtering
- # beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
- beta = 0.007
- #wrench = beta * wrench + (1 - beta) * past_data['wrench'][-1]
-
- wrench = robot.getMtool().toDualActionMatrix().T @ wrench
- wrench = Z @ wrench
- # deepcopy for good coding practise (and correctness here)
- save_past_dict['wrench'] = copy.deepcopy(wrench)
- # rolling average
- if i % 100 == 0:
- print(wrench)
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
- dq = robot.getQd()[:6].reshape((6,1))
- # get joitn
- tau = J.T @ wrench
- tau = tau[:6].reshape((6,1))
- # compute control law:
- #vel_cmd = dmp.vel + args.kp * (dmp.pos - q[:6].reshape((6,1))) + args.alpha * tau
- vel_cmd = np.zeros(6)
- robot.sendQd(vel_cmd)
-
- # TODO find a better criterion for stopping
- if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
- breakFlag = True
- # 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['wrench'] = wrench[:6].reshape((6,))
-
- return breakFlag, save_past_dict, log_item
-
-if __name__ == "__main__":
- #######################################################################
- # software setup #
- #######################################################################
- args = getArgs()
- clikController = getClikController(args)
- robot = RobotManager(args)
-
- # calibrate FT first
- speed_down = np.array([0,0,-1,0,0,0])
- moveUntilContact(args, robot, speed_down)
- m_goal_up = robot.getMtool()
- m_goal_up.translation[2] += 0.1
- moveL(args, robot, m_goal_up)
-
- # 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 = {
-# 'wrench': np.zeros((args.max_iterations, 6)),
-# }
-# controlLoop = partial(controlLoopPlanarDragging, dmp, tc, controller, robot)
-# loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
-# #######################################################################
-# # physical setup #
-# #######################################################################
-# # TODO: add marker picking
-# # get up from the board
-# current_pose = robot.getMtool()
-# # and now we can actually run
-# log_dict, final_iteration = loop_manager.run()
-# mtool = robot.getMtool()
-# mtool.translation[1] = mtool.translation[1] - 0.1
-# moveL(args, robot, mtool)
-
- plotFromDict(log_dict, args)
- robot.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/point_impedance_control.py b/python/examples/point_impedance_control.py
index 84801cf8cf749aca9d86a7f575986992ce1da7c7..f8c5527a65a6864dba07f88d1c54e3a916fee9dd 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -11,14 +11,7 @@ from ur_simple_control.clik.clik import getClikArgs, getClikController, moveL, m
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.basics.basics import moveJPI
-#######################################################################
-# arguments #
-#######################################################################
-
def getArgs():
- #######################################################################
- # generic arguments #
- #######################################################################
parser = getMinimalArgParser()
parser = getClikArgs(parser)
parser.add_argument('--kp', type=float, \
@@ -36,10 +29,6 @@ def getArgs():
return args
-#######################################################################
-# control loop #
-#######################################################################
-
# feedforward velocity, feedback position and force for impedance
def controller():
pass
@@ -60,7 +49,8 @@ def controlLoopPointImpedance(args, q_init, controller, robot : RobotManager, i,
#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]
- wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
if not args.z_only:
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
else:
@@ -108,7 +98,8 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
wrench = robot.getWrench()
log_item['wrench_raw'] = wrench.reshape((6,))
save_past_dict['wrench'] = copy.deepcopy(wrench)
- wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
# good generic values
#Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
# but let's stick to the default for now
@@ -145,13 +136,7 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
if __name__ == "__main__":
- #######################################################################
- # software setup #
- #######################################################################
-
args = getArgs()
- if args.debug_prints:
- print("you will get a lot of stuff printed out, as requested")
clikController = getClikController(args)
robot = RobotManager(args)
@@ -183,29 +168,14 @@ if __name__ == "__main__":
controlLoop = partial(controlLoopCartesianPointImpedance, args, Mtool_init, clikController, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
-
- #moveJ(args, robot, dmp.pos.reshape((6,)))
- # and now we can actually run
loop_manager.run()
- #plotFromDict(log_dict, args)
- # plotting is now initiated in stophandler because then we get the plot
- # even if we end sooner
- #loop_manager.stopHandler(None, None)
- # TODO: add some math to analyze path precision
-
if not args.pinocchio_only:
robot.stopRobot()
if args.save_log:
+ robot.log_manager.saveLog()
robot.log_manager.plotAllControlLoops()
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
-
- if args.save_log:
- robot.log_manager.saveLog()
-
-
-
- #loop_manager.stopHandler(None, None)
diff --git a/python/examples/test_movej.py b/python/examples/test_movej.py
deleted file mode 100644
index be853026c99557a5533ae34e0542f8a42fa640d2..0000000000000000000000000000000000000000
--- a/python/examples/test_movej.py
+++ /dev/null
@@ -1,204 +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.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.basics.basics import moveJ
-import matplotlib
-
-#######################################################################
-# arguments #
-#######################################################################
-
-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-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.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('--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 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'])
- # 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.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)
-
- 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__":
- #######################################################################
- # software setup #
- #######################################################################
-
- #matplotlib.use('tkagg')
- args = getArgs()
- if args.debug_prints:
- print("you will get a lot of stuff printed out, as requested")
- clikController = getClikController(args)
- robot = RobotManager(args)
-
- # calibrate FT first
- #wrench_offset = robot.calibrateFT()
-
- # 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_item = {
-# 'qs' : np.zeros(robot.n_arm_joints),
-# 'dqs' : np.zeros(robot.n_arm_joints),
-# 'wrench_raw' : np.zeros(6),
-# 'wrench_used' : np.zeros(6),
-# }
- q_init = robot.getQ()
-
-# controlLoop = partial(controlLoopPointImpedance, q_init, controller, robot)
-# loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
-
- q_init[5] += 0.1
- moveJ(args, robot, q_init)
- # and now we can actually run
-# loop_manager.run()
-
- #plotFromDict(log_dict, args)
- # plotting is now initiated in stophandler because then we get the plot
- # even if we end sooner
- # TODO: add some math to analyze path precision
-
-
-
-
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
index 2ee2c2bb928eebc5b4fada560343da2315a9ec36..6ac766121c788ec1d445bbbec43e5015235151f4 100644
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -27,12 +27,9 @@ examples/joint_trajectory.csv
examples/path_following_mpc.py
examples/path_in_pixels.csv
examples/pin_contact3d.py
-examples/planar_dragging_via_top_contact_force.py
examples/point_impedance_control.py
examples/pushing_via_friction_cones.py
examples/test_crocoddyl_opt_ctrl.py
-examples/test_gripper.py
-examples/test_movej.py
examples/__pycache__/robotiq_gripper.cpython-310.pyc
examples/data/clik_comparison_0.pickle
examples/data/clik_comparison_1.pickle
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc
index 6cbe15e807a9d591b4daa2bb718e2ee6f594d68b..e85b1e4e44d05747ce2ca03bebd75077c0d0aa51 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 264baf9e4f1638bebd2db4c6ceb8f873c61fa407..08cab676ac3eb5e9425e00ae79331b2574792e80 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/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc
index ab90bd5b2c4a7c3713769660b0a981284e5218e1..90f7a9633aa4ec8f8144b86339c2c17b3825f0c6 100644
Binary files a/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index cc48db49956e5152a92dda0aa88ecd852b9cd0ee..b1e46579879d5b082e636e725db1b2c84e396044 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc
index ae377943be843c28581a25e2d49812d836316b2d..f8bb567b50b60daa7ddfa22992641874ea904ffb 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 7612509c01d7059305d4df9f10b48707790071a7..fa1f575f175253673488f0d4adfd5aefa7f9ad77 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -56,7 +56,7 @@ def getClikArgs(parser):
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('--viz-path', action=argparse.BooleanOptionalAction, \
+ parser.add_argument('--viz-test-path', action=argparse.BooleanOptionalAction, \
help="number of max clik iterations between path points", default=False)
return parser
@@ -204,6 +204,7 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
# compute the joint velocities.
qd = clik_controller(J, speed)
robot.sendQd(qd)
+ log_item['qs'] = q.reshape((robot.model.nq,))
log_item['wrench'] = wrench.reshape((6,))
return breakFlag, {}, log_item
@@ -220,8 +221,6 @@ def moveUntilContact(args, robot, speed):
log_item = {'wrench' : np.zeros(6)}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
- # TODO: remove, this isn't doing anything
- time.sleep(0.01)
print("Collision detected!!")
def moveL(args, robot : RobotManager, goal_point):
@@ -389,9 +388,13 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
moveL(sim_args, sim_robot, pose)
# loop logs is a dict, dict keys list preserves input order
new_q = sim_robot.q.copy()
- if args.viz_path:
- robot.updateViz(sim_robot.q, sim_robot.T_w_e)
- time.sleep(0.02)
+ if args.viz_test_path:
+ # look into visualize.py for details on what's available
+ T_w_e = sim_robot.getT_w_e()
+ robot.updateViz({"q" : new_q,
+ "T_w_e" : T_w_e,
+ "point" : T_w_e.copy()})
+ time.sleep(0.005)
qs.append(new_q[:6])
# plot this on the real robot
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc
index 7ea4cdffdac18fc953fc8e9b815ef847824199c3..2f9b2824737ebed52a6982e9abb3a3a070851fdd 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc differ
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 625e7f6cada01c136ee98a287887a025f5e1de34..af096cd0eb3d6119386239959f2086f901c097ba 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/managers.py b/python/ur_simple_control/managers.py
index d09e663410af99c37417d74fc01345c2aef2e1c4..7e2693edabb58c152f3f0480c5c5839db3cbe1d8 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -17,6 +17,8 @@ from ur_simple_control.util.logging_utils import LogManager
from multiprocessing import Process, Queue
import argparse
from sys import exc_info
+from types import NoneType
+from os import getpid
"""
general notes
@@ -98,7 +100,7 @@ def getMinimalArgParser():
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)
+ BE CAREFUL WITH THIS.", default=1.0)
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", \
@@ -191,6 +193,7 @@ class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_item, log_item):
signal.signal(signal.SIGINT, self.stopHandler)
+ self.pid = getpid()
self.max_iterations = args.max_iterations
self.robot_manager = robot_manager
self.controlLoop = controlLoop
@@ -270,7 +273,6 @@ class ControlLoopManager:
#if key not in self.log_dict.keys():
# raise KeyError("you need to provide log items you promised!")
# break
- #self.robot_manager.stopHandler(None, None)
self.log_dict[key].append(log_item[key])
# TODO: do it this way if running on the real robot.
@@ -281,10 +283,8 @@ class ControlLoopManager:
# don't send what wasn't ready
if self.args.visualize_manipulator:
if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
- self.robot_manager.manipulator_visualizer_queue.put_nowait({"q" : self.robot_manager.q,
- "T_w_e" : self.robot_manager.getT_w_e()})
-# if self.args.debug_prints:
-# print("manipulator_visualizer_queue size status:", self.robot_manager.manipulator_visualizer_queue.qsize())
+ self.robot_manager.updateViz({"q" : self.robot_manager.q,
+ "T_w_e" : self.robot_manager.getT_w_e()})
if self.args.real_time_plotting:
# don't put new stuff in if it didn't handle the previous stuff.
# it's a plotter, who cares if it's late.
@@ -323,15 +323,13 @@ class ControlLoopManager:
if self.args.debug_prints:
print("i am putting befree in plotter_queue to stop the real time visualizer")
self.plotter_queue.put_nowait("befree")
- # just fcks me over when i call again from stopHandler,
- # and that really needs to work
- #self.plotter_queue.close()
- # give it time to become free
- #time.sleep(0.1)
- # TODO: check if this is causing a delay
- self.real_time_plotter_process.terminate()
- if self.args.debug_prints:
- print("terminated real_time_plotter_process")
+ try:
+ self.real_time_plotter_process.terminate()
+ if self.args.debug_prints:
+ print("terminated real_time_plotter_process")
+ except AttributeError:
+ if self.args.debug_prints:
+ print("real-time-plotter is dead already")
if self.args.save_log:
self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
@@ -346,6 +344,12 @@ class ControlLoopManager:
nor does stopJ do anything, so it goes to freedriveMode
and then exits it, which actually stops ur robots at least.
"""
+ # when we make a new process, it inherits signal handling.
+ # which means we call this more than once.
+ # and that could lead to race conditions.
+ # but if we exit immediatealy it's fine
+ if getpid() != self.pid:
+ return
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
vel_cmd = np.zeros(self.robot_manager.model.nv)
@@ -360,41 +364,53 @@ class ControlLoopManager:
self.robot_manager.rtde_control.freedriveMode()
if self.args.save_log:
+ print("saving log")
+ # this does not get run if you exited with ctrl-c
+ self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
self.robot_manager.log_manager.saveLog()
- # set kill command and join visualization processes
- # TODO: actually send them a SIGINT and a SIGKILL if necessary
+
+ ####################################
+ # kill real-time-plotter process #
+ ####################################
if self.args.real_time_plotting:
if self.args.debug_prints:
print("i am putting befree in plotter_queue to stop the real time visualizer")
self.plotter_queue.put_nowait("befree")
- self.real_time_plotter_process.terminate()
- if self.args.debug_prints:
- print("terminated real_time_plotter_process")
-
- #self.real_time_plotter_process.kill()
- #self.real_time_plotter_process.terminate()
+ # for good measure
+ time.sleep(0.1)
+ #if type(self.real_time_plotter_process) != NoneType:
+ try:
+ self.real_time_plotter_process.terminate()
+ if self.args.debug_prints:
+ print("terminated real_time_plotter_process")
+ except AttributeError:
+ if self.args.debug_prints:
+ print("real-time-plotter is dead already")
+ #############################
+ # kill visualizer process #
+ #############################
if self.args.visualize_manipulator:
if self.args.debug_prints:
print("i am putting befree in manipulator to stop the manipulator visualizer")
self.robot_manager.manipulator_visualizer_queue.put_nowait({"befree" : "befree"})
- #time.sleep(1)
- #self.robot_manager.manipulator_visualizer_process.join()
- self.robot_manager.manipulator_visualizer_process.terminate()
- if self.args.debug_prints:
- print("terminated manipulator_visualizer_process")
- #self.robot_manager.manipulator_visualizer_process.kill()
-# self.robot_manager.manipulator_visualizer_process.terminate()
-# if self.args.debug_prints:
-# print("joined manipulator_visualizer_process")
-# self.robot_manager.manipulator_visualizer_process.kill()
-# if self.args.debug_prints:
-# print("joined manipulator_visualizer_process")
-
- # 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)
+ # for good measure
+ time.sleep(0.1)
+ #if type(self.robot_manager.manipulator_visualizer_process) != NoneType:
+ try:
+ self.robot_manager.manipulator_visualizer_process.terminate()
+ if self.args.debug_prints:
+ print("terminated manipulator_visualizer_process")
+ except AttributeError:
+ if self.args.debug_prints:
+ print("real-time-plotter is dead already")
+
+ # let's plot the loop you stopped.
+ # --> but then we stop here and can't kill the program with ctrl-c
+ # and that is insufferable. if you want the plot open the log
+ #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)
if not self.args.pinocchio_only:
self.robot_manager.rtde_control.endFreedriveMode()
@@ -432,7 +448,8 @@ class RobotManager:
def __init__(self, args):
self.args = args
self.pinocchio_only = args.pinocchio_only
- self.simulation = args.simulation
+ if self.args.simulation:
+ self.args.robot_ip = "127.0.0.1"
# load model
# collision and visual models are none if args.visualize == False
self.robot_name = args.robot
@@ -524,8 +541,7 @@ class RobotManager:
# same note as v_q, but it's a_q.
self.a_q = np.zeros(self.model.nv)
# initialize and connect the interfaces
- self.simulation = args.simulation
- if (not args.simulation) and (not args.pinocchio_only) :
+ if not args.pinocchio_only:
# NOTE: you can't connect twice, so you can't have more than one RobotManager.
# if this produces errors like "already in use", and it's not already in use,
# try just running your new program again. it could be that the socket wasn't given
@@ -534,21 +550,16 @@ class RobotManager:
self.rtde_control = RTDEControlInterface(args.robot_ip)
self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
self.rtde_io = RTDEIOInterface(args.robot_ip)
+ self.rtde_io.setSpeedSlider(args.speed_slider)
# NOTE: the force/torque sensor just has large offsets for no reason,
# and you need to minus them to have usable readings.
# we provide this with calibrateFT
self.wrench_offset = self.calibrateFT()
-
- elif not args.pinocchio_only:
- self.rtde_control = RTDEControlInterface("127.0.0.1")
- self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
- self.rtde_io = RTDEIOInterface("127.0.0.1")
+ else:
+ self.wrench_offset = np.zeros(6)
self.speed_slider = args.speed_slider
- if not args.pinocchio_only:
- self.rtde_io.setSpeedSlider(args.speed_slider)
- # TODO: make general for other robots
if args.pinocchio_only and args.start_from_current_pose:
self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
q = self.rtde_receive.getActualQ()
@@ -595,8 +606,6 @@ class RobotManager:
def getWrench(self):
return self.wrench.copy()
-
-
def calibrateFT(self):
"""
calibrateFT
@@ -1000,7 +1009,7 @@ class RobotManager:
else:
raise NotImplementedError("freedrive function only written for ur5e")
- def updateViz(self, q : np.ndarray, T_w_e : pin.SE3):
+ def updateViz(self, viz_dict : dict):
"""
updateViz
---------
@@ -1009,8 +1018,7 @@ class RobotManager:
because it shouldn't - it should only update the visualizer
"""
if self.args.visualize_manipulator:
- self.manipulator_visualizer_queue.put_nowait({"q" : q,
- "T_w_e" : T_w_e})
+ self.manipulator_visualizer_queue.put_nowait(viz_dict)
else:
if self.args.debug_prints:
print("you didn't select viz")
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc
index edcc33b2f845e875e6c72b885b613763e7842272..115adf251a4a1d068f6e90128ff1d264ae83feaa 100644
Binary files a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-312.pyc
index 6ad113721478b35bbd5d12ab47516bb8e525a9e8..ed7affc060b9b7a1b0d62afa8325a68bc6052396 100644
Binary files a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc
index 213dacf5bf110b7ab38111a6ec0543745a646fef..388924771e93bdbcaeb01a6e13de839f5951627b 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc differ
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 35be94c5823e1b794d4e860079a820d5833a2d05..a2f53dd7883b0d046ebcfd3d6c264f4d33166614 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 2c3ff4b5d9548da33d7e17b7acc30f20acf90019..6ec614f01aa747b47466c72e79dea207cfadecc0 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__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 18daa3d1842a6fdb72871be0062ebe3e3a1de737..d2682d9cab363be813bfcb81248a50cd94b44b00 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.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 a3adaa98121287f462d494b5f9a5e08e370b88be..0813aa172509b934f2449f5619bc850471096527 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/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 948637e31866e25fdedb6a586cd9be16ecf795c7..31c7e15b6f676658deaac11a78c5881d8fedf361 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -89,11 +89,15 @@ class 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)
+ if os.path.exists(save_file_path):
+ log_file = open(save_file_path, 'rb')
+ tmp_dict = pickle.load(log_file)
+ log_file.close()
+ self.__dict__.clear()
+ self.__dict__.update(tmp_dict)
+ else:
+ print("you didn't give me a correct save_file_path! exiting")
+ exit()
def plotAllControlLoops(self):
if not self.are_logs_vectorized_flag:
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc
index 68ab8a11a32c6a9fe3e82b02daf0c28c1d668557..ef461beee0f1f95021b9d5b6ac3926ed0403e6b3 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc differ
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 1dfd5ff4a37a424e79548a590c7d32a9aa37bd4d..6a671df2b112d230043f2a4c639f5bcbd088f65b 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
index 9537c1573b4cd6ace510792eeb22e1f23c9d07f1..064d192f8de93a3156ae6978bce6b2ce1984b66a 100644
--- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -44,13 +44,13 @@ class ManipulatorComparisonManager:
self.logm1 = LogManager(None)
self.logm1.loadLog(args.log_file1)
else:
- print("you did not give me a valid path, exiting")
+ print("you did not give me a valid path for log1, 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")
+ print("you did not give me a valid path for log2, exiting")
exit()
self.manipulator_visualizer_cmd_queue = Queue()
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index 82ebdf4f9eadebab5fa660fe7a0bb697dcc2fb0e..781dfc3de0c97693b0ce006dba3b64c24f11ab2d 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -38,6 +38,9 @@ class MeshcatVisualizer(PMV):
url=None,
autoclean=False,
):
+ # there will be times when i just want to drop in points
+ # which will never be changed
+ self.n_points = 0
if robot is not None:
super().__init__(robot.model, robot.collision_model, robot.visual_model)
elif model is not None:
@@ -52,7 +55,7 @@ class MeshcatVisualizer(PMV):
server = None
if robot is not None or model is not None:
- self.initViewer(loadModel=True, viewer=server)
+ self.initViewer(loadModel=True, viewer=server, open=True)
else:
self.viewer = server if server is not None else meshcat.Visualizer()
@@ -77,12 +80,24 @@ class MeshcatVisualizer(PMV):
material = materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Ellipsoid(dims), material)
+ def addPoint(self, point : pin.SE3, radius=5e-3, color=[1, 0, 0, 0.8]):
+ point_name = f"world/point_{self.n_points}"
+ self.addSphere(point_name, radius, color)
+ self.applyConfiguration(point_name, point)
+ self.n_points += 1
+
+ def addPath(self, name, path : list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
+ for i, point in enumerate(path):
+ self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ self.applyConfiguration(f"world/path_{name}_point_{i}", point)
+
def applyConfiguration(self, name, placement):
if isinstance(placement, list) or isinstance(placement, tuple):
placement = np.array(placement)
if isinstance(placement, pin.SE3):
- R, p = placement.rotation, placement.translation
- T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
+ #R, p = placement.rotation, placement.translation
+ #T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
+ T = placement.homogeneous
elif isinstance(placement, np.ndarray):
if placement.shape == (7,): # XYZ-quat
R = pin.Quaternion(np.reshape(placement[3:], [4, 1])).matrix()
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 55459a54f7a43dc95cbee1f88453003c0550f1ca..0272ad7c454588e4994d03fa27d3f09c551ef6b9 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -3,9 +3,9 @@ import matplotlib.pyplot as plt
from collections import deque, namedtuple
import time
import copy
-from pinocchio.visualize import MeshcatVisualizer
+#from pinocchio.visualize import MeshcatVisualizer
# TODO: use wrapped meshcat visualizer to have access to more nifty plotting
-#from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
+from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
import meshcat_shapes
# tkinter stuff for later reference
@@ -162,17 +162,11 @@ def realTimePlotter(args, queue):
def manipulatorVisualizer(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
- init_target_frame = False
- init_ee_frame = False
- #meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
- # TODO: use wrapped meshcat visualizer to have access to more nifty plotting
- #viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
- viz = MeshcatVisualizer(model, collision_model, visual_model)
- viz.initViewer(open=True)
+ viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
viz.loadViewerModel()
+ # set shapes we know we'll use
+ meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
+ meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
print("MANIPULATORVISUALIZER: FULLY ONLINE")
try:
while True:
@@ -185,21 +179,13 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
viz.viewer.window.server_proc.wait()
break
if key == "Mgoal":
- # having this via flag is stupid, but i can't
- # be bothered with something else rn
- if init_target_frame == False:
- meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
- init_target_frame = True
viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
if key == "T_w_e":
- # having this via flag is stupid, but i can't
- # be bothered with something else rn
- if init_ee_frame == False:
- meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
- init_ee_frame = True
viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
if key == "q":
viz.display(cmd["q"])
+ if key == "point":
+ viz.addPoint(cmd["point"])
except KeyboardInterrupt:
if args.debug_prints:
@@ -389,7 +375,6 @@ def logPlotter(args, log, cmd_queue, ack_queue):
# NOTE: this is stupid, i just want to catch the resize event
#if not (counter % 50 == 0):
if True:
- print(counter)
canvas.blit(fig.bbox)
canvas.flush_events()
else: