diff --git a/Dockerfile b/Dockerfile
index ac05d113161767a91153dbd2c9ae0ca6c69f5851..5939dbe18e02848d3ef01e7c6f5a8b2c505372d4 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -76,5 +76,5 @@ RUN conda config --add channels conda-forge
#RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
RUN conda install -y casadi opencv
RUN conda install --solver=classic -y pinocchio -c conda-forge
-RUN pip install matplotlib meshcat ur_rtde \
+RUN pip install matplotlib meshcat ur_rtde argcomplete \
qpsolvers ecos example_robot_data meshcat_shapes
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py
index cc84856af9aad8467a3c8f4f7d0816e00bb1d5f5..dd2ef0bcca65630320d0124a45192c4ede780f7f 100644
--- a/python/examples/casadi_ocp_collision_avoidance.py
+++ b/python/examples/casadi_ocp_collision_avoidance.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
import pinocchio as pin
import numpy as np
import time
@@ -10,6 +11,7 @@ def get_args():
parser = getMinimalArgParser()
parser.description = 'optimal control in pinocchio.casadi for obstacle avoidance'
# add more arguments here from different Simple Manipulator Control modules
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 99867e0bf4a61c04763b211d86f5d006bd960c72..0ccf6c8015ed562cef0779e1187d5bea399af2d0 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -1,15 +1,18 @@
+# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
-import argparse
+import argcomplete, argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
+
def get_args():
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)
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index ce9bd054ad34a5d5c13298002bd6d919515429cc..556f71ad5e158dea20bb854ae5c95ccc7274ae46 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
import argparse
@@ -19,6 +20,7 @@ def get_args():
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
parser = getClikArgs(parser) # literally just for goal error
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 1b7ece6e644175d79fb4d5d22a4be829a3bcd1ce..c062fce6901717262ce8b732a41886f8666b571b 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
import argparse
@@ -15,6 +16,7 @@ import crocoddyl
def get_args():
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index a9240db63994c88ea41ad992b91a50761f70e460..083c451f684f3ea9a4b9479d7d6291d59200126d 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -1,9 +1,10 @@
+# PYTHON_ARGCOMPLETE_OK
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt
import matplotlib
import copy
-import argparse
+import argparse, argcomplete
import time
import pickle
from functools import partial
@@ -48,6 +49,7 @@ def getArgs():
parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
help="are you wiping the board (default is no because you're writing)", \
default=False)
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
if args.gripper and args.simulation:
raise NotImplementedError('Did not figure out how to put the gripper in \
@@ -72,55 +74,70 @@ q: -0.5586 -2.3103 -1.1638 -1.2468 1.6492 0.262 0.0 0.0
"""
def getMarker(args, robot, plane_pose):
- # init position is the safe one above starting point of the board
- above_starting_write_point = pin.SE3.Identity()
- # start 20cm away from the board
- above_starting_write_point.translation[2] = -0.2
- above_starting_write_point = plane_pose.act(above_starting_write_point)
- compliantMoveL(args, robot, above_starting_write_point)
- Tinit = robot.getT_w_e().copy()
- q_init = robot.getQ()
- #exit()
- #moveJ(args, robot, q_init)
- compliantMoveL(args, robot, above_starting_write_point)
- print("above_starting_write_point", above_starting_write_point)
- # goal position, predefined
- Tgoal = pin.SE3()
- Tgoal.rotation = np.array([
- [ -0.73032, -0.682357, 0.0319574],
- [-0.679774, 0.730578, 0.0645244],
- [-0.067376, 0.0253997, -0.997404]])
- Tgoal.translation = np.array([0.574534, -0.508343, 0.249325])
-
- # slighly up from goal to not hit marker weirdly
- TgoalUP = Tgoal.copy()
- TgoalUP.translation += np.array([0,0,0.2])
- #TgoalUP.translation += np.array([0,0,0.3])
-
-
- print("going to above marker")
- robot.Mgoal = TgoalUP.copy()
- compliantMoveL(args, robot, TgoalUP.copy())
- #moveL(args, robot, TgoalUP.copy())
- print("going down to marker")
- robot.Mgoal = Tgoal.copy()
- compliantMoveL(args, robot, Tgoal.copy())
- #moveL(args, robot, Tgoal.copy())
- print("picking up marker")
+ # load traj
+ file = open('./data/from_writing_to_handover.pickle', 'rb')
+ point_dict = pickle.load(file)
+ file.close()
+ dmp_forward = DMP(point_dict['qs'])
+ #for q_ref in point_dict['qs']:
+ # moveJPI(args, robot, q_ref[:6])
+ robot.openGripper()
+ time.sleep(5)
robot.closeGripper()
- time.sleep(2)
- print("going up")
- robot.Mgoal = TgoalUP.copy()
- compliantMoveL(args, robot, TgoalUP.copy())
- #moveL(args, robot, TgoalUP.copy())
- print("going back")
- # TODO: this HAS to be a movej
- # in fact all of them should be
- robot.Mgoal = Tinit.copy()
- #compliantMoveL(args, robot, Tinit.copy())
- moveJPI(args, robot, q_init)
- #moveL(args, robot, Tinit.copy())
- print("got back")
+ point_dict['qs'].reverse()
+ dmp_back = DMP(point_dict['qs'])
+ #for q_ref in point_dict['qs']:
+ # moveJPI(args, robot, q_ref[:6])
+ # # init position is the safe one above starting point of the board
+ # above_starting_write_point = pin.SE3.Identity()
+ # # start 20cm away from the board
+ # above_starting_write_point.translation[2] = -0.2
+ # above_starting_write_point = plane_pose.act(above_starting_write_point)
+ # compliantMoveL(args, robot, above_starting_write_point)
+ # Tinit = robot.getT_w_e().copy()
+ # q_init = robot.getQ()
+ # #exit()
+ # #moveJ(args, robot, q_init)
+ # compliantMoveL(args, robot, above_starting_write_point)
+ # print("above_starting_write_point", above_starting_write_point)
+ # # goal position, predefined
+ # Tgoal = pin.SE3()
+ # Tgoal.rotation = np.array([
+ # [ -0.73032, -0.682357, 0.0319574],
+ # [-0.679774, 0.730578, 0.0645244],
+ # [-0.067376, 0.0253997, -0.997404]])
+ # Tgoal.translation = np.array([0.574534, -0.508343, 0.249325])
+ #
+ # # slighly up from goal to not hit marker weirdly
+ # TgoalUP = Tgoal.copy()
+ # TgoalUP.translation += np.array([0,0,0.2])
+ # #TgoalUP.translation += np.array([0,0,0.3])
+ #
+ #
+ # print("going to above marker")
+ # robot.Mgoal = TgoalUP.copy()
+ # compliantMoveL(args, robot, TgoalUP.copy())
+ # #moveL(args, robot, TgoalUP.copy())
+ # print("going down to marker")
+ # robot.Mgoal = Tgoal.copy()
+ # compliantMoveL(args, robot, Tgoal.copy())
+ # #moveL(args, robot, Tgoal.copy())
+ # print("picking up marker")
+ # robot.closeGripper()
+ # time.sleep(2)
+ # print("going up")
+ # robot.Mgoal = TgoalUP.copy()
+ # compliantMoveL(args, robot, TgoalUP.copy())
+ # #moveL(args, robot, TgoalUP.copy())
+ # print("going back")
+ # # TODO: this HAS to be a movej
+ # # in fact all of them should be
+ # robot.Mgoal = Tinit.copy()
+ # #compliantMoveL(args, robot, Tinit.copy())
+ # moveJPI(args, robot, q_init)
+ # #moveL(args, robot, Tinit.copy())
+ # print("got back")
+
def findMarkerOffset(args, robot : RobotManager, plane_pose : pin.SE3):
"""
@@ -169,7 +186,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
dmp.set_tau(tau_dmp)
q = robot.getQ()
T_w_e = robot.getT_w_e()
- Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.5]))
+ Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
wrench = robot.getWrench()
save_past_dict['wrench'] = wrench.copy()
@@ -182,7 +199,6 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
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)
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
dq = robot.getQd()[:6].reshape((6,1))
# get joint
@@ -218,7 +234,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
def write(args, robot : RobotManager, joint_trajectory):
# create DMP based on the trajectory
- dmp = DMP(joint_trajectory)
+ dmp = DMP(joint_trajectory, a_s=1.0)
if not args.temporal_coupling:
tc = NoTC()
else:
@@ -275,13 +291,15 @@ if __name__ == "__main__":
# software setup #
#######################################################################
args = getArgs()
- assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
+ #assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
args.mm_into_board = args.mm_into_board / 1000
print(args)
clikController = getClikController(args)
robot = RobotManager(args)
if args.pinocchio_only:
- robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + np.random.random(8) * 0.1
+ rand_pertb = np.zeros(8)
+ rand_pertb[:6] = np.random.random(6) * 0.1
+ robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + rand_pertb
robot._step()
#######################################################################
@@ -310,13 +328,17 @@ if __name__ == "__main__":
file.close()
plane_pose = plane_calib_dict['plane_top_left_pose']
q_init = plane_calib_dict['q_init']
+ # stupid fix dw about this it's supposed to be 0.0 on the gripper
+ # because we don't use that actually
+ q_init[-1] = 0.0
+ q_init[-2] = 0.0
# make the path 3D
path_points_3D = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
# TODO: fix thi function
if args.pick_up_marker:
- raise NotImplementedError("sorry")
- getMarker(args, robot, marker_bottom_pose)
+ #raise NotImplementedError("sorry")
+ getMarker(args, robot, None)
# marker moves between runs, i change markers etc,
# this goes to start, goes down to touch the board - that's the marker
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 90d134cd621bb90a419f8a13d0345bd798f314b0..9c1585c30bda4fe8e717e0f0f4c7640b43d057e6 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,88 +1,204 @@
-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
+0.00000,0.81031,-1.52180,-1.33221,-1.12496,2.14787,-0.53658
+0.07389,0.81027,-1.52181,-1.33221,-1.12497,2.14789,-0.53661
+0.14778,0.81023,-1.52183,-1.33219,-1.12497,2.14792,-0.53665
+0.22167,0.81020,-1.52185,-1.33218,-1.12498,2.14794,-0.53667
+0.29557,0.81017,-1.52187,-1.33216,-1.12499,2.14795,-0.53670
+0.36946,0.81014,-1.52189,-1.33212,-1.12501,2.14797,-0.53671
+0.44335,0.81013,-1.52192,-1.33207,-1.12503,2.14797,-0.53672
+0.51724,0.81013,-1.52195,-1.33200,-1.12506,2.14797,-0.53672
+0.59113,0.81066,-1.52532,-1.32436,-1.12857,2.14738,-0.53604
+0.66502,0.81313,-1.52997,-1.31241,-1.13407,2.14543,-0.53361
+0.73892,0.81716,-1.53342,-1.30155,-1.13912,2.14250,-0.52989
+0.81281,0.82171,-1.53543,-1.29315,-1.14309,2.13929,-0.52582
+0.88670,0.82724,-1.53669,-1.28540,-1.14679,2.13544,-0.52093
+0.96059,0.83184,-1.53714,-1.28018,-1.14932,2.13225,-0.51690
+1.03448,0.83631,-1.53705,-1.27616,-1.15131,2.12916,-0.51302
+1.10837,0.84050,-1.53677,-1.27281,-1.15298,2.12627,-0.50940
+1.18227,0.84541,-1.53610,-1.26959,-1.15463,2.12289,-0.50518
+1.25616,0.85111,-1.53486,-1.26676,-1.15614,2.11896,-0.50032
+1.33005,0.85843,-1.53250,-1.26461,-1.15743,2.11391,-0.49411
+1.40394,0.86563,-1.52948,-1.26384,-1.15810,2.10893,-0.48804
+1.47783,0.87190,-1.52636,-1.26411,-1.15828,2.10458,-0.48278
+1.55172,0.87795,-1.52282,-1.26538,-1.15801,2.10038,-0.47773
+1.62562,0.88392,-1.51875,-1.26771,-1.15727,2.09622,-0.47277
+1.69951,0.90041,-1.50512,-1.27861,-1.15333,2.08468,-0.45916
+1.77340,0.90575,-1.50060,-1.28240,-1.15194,2.08093,-0.45479
+1.84729,0.90934,-1.49751,-1.28507,-1.15096,2.07840,-0.45186
+1.92118,0.91379,-1.49361,-1.28851,-1.14968,2.07526,-0.44824
+1.99507,0.91829,-1.48961,-1.29210,-1.14835,2.07208,-0.44459
+2.06897,0.92280,-1.48558,-1.29576,-1.14699,2.06890,-0.44095
+2.14286,0.92991,-1.47893,-1.30211,-1.14462,2.06385,-0.43523
+2.21675,0.93876,-1.47015,-1.31094,-1.14128,2.05756,-0.42815
+2.29064,0.94672,-1.46198,-1.31943,-1.13807,2.05189,-0.42183
+2.36453,0.95384,-1.45474,-1.32693,-1.13525,2.04679,-0.41620
+2.43842,0.95924,-1.44904,-1.33302,-1.13295,2.04292,-0.41194
+2.51232,0.96371,-1.44415,-1.33837,-1.13092,2.03971,-0.40843
+2.58621,0.96556,-1.44209,-1.34066,-1.13005,2.03838,-0.40699
+2.66010,0.96723,-1.44027,-1.34265,-1.12929,2.03718,-0.40568
+2.73399,0.96909,-1.43820,-1.34494,-1.12843,2.03584,-0.40423
+2.80788,0.97007,-1.43707,-1.34622,-1.12794,2.03513,-0.40346
+2.88177,0.97104,-1.43592,-1.34754,-1.12743,2.03443,-0.40270
+2.95567,0.97183,-1.43500,-1.34859,-1.12703,2.03387,-0.40208
+3.02956,0.98021,-1.42081,-1.36738,-1.11958,2.02781,-0.39554
+3.10345,0.98413,-1.41224,-1.37951,-1.11469,2.02496,-0.39248
+3.17734,0.98630,-1.40726,-1.38663,-1.11182,2.02338,-0.39078
+3.25123,0.98825,-1.40170,-1.39489,-1.10847,2.02196,-0.38925
+3.32512,0.99178,-1.38971,-1.41308,-1.10109,2.01938,-0.38648
+3.39901,0.99388,-1.37699,-1.43341,-1.09276,2.01783,-0.38479
+3.47291,0.99463,-1.36646,-1.45083,-1.08560,2.01726,-0.38414
+3.54680,0.99453,-1.35541,-1.46959,-1.07787,2.01730,-0.38414
+3.62069,0.99367,-1.34770,-1.48318,-1.07226,2.01790,-0.38473
+3.69458,0.99142,-1.34028,-1.49715,-1.06641,2.01950,-0.38639
+3.76847,0.98744,-1.33201,-1.51364,-1.05946,2.02235,-0.38936
+3.84236,0.98387,-1.32691,-1.52454,-1.05482,2.02491,-0.39205
+3.91626,0.98007,-1.32273,-1.53405,-1.05073,2.02764,-0.39493
+3.99015,0.97297,-1.31664,-1.54893,-1.04430,2.03272,-0.40034
+4.06404,0.96487,-1.31202,-1.56207,-1.03852,2.03852,-0.40657
+4.13793,0.95750,-1.30877,-1.57248,-1.03391,2.04378,-0.41227
+4.21182,0.95264,-1.30707,-1.57863,-1.03116,2.04724,-0.41604
+4.28571,0.94704,-1.30574,-1.58469,-1.02840,2.05123,-0.42042
+4.35961,0.92888,-1.30723,-1.59508,-1.02313,2.06411,-0.43477
+4.43350,0.91125,-1.31255,-1.59912,-1.02043,2.07654,-0.44891
+4.50739,0.89782,-1.31767,-1.60067,-1.01902,2.08596,-0.45982
+4.58128,0.88341,-1.32412,-1.60095,-1.01809,2.09601,-0.47166
+4.65517,0.86673,-1.33250,-1.60004,-1.01755,2.10757,-0.48554
+4.72906,0.85684,-1.33811,-1.59859,-1.01761,2.11439,-0.49387
+4.80296,0.84799,-1.34345,-1.59684,-1.01787,2.12046,-0.50138
+4.87685,0.83791,-1.34972,-1.59464,-1.01826,2.12735,-0.51000
+4.95074,0.82482,-1.35824,-1.59135,-1.01897,2.13624,-0.52130
+5.02463,0.81200,-1.36693,-1.58772,-1.01987,2.14490,-0.53250
+5.09852,0.79943,-1.37577,-1.58383,-1.02092,2.15333,-0.54360
+5.17241,0.79432,-1.37946,-1.58214,-1.02140,2.15674,-0.54815
+5.24631,0.78517,-1.38558,-1.57998,-1.02194,2.16283,-0.55634
+5.32020,0.77680,-1.39080,-1.57872,-1.02216,2.16837,-0.56389
+5.39409,0.76801,-1.39581,-1.57824,-1.02208,2.17415,-0.57186
+5.46798,0.76077,-1.39944,-1.57877,-1.02166,2.17890,-0.57849
+5.54187,0.75399,-1.40262,-1.57966,-1.02112,2.18332,-0.58473
+5.61576,0.74798,-1.40518,-1.58094,-1.02045,2.18722,-0.59028
+5.68966,0.73951,-1.40856,-1.58320,-1.01934,2.19270,-0.59817
+5.76355,0.72436,-1.41257,-1.59082,-1.01597,2.20241,-0.61241
+5.83744,0.71848,-1.41362,-1.59469,-1.01432,2.20615,-0.61798
+5.91133,0.71308,-1.41397,-1.59928,-1.01242,2.20958,-0.62313
+5.98522,0.71019,-1.41409,-1.60186,-1.01135,2.21140,-0.62589
+6.05911,0.70676,-1.41408,-1.60518,-1.00998,2.21357,-0.62918
+6.13300,0.70306,-1.41371,-1.60937,-1.00827,2.21589,-0.63273
+6.20690,0.70004,-1.41318,-1.61319,-1.00673,2.21779,-0.63565
+6.28079,0.69766,-1.41263,-1.61639,-1.00544,2.21928,-0.63795
+6.35468,0.69534,-1.41199,-1.61971,-1.00411,2.22072,-0.64019
+6.42857,0.69475,-1.41182,-1.62058,-1.00377,2.22110,-0.64077
+6.50246,0.69418,-1.41165,-1.62141,-1.00344,2.22145,-0.64131
+6.57635,0.69359,-1.41146,-1.62228,-1.00308,2.22181,-0.64189
+6.65025,0.69304,-1.41128,-1.62312,-1.00275,2.22216,-0.64243
+6.72414,0.69304,-1.41130,-1.62310,-1.00276,2.22216,-0.64242
+6.79803,0.69305,-1.41132,-1.62305,-1.00278,2.22216,-0.64242
+6.87192,0.69306,-1.41135,-1.62300,-1.00280,2.22215,-0.64241
+6.94581,0.69445,-1.41562,-1.61467,-1.00605,2.22130,-0.64108
+7.01970,0.69512,-1.41766,-1.61067,-1.00762,2.22089,-0.64045
+7.09360,0.69537,-1.41853,-1.60901,-1.00827,2.22073,-0.64021
+7.16749,0.69577,-1.41974,-1.60664,-1.00921,2.22049,-0.63983
+7.24138,0.69715,-1.42678,-1.59373,-1.01429,2.21964,-0.63853
+7.31527,0.69804,-1.43558,-1.57836,-1.02037,2.21911,-0.63774
+7.38916,0.69842,-1.44486,-1.56263,-1.02664,2.21890,-0.63743
+7.46305,0.69833,-1.45384,-1.54779,-1.03259,2.21898,-0.63759
+7.53695,0.69730,-1.46948,-1.52265,-1.04274,2.21966,-0.63869
+7.61084,0.69683,-1.47401,-1.51550,-1.04565,2.21996,-0.63918
+7.68473,0.69639,-1.47734,-1.51033,-1.04776,2.22025,-0.63963
+7.75862,0.69544,-1.48281,-1.50207,-1.05113,2.22086,-0.64060
+7.83251,0.69339,-1.49101,-1.49028,-1.05595,2.22215,-0.64265
+7.90640,0.69051,-1.50139,-1.47560,-1.06197,2.22397,-0.64552
+7.98030,0.68779,-1.51048,-1.46293,-1.06720,2.22568,-0.64825
+8.05419,0.68377,-1.52287,-1.44593,-1.07424,2.22821,-0.65228
+8.12808,0.67956,-1.53536,-1.42890,-1.08135,2.23084,-0.65652
+8.20197,0.67475,-1.54853,-1.41130,-1.08874,2.23383,-0.66137
+8.27586,0.67229,-1.55513,-1.40250,-1.09245,2.23536,-0.66386
+8.34975,0.67021,-1.56074,-1.39501,-1.09563,2.23665,-0.66597
+8.42365,0.66812,-1.56673,-1.38687,-1.09909,2.23795,-0.66810
+8.49754,0.66535,-1.57510,-1.37530,-1.10402,2.23966,-0.67093
+8.57143,0.66201,-1.58627,-1.35944,-1.11081,2.24172,-0.67436
+8.64532,0.65992,-1.59332,-1.34941,-1.11513,2.24301,-0.67653
+8.71921,0.65884,-1.59709,-1.34400,-1.11746,2.24368,-0.67764
+8.79310,0.65846,-1.59845,-1.34202,-1.11832,2.24391,-0.67803
+8.86700,0.65809,-1.59986,-1.33996,-1.11921,2.24414,-0.67842
+8.94089,0.65681,-1.60458,-1.33308,-1.12219,2.24493,-0.67975
+9.01478,0.65510,-1.62393,-1.30083,-1.13625,2.24601,-0.68164
+9.08867,0.65495,-1.63255,-1.28570,-1.14291,2.24612,-0.68186
+9.16256,0.65513,-1.64053,-1.27128,-1.14928,2.24604,-0.68176
+9.23645,0.65530,-1.64582,-1.26165,-1.15355,2.24595,-0.68164
+9.31034,0.65549,-1.64982,-1.25427,-1.15683,2.24585,-0.68149
+9.38424,0.65572,-1.65380,-1.24687,-1.16013,2.24571,-0.68129
+9.45813,0.65585,-1.65576,-1.24322,-1.16175,2.24564,-0.68118
+9.53202,0.65671,-1.66242,-1.23029,-1.16753,2.24514,-0.68038
+9.60591,0.65735,-1.66694,-1.22143,-1.17150,2.24476,-0.67978
+9.67980,0.65839,-1.67280,-1.20969,-1.17677,2.24415,-0.67880
+9.75369,0.65904,-1.67609,-1.20301,-1.17978,2.24376,-0.67818
+9.82759,0.66053,-1.68252,-1.18967,-1.18580,2.24287,-0.67674
+9.90148,0.66248,-1.68907,-1.17556,-1.19218,2.24170,-0.67485
+9.97537,0.66471,-1.69543,-1.16147,-1.19859,2.24036,-0.67269
+10.04926,0.66692,-1.70069,-1.14938,-1.20410,2.23902,-0.67054
+10.12315,0.66870,-1.70451,-1.14037,-1.20822,2.23794,-0.66881
+10.19704,0.67060,-1.70826,-1.13137,-1.21235,2.23678,-0.66696
+10.27094,0.67228,-1.71109,-1.12428,-1.21561,2.23576,-0.66533
+10.34483,0.67369,-1.71343,-1.11839,-1.21832,2.23490,-0.66395
+10.41872,0.67641,-1.71700,-1.10874,-1.22277,2.23323,-0.66130
+10.49261,0.67856,-1.71931,-1.10207,-1.22586,2.23191,-0.65920
+10.56650,0.68119,-1.72202,-1.09413,-1.22954,2.23030,-0.65665
+10.64039,0.68443,-1.72496,-1.08506,-1.23375,2.22830,-0.65350
+10.71429,0.68618,-1.72669,-1.07992,-1.23615,2.22722,-0.65181
+10.78818,0.68815,-1.72833,-1.07469,-1.23858,2.22600,-0.64990
+10.86207,0.69072,-1.73020,-1.06837,-1.24154,2.22440,-0.64741
+10.93596,0.69272,-1.73164,-1.06349,-1.24382,2.22316,-0.64549
+11.00985,0.69489,-1.73299,-1.05856,-1.24613,2.22181,-0.64339
+11.08374,0.69764,-1.73456,-1.05262,-1.24892,2.22009,-0.64074
+11.15764,0.70649,-1.73828,-1.03600,-1.25676,2.21455,-0.63226
+11.23153,0.70998,-1.73927,-1.03034,-1.25945,2.21235,-0.62893
+11.30542,0.71301,-1.74020,-1.02532,-1.26183,2.21044,-0.62604
+11.37931,0.71667,-1.74090,-1.02005,-1.26435,2.20812,-0.62257
+11.45320,0.72094,-1.74149,-1.01436,-1.26708,2.20541,-0.61853
+11.52709,0.72789,-1.74123,-1.00739,-1.27046,2.20098,-0.61197
+11.60099,0.73379,-1.73982,-1.00376,-1.27228,2.19720,-0.60642
+11.67488,0.74115,-1.73655,-1.00209,-1.27323,2.19246,-0.59953
+11.74877,0.74838,-1.73140,-1.00415,-1.27246,2.18778,-0.59279
+11.82266,0.75286,-1.72737,-1.00701,-1.27124,2.18487,-0.58862
+11.89655,0.75811,-1.72180,-1.01201,-1.26906,2.18144,-0.58375
+11.97044,0.76343,-1.71550,-1.01830,-1.26628,2.17796,-0.57884
+12.04433,0.76798,-1.70952,-1.02484,-1.26337,2.17497,-0.57465
+12.11823,0.77182,-1.70406,-1.03111,-1.26057,2.17244,-0.57112
+12.19212,0.77438,-1.70024,-1.03567,-1.25853,2.17075,-0.56877
+12.26601,0.77752,-1.69532,-1.04168,-1.25584,2.16867,-0.56589
+12.33990,0.78123,-1.68908,-1.04958,-1.25229,2.16621,-0.56251
+12.41379,0.78605,-1.68028,-1.06114,-1.24710,2.16300,-0.55811
+12.48768,0.79018,-1.67224,-1.07196,-1.24224,2.16025,-0.55435
+12.56158,0.79368,-1.66504,-1.08188,-1.23779,2.15791,-0.55117
+12.63547,0.80026,-1.64937,-1.10440,-1.22768,2.15349,-0.54521
+12.70936,0.80301,-1.64199,-1.11534,-1.22277,2.15164,-0.54271
+12.78325,0.80601,-1.63333,-1.12836,-1.21693,2.14961,-0.53999
+12.85714,0.80806,-1.62663,-1.13869,-1.21230,2.14822,-0.53814
+12.93103,0.81107,-1.61640,-1.15452,-1.20522,2.14618,-0.53541
+13.00493,0.81274,-1.61083,-1.16312,-1.20139,2.14504,-0.53389
+13.07882,0.81365,-1.60774,-1.16790,-1.19926,2.14443,-0.53307
+13.15271,0.81409,-1.60624,-1.17023,-1.19822,2.14413,-0.53268
+13.22660,0.81453,-1.60468,-1.17264,-1.19715,2.14383,-0.53228
+13.30049,0.81497,-1.60313,-1.17505,-1.19607,2.14353,-0.53188
+13.37438,0.81582,-1.60006,-1.17984,-1.19394,2.14295,-0.53111
+13.44828,0.81740,-1.59454,-1.18839,-1.19015,2.14187,-0.52969
+13.52217,0.82028,-1.58327,-1.20611,-1.18231,2.13991,-0.52709
+13.59606,0.82251,-1.57089,-1.22637,-1.17335,2.13837,-0.52504
+13.66995,0.82367,-1.56217,-1.24095,-1.16691,2.13757,-0.52396
+13.74384,0.82414,-1.55639,-1.25089,-1.16252,2.13723,-0.52350
+13.81773,0.82443,-1.55210,-1.25828,-1.15925,2.13703,-0.52322
+13.89163,0.82424,-1.54012,-1.27978,-1.14977,2.13713,-0.52328
+13.96552,0.82375,-1.53314,-1.29259,-1.14413,2.13744,-0.52364
+14.03941,0.82298,-1.52752,-1.30322,-1.13945,2.13795,-0.52426
+14.11330,0.82230,-1.52349,-1.31095,-1.13604,2.13839,-0.52481
+14.18719,0.82164,-1.52058,-1.31666,-1.13353,2.13883,-0.52536
+14.26108,0.82085,-1.51775,-1.32236,-1.13101,2.13936,-0.52602
+14.33498,0.82042,-1.51635,-1.32520,-1.12976,2.13964,-0.52638
+14.40887,0.81918,-1.51297,-1.33225,-1.12665,2.14047,-0.52743
+14.48276,0.81866,-1.51165,-1.33502,-1.12543,2.14081,-0.52786
+14.55665,0.81839,-1.51098,-1.33645,-1.12480,2.14100,-0.52809
+14.63054,0.81812,-1.51034,-1.33780,-1.12420,2.14117,-0.52832
+14.70443,0.81784,-1.50968,-1.33921,-1.12358,2.14136,-0.52856
+14.77833,0.81755,-1.50902,-1.34062,-1.12296,2.14155,-0.52880
+14.85222,0.81726,-1.50837,-1.34202,-1.12234,2.14175,-0.52905
+14.92611,0.81725,-1.50834,-1.34209,-1.12231,2.14176,-0.52906
+15.00000,0.81723,-1.50830,-1.34216,-1.12228,2.14177,-0.52907
diff --git a/python/examples/log_analysis_example.py b/python/examples/log_analysis_example.py
index e540fea5dfe8e19df9e1809ecbf4930d4044e577..1765af9e6c38f5a2efd43f97c9caf1b14ae85525 100644
--- a/python/examples/log_analysis_example.py
+++ b/python/examples/log_analysis_example.py
@@ -1,14 +1,16 @@
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
+from ur_simple_control.visualize.manipulator_comparison_visualizer import getLogComparisonArgs, ManipulatorComparisonManager
if __name__ == "__main__":
+ args = getLogComparisonArgs()
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.loadLog(args.log_file1)
+# mcm = ManipulatorComparisonManager(args)
+# mcm.visualizeWholeRuns()
+ # TODO: put this into tabs
log_manager.plotAllControlLoops()
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 5387c0173562bc75beddf72e9861beb6f1114d9f..9651e99f1570de9982247652252dd02b50f7f27e 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
import argparse
@@ -18,6 +19,7 @@ def get_args():
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
parser = getClikArgs(parser) # literally just for goal error
+ argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index d26413b033264908c12ecab921c508243c1a8e96..bc1f19d77df1c9f6e5a337ed43e102f341f0b04b 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,88 +1,204 @@
-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
+0.64609,0.56843
+0.64609,0.56843
+0.64609,0.57243
+0.64312,0.57776
+0.64312,0.58042
+0.63868,0.58841
+0.63720,0.59907
+0.63424,0.61106
+0.63127,0.62171
+0.62535,0.63503
+0.61646,0.64702
+0.60757,0.65634
+0.59868,0.66567
+0.59127,0.67233
+0.58387,0.67766
+0.57794,0.68298
+0.57053,0.68831
+0.56165,0.69364
+0.54979,0.69897
+0.53794,0.70296
+0.52757,0.70563
+0.51720,0.70696
+0.50683,0.70696
+0.47869,0.70696
+0.46980,0.70696
+0.46387,0.70696
+0.45646,0.70696
+0.44906,0.70696
+0.44165,0.70696
+0.42980,0.70563
+0.41498,0.70296
+0.40165,0.70030
+0.38980,0.69897
+0.38091,0.69630
+0.37350,0.69364
+0.37054,0.69231
+0.36758,0.69231
+0.36462,0.69098
+0.36313,0.68964
+0.36165,0.68831
+0.36017,0.68831
+0.34684,0.66966
+0.34091,0.65768
+0.33647,0.65235
+0.33351,0.64436
+0.32462,0.62970
+0.31869,0.61106
+0.31425,0.59507
+0.30980,0.57776
+0.30832,0.56444
+0.30980,0.54978
+0.31128,0.53247
+0.31425,0.52048
+0.31721,0.50982
+0.32165,0.49251
+0.32906,0.47652
+0.33499,0.46320
+0.33943,0.45521
+0.34536,0.44722
+0.36906,0.43124
+0.39276,0.42191
+0.40906,0.41392
+0.42684,0.40593
+0.44758,0.39660
+0.46091,0.39261
+0.47276,0.38861
+0.48609,0.38328
+0.50387,0.37662
+0.52165,0.36996
+0.53942,0.36330
+0.54683,0.36064
+0.55868,0.35265
+0.56905,0.34466
+0.57942,0.33533
+0.58683,0.32601
+0.59424,0.31802
+0.60016,0.31002
+0.60905,0.29937
+0.62090,0.27539
+0.62387,0.26474
+0.62535,0.25408
+0.62683,0.24875
+0.62831,0.24209
+0.62831,0.23410
+0.62831,0.22744
+0.62831,0.22211
+0.62831,0.21678
+0.62831,0.21545
+0.62831,0.21412
+0.62831,0.21279
+0.62831,0.21146
+0.62979,0.25541
+0.63127,0.26740
+0.63424,0.27406
+0.63868,0.28338
+0.64016,0.28738
+0.64164,0.28871
+0.64164,0.29138
+0.65053,0.30203
+0.66090,0.31402
+0.67127,0.32601
+0.68164,0.33666
+0.69942,0.35398
+0.70534,0.35798
+0.70979,0.36064
+0.71720,0.36464
+0.72905,0.36863
+0.74238,0.37529
+0.75423,0.38062
+0.77053,0.38728
+0.78682,0.39394
+0.80460,0.39927
+0.81349,0.40193
+0.82090,0.40460
+0.82830,0.40859
+0.83867,0.41392
+0.85201,0.42191
+0.86089,0.42591
+0.86534,0.42857
+0.86682,0.42990
+0.86830,0.43124
+0.87423,0.43390
+0.88904,0.46054
+0.89497,0.47253
+0.90089,0.48318
+0.90534,0.48984
+0.90830,0.49517
+0.91126,0.50050
+0.91274,0.50316
+0.91571,0.51382
+0.91867,0.52048
+0.92163,0.52980
+0.92311,0.53513
+0.92608,0.54579
+0.92756,0.55778
+0.92904,0.56976
+0.92904,0.58042
+0.92904,0.58841
+0.92904,0.59640
+0.92756,0.60306
+0.92756,0.60839
+0.92460,0.61772
+0.92163,0.62438
+0.92015,0.63237
+0.91719,0.64169
+0.91719,0.64702
+0.91423,0.65235
+0.91126,0.65901
+0.90978,0.66434
+0.90682,0.66966
+0.90386,0.67632
+0.89349,0.69630
+0.88756,0.70296
+0.88460,0.70962
+0.87867,0.71628
+0.87275,0.72428
+0.86089,0.73493
+0.84904,0.74159
+0.83423,0.74825
+0.81793,0.75092
+0.80756,0.75092
+0.79571,0.75092
+0.78386,0.75092
+0.77349,0.74958
+0.76460,0.74825
+0.75868,0.74692
+0.75127,0.74559
+0.74238,0.74292
+0.73053,0.73893
+0.72016,0.73493
+0.71127,0.73094
+0.69349,0.72028
+0.68609,0.71362
+0.67720,0.70696
+0.67127,0.70030
+0.66090,0.69231
+0.65498,0.68831
+0.65201,0.68565
+0.65053,0.68432
+0.64905,0.68298
+0.64757,0.68165
+0.64461,0.67899
+0.63868,0.67499
+0.62831,0.66434
+0.61942,0.64968
+0.61350,0.63903
+0.61053,0.63104
+0.60757,0.62571
+0.60164,0.60839
+0.59868,0.59774
+0.59720,0.58841
+0.59572,0.58175
+0.59572,0.57642
+0.59572,0.57110
+0.59572,0.56843
+0.59572,0.56177
+0.59572,0.55911
+0.59572,0.55778
+0.59572,0.55644
+0.59572,0.55511
+0.59572,0.55378
+0.59572,0.55245
+0.59572,0.55245
+0.59572,0.55245
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index f8c5527a65a6864dba07f88d1c54e3a916fee9dd..acc0302fedf593c360f8cf0382af990a27288650 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -1,7 +1,8 @@
+# PYTHON_ARGCOMPLETE_OK
import pinocchio as pin
import numpy as np
import copy
-import argparse
+import argparse, argcomplete
import time
from functools import partial
from ur_simple_control.visualize.visualize import plotFromDict
@@ -25,6 +26,7 @@ def getArgs():
parser.add_argument('--z-only', action=argparse.BooleanOptionalAction, \
help="whether you have general impedance or just ee z axis", default=False)
+# argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
@@ -104,7 +106,7 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
#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
if not args.z_only:
- Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
else:
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
#Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 08cab676ac3eb5e9425e00ae79331b2574792e80..14d9c2bf74a6ca6676bc6f7fa0e0d6feada992d5 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/clik/clik.py b/python/ur_simple_control/clik/clik.py
index fa1f575f175253673488f0d4adfd5aefa7f9ad77..1e511a5d35b53a6d8396fac59e850491e60073bc 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -153,9 +153,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
SEerror = T_w_e.actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
-# print("Convergence achieved, reached destionation!")
breakFlag = True
- #J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities based on controller you passed
qd = clik_controller(J, err_vector)
@@ -200,7 +198,7 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
print("let's say you hit something lule")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities.
qd = clik_controller(J, speed)
robot.sendQd(qd)
@@ -219,6 +217,7 @@ def moveUntilContact(args, robot, speed):
controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {'wrench' : np.zeros(6)}
+ log_item['qs'] = np.zeros((robot.model.nq,))
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
print("Collision detected!!")
@@ -281,23 +280,15 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
if np.linalg.norm(wrench) < args.minimum_detectable_force_norm:
wrench = np.zeros(6)
save_past_dict['wrench'] = copy.deepcopy(wrench)
- wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- #mapping = np.zeros((6,6))
- #mapping[0:3, 0:3] = T_w_e.rotation
- #mapping[3:6, 3:6] = T_w_e.rotation
- #wrench = mapping.T @ wrench
- #Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
wrench = Z @ wrench
- #pin.forwardKinematics(robot.model, robot.data, q)
# first check whether we're at the goal
SEerror = T_w_e.actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
-# print("Convergence achieved, reached destionation!")
breakFlag = True
- # pin.computeJointJacobian is much different than the C++ version lel
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities.
# just plug and play different ones
qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * args.tikhonov_damp) @ err_vector
@@ -394,7 +385,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
robot.updateViz({"q" : new_q,
"T_w_e" : T_w_e,
"point" : T_w_e.copy()})
- time.sleep(0.005)
+ #time.sleep(0.005)
qs.append(new_q[:6])
# plot this on the real robot
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 af096cd0eb3d6119386239959f2086f901c097ba..387629268df78baa2af96f268f427ed3c2d093dc 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 5d2da4b4762b8feca20ca9b93bb177f80fef1886..11f8364de0bc46e799590039cd28d52668bf5818 100644
--- a/python/ur_simple_control/dmp/dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -1,5 +1,6 @@
import numpy as np
import argparse
+from ur_simple_control.managers import RobotManager
# TODO:
# 1. change the dimensions so that they make sense,
# i.e. shape = (N_points, dimension_of_points)
@@ -273,3 +274,26 @@ class TCVelAccConstrained:
taud = np.max((taud, taud_min))
return taud
+
+
+def controlLoopDMP(args, robot : RobotManager, dmp : DMP, i, past_data):
+ """
+ controlLoopDMP
+ -----------------------------
+ execute a set-up dmp
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ wrench = robot.getWrench()
+
+ q_ref = dmp.step()
+
+ robot.sendQd(qd_cmd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 7e2693edabb58c152f3f0480c5c5839db3cbe1d8..898d1e58d45506aa29464558598f1832c44198db 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
# TODO rename all private variables to start with '_'
# TODO: make importing nicer with __init__.py files
import pinocchio as pin
@@ -15,7 +16,9 @@ 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
+# argcomplete is an external package which creates tab completion in shell
+# argparse is argument parsing from the standard library
+import argcomplete, argparse
from sys import exc_info
from types import NoneType
from os import getpid
@@ -311,14 +314,9 @@ class ControlLoopManager:
# (because i did know about it, just didn't even think of changing it)
if not (self.args.pinocchio_only and self.args.fast_simulation):
time.sleep(self.robot_manager.dt - diff)
-
-
- if self.args.debug_prints:
- if i < self.max_iterations -1:
- print("success in", i, "iterations!")
- else:
- print("FAIL: did not succed in", max_iterations, "iterations")
-
+ ######################################################################
+ # for over
+ ######################################################################
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")
@@ -330,9 +328,14 @@ class ControlLoopManager:
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)
+ if i < self.max_iterations -1:
+ if self.args.debug_prints:
+ print("success in", i, "iterations!")
+ else:
+ print("FAIL: did not succed in", self.max_iterations, "iterations")
+ self.stopHandler(None, None)
def stopHandler(self, signum, frame):
"""
@@ -353,7 +356,6 @@ class ControlLoopManager:
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
vel_cmd = np.zeros(self.robot_manager.model.nv)
- #self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
self.robot_manager.sendQd(vel_cmd)
# hopefully this actually stops it
if not self.args.pinocchio_only:
@@ -494,8 +496,9 @@ class RobotManager:
# last joint because pinocchio adds base frame as 0th joint.
# and since this is unintuitive, we add the other variable too
# so that the control designer doesn't need to think about such bs
- self.JOINT_ID = 6
+ #self.JOINT_ID = 6
self.ee_frame_id = self.model.getFrameId("tool0")
+ #self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
self.update_rate = 500 #Hz
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
@@ -596,8 +599,8 @@ class RobotManager:
pin.forwardKinematics(self.model, self.data, q_given,
np.zeros(self.model.nv), np.zeros(self.model.nv))
# NOTE: this also returns the frame, so less copying possible
+ #pin.updateFramePlacements(self.model, self.data)
pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- #return self.data.oMi[self.JOINT_ID].copy()
return self.data.oMf[self.ee_frame_id].copy()
@@ -674,7 +677,6 @@ class RobotManager:
#pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- #self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
# wrench in EE should obviously be the default
self._getWrenchInEE(step_called=True)
@@ -721,8 +723,9 @@ class RobotManager:
q.append((self.gripper_pos / 255) * 0.025)
else:
# just fill it with zeros otherwise
- q.append(0.0)
- q.append(0.0)
+ if self.robot_name == "ur5e":
+ q.append(0.0)
+ q.append(0.0)
# let's just have both options for getting q, it's just a 8d float list
# readability is a somewhat subjective quality after all
q = np.array(q)
@@ -883,6 +886,15 @@ class RobotManager:
self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ if self.robot_name == "gripperlessur5e":
+ qd_cmd = qd[:6]
+ if not self.pinocchio_only:
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ else:
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+
def openGripper(self):
if self.gripper is None:
@@ -928,7 +940,6 @@ class RobotManager:
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- #T_w_e = self.data.oMi[self.JOINT_ID]
T_w_e = self.data.oMf[self.ee_frame_id]
print("You can only specify the translation right now.")
if not self.pinocchio_only:
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 d2682d9cab363be813bfcb81248a50cd94b44b00..05d82360395cf608e6ad774a2bfb5c43c0ebd23f 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 0813aa172509b934f2449f5619bc850471096527..a70360a9ceaab78c4f4dbc30377f789bb8b68029 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/get_model.py b/python/ur_simple_control/util/get_model.py
index c53ade8b9c173b4cf9caa6cad6d32c50bc086087..afe4dcc7af341645d0e5c74c0bfae4d1cb394dc7 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -104,7 +104,39 @@ def get_model():
def getGripperlessUR5e():
robot = example_robot_data.load("ur5")
- return robot.model, robot.collision_model, robot.visual_model, robot.data
+
+ shoulder_trans = np.array([0, 0, 0.1625134425523304])
+ shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
+ shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy),shoulder_trans)
+
+ upper_arm_trans = np.array([0.000300915150907851, 0, 0])
+ upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
+ upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy),upper_arm_trans)
+
+ forearm_trans = np.array([-0.4249536100418752, 0, 0])
+ forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
+ forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy),forearm_trans)
+
+ wrist_1_trans = np.array([-0.3922353894477613, -0.001171506236920081, 0.1337997346972175])
+ wrist_1_rpy = np.array([0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06])
+ wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy),wrist_1_trans)
+
+ wrist_2_trans = np.array([5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632])
+ wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
+ wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy),wrist_2_trans)
+
+ wrist_3_trans = np.array([9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612])
+ wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
+ wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
+
+ robot.model.jointPlacements[1] = shoulder_se3
+ robot.model.jointPlacements[2] = upper_arm_se3
+ robot.model.jointPlacements[3] = forearm_se3
+ robot.model.jointPlacements[4] = wrist_1_se3
+ robot.model.jointPlacements[5] = wrist_2_se3
+ robot.model.jointPlacements[6] = wrist_3_se3
+ data = pin.Data(robot.model)
+ return robot.model, robot.collision_model, robot.visual_model, data
# this gives me a flying joint for the camera,
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 31c7e15b6f676658deaac11a78c5881d8fedf361..da6ce8796913039329fd610dc363e29b9165b56b 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -105,7 +105,7 @@ class LogManager:
self.are_logs_vectorized_flag = True
for control_loop_name in self.loop_logs:
- plotFromDict(self.loop_logs[control_loop_name], len(self.loop_logs[control_loop_name]['qs']), self.args)
+ plotFromDict(self.loop_logs[control_loop_name], len(self.loop_logs[control_loop_name]['qs']), self.args, title=control_loop_name)
def findLatestIndex(self):
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 6a671df2b112d230043f2a4c639f5bcbd088f65b..a5a1f53946b06f0ead307dfed8f5505c1e54eb9a 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/main.py b/python/ur_simple_control/visualize/obsolete_to_be_removed/main.py
similarity index 100%
rename from python/ur_simple_control/visualize/main.py
rename to python/ur_simple_control/visualize/obsolete_to_be_removed/main.py
diff --git a/python/ur_simple_control/visualize/make_run.py b/python/ur_simple_control/visualize/obsolete_to_be_removed/make_run.py
similarity index 100%
rename from python/ur_simple_control/visualize/make_run.py
rename to python/ur_simple_control/visualize/obsolete_to_be_removed/make_run.py
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 0272ad7c454588e4994d03fa27d3f09c551ef6b9..4cf8d138f7b5685fddd9b9059c36c148eb6e6756 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -36,7 +36,7 @@ def getNRowsMColumnsFromTotalNumber(n_plots):
return n_rows, n_cols
-def plotFromDict(plot_data, final_iteration, args):
+def plotFromDict(plot_data, final_iteration, args, title="title"):
"""
plotFromDict
------------
@@ -52,6 +52,7 @@ def plotFromDict(plot_data, final_iteration, args):
ax_dict ={}
# NOTE: cutting off after final iterations is a vestige from a time
# when logs were prealocated arrays, but it ain't hurtin' nobody as it is
+ plt.title(title)
for i, data_key in enumerate(plot_data):
colors = plt.cm.jet(np.linspace(0, 1, plot_data[data_key].shape[1]))
ax_dict[data_key] = plt.subplot(int(subplot_col_row + str(i + 1)))