From a33bb545bd974ffe32a71102eaec2042c7b3d937 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 21 Sep 2024 14:31:21 +0200
Subject: [PATCH] drawing from input works ok
---
python/examples/drawing_from_input_drawing.py | 44 +--
python/examples/path_in_pixels.csv | 318 ++++++------------
.../__pycache__/managers.cpython-312.pyc | Bin 25122 -> 26575 bytes
.../clik_point_to_point.cpython-312.pyc | Bin 14530 -> 14530 bytes
.../clik_trajectory_following.cpython-312.pyc | Bin 3754 -> 3739 bytes
.../clik/clik_point_to_point.py | 8 +-
python/ur_simple_control/managers.py | 41 ++-
.../__pycache__/visualize.cpython-312.pyc | Bin 9494 -> 9494 bytes
8 files changed, 160 insertions(+), 251 deletions(-)
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 34d5150..2bf8e0f 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -195,13 +195,13 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
above_starting_write_point.translation[2] = -0.2
transf_bord_to_board = pin.SE3(rotation_matrix, translation_vector)
above_starting_write_point = transf_bord_to_board.act(above_starting_write_point)
- print("above_starting_write_point", above_starting_write_point)
- print("Mtool", robot.getMtool())
- Tinit = robot.getMtool().copy()
+ 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([
@@ -215,14 +215,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
TgoalUP.translation += np.array([0,0,0.2])
#TgoalUP.translation += np.array([0,0,0.3])
- # instantiate controller
- #clik_controller = getClikController(args)
- #controlLoop = partial(controlLoopClik, robot, clik_controller)
- #log_item = {
- # 'qs' : np.zeros(robot.model.nq),
- # 'dqs' : np.zeros(robot.model.nq),
- # }
- #loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
print("going to above marker")
robot.Mgoal = TgoalUP.copy()
@@ -235,7 +227,7 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#moveL(args, robot, Tgoal.copy())
#log_dict, final_iteration = loop_manager.run()
print("picking up marker")
- robot.gripper.move(255,255,255)
+ robot.closeGripper()
time.sleep(2)
print("going up")
robot.Mgoal = TgoalUP.copy()
@@ -251,11 +243,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#moveL(args, robot, Tinit.copy())
#log_dict, final_iteration = loop_manager.run()
print("got back")
-# robot.stopHandler(None, None)
-# robot.stopHandler(None, None)
-# robot.stopHandler(None, None)
-# print("bye")
-# exit()
"""
findMarkerOffset
@@ -279,11 +266,10 @@ def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
transf_bord_to_board = pin.SE3(rotation_matrix, translation_vector)
above_starting_write_point = transf_bord_to_board.act(above_starting_write_point)
print("above_starting_write_point", above_starting_write_point)
- print("Mtool", robot.getMtool())
+ print("current T_w_e", robot.getT_w_e())
#exit()
#moveJ(args, robot, q_init)
compliantMoveL(args, robot, above_starting_write_point)
- #Tinit = robot.getMtool().copy()
if args.board_axis == 'z':
axis_of_rot = rotation_matrix[:,2]
elif args.board_axis == 'y':
@@ -307,7 +293,7 @@ def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
moveUntilContact(args, robot, speed)
# we use the pin coordinate system because that's what's
# the correct thing long term accross different robots etc
- current_translation = robot.getMtool().translation
+ current_translation = robot.getT_w_e().translation
# i only care about the z because i'm fixing the path atm
# but, let's account for the possible milimiter offset 'cos why not
#print("translation_vector", translation_vector)
@@ -361,7 +347,7 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau_dmp)
q = robot.getQ()
- Mtool = robot.getMtool()
+ T_w_e = robot.getT_w_e()
# if args.board_axis == 'z':
# Z = np.diag(np.array([0.0, 0.0, 2.0, 1.0, 1.0, 1.0]))
# if args.board_axis == 'y':
@@ -479,12 +465,12 @@ if __name__ == "__main__":
q_init = np.array([ 1.32022870e+00, -1.40114772e+00, -1.27237797e+00, -1.15715911e+00,
1.76543856e+00, -2.38652054e-01, 2.94117647e-04, 2.94117647e-04])
#q_init = robot.getQ()
- #Mtool = robot.getMtool()
+ #T_w_e = robot.getT_w_e()
#rotation_matrix = np.array([
# [-1, 0, 0],
# [0, 0, -1],
# [0, -1, 0]])
- #translation_vector = Mtool.translation.copy()
+ #translation_vector = T_w_e.translation.copy()
# make the path 3D
path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
@@ -570,8 +556,8 @@ if __name__ == "__main__":
#######################################################################
# TODO: add marker picking
# get up from the board
- robot._getMtool()
- current_pose = robot.getMtool()
+ robot._getT_w_e()
+ current_pose = robot.getT_w_e()
# Z
#current_pose.translation[2] = current_pose.translation[2] + 0.03
# Y
@@ -585,12 +571,12 @@ if __name__ == "__main__":
first_q.append(0.0)
first_q = np.array(first_q)
#pin.forwardKinematics(robot.model, robot.data, first_q)
- mtool = robot.getMtool(q_given=first_q)
+ mtool = robot.getT_w_e(q_given=first_q)
#mtool.translation[1] = mtool.translation[1] - 0.0035
#mtool.translation[1] = mtool.translation[1] - 0.012
mtool.translation[1] = mtool.translation[1] - 0.006
if args.debug_prints:
- print("im at:", robot.getMtool())
+ print("im at:", robot.getT_w_e())
print("going to:", mtool)
print('going to starting write position')
# TODO: write a compliantMoveL - be careful that everything is in the body frame
@@ -607,7 +593,7 @@ if __name__ == "__main__":
# and now we can actually run
log_dict, final_iteration = loop_manager.run()
#loop_manager.stopHandler(None, None)
- mtool = robot.getMtool()
+ mtool = robot.getT_w_e()
print("move a bit back")
if args.board_axis == 'z' or args.board_axis == 'y':
mtool.translation[1] = mtool.translation[1] - 0.1
@@ -615,6 +601,8 @@ if __name__ == "__main__":
# mtool.translation[2] = mtool.translation[2] - 0.1
compliantMoveL(args, robot, mtool)
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
#plotFromDict(log_dict, args)
loop_manager.stopHandler(None, None)
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 726a74d..d91143f 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,213 +1,105 @@
-0.53241,0.58209
-0.53241,0.58209
-0.53039,0.58080
-0.52432,0.57951
-0.52432,0.57951
-0.52230,0.57821
-0.51825,0.57692
-0.50612,0.57046
-0.50005,0.56917
-0.49196,0.56400
-0.45758,0.55495
-0.44949,0.54978
-0.44342,0.54849
-0.43129,0.54591
-0.42522,0.54332
-0.42320,0.54332
-0.41309,0.54074
-0.41106,0.53945
-0.40904,0.53815
-0.39488,0.53557
-0.39286,0.53428
-0.39084,0.53428
-0.38882,0.53298
-0.38073,0.52782
-0.37870,0.52652
-0.37061,0.52135
-0.34634,0.50456
-0.33623,0.49939
-0.33016,0.49551
-0.30590,0.47871
-0.29578,0.47354
-0.28972,0.46966
-0.26342,0.45287
-0.25736,0.45157
-0.24927,0.44640
-0.21691,0.43348
-0.21286,0.43219
-0.21084,0.43090
-0.20680,0.42961
-0.20477,0.42831
-0.20477,0.42702
-0.20477,0.42702
-0.20882,0.42702
-0.21489,0.42702
-0.23309,0.42702
-0.24522,0.42702
-0.30185,0.42702
-0.32005,0.42961
-0.33421,0.42961
-0.37870,0.43477
-0.40095,0.43736
-0.41309,0.43736
-0.47174,0.44253
-0.48994,0.44253
-0.51219,0.44253
-0.56275,0.44770
-0.57690,0.44770
-0.58702,0.44770
-0.60926,0.44770
-0.65982,0.44770
-0.68207,0.44770
-0.68814,0.44770
-0.69016,0.44770
-0.69016,0.44770
-0.68612,0.44899
-0.68409,0.45028
-0.68207,0.45287
-0.66994,0.45545
-0.66185,0.46062
-0.65578,0.46191
-0.60724,0.47742
-0.59713,0.48000
-0.58702,0.48259
-0.54454,0.49163
-0.53039,0.49422
-0.47174,0.50456
-0.45758,0.50714
-0.44544,0.51102
-0.43331,0.51231
-0.39893,0.52135
-0.38882,0.52652
-0.38073,0.52911
-0.36859,0.53169
-0.36050,0.53298
-0.35443,0.53557
-0.34634,0.54074
-0.34432,0.54203
-0.34230,0.54332
-0.34028,0.54461
-0.34432,0.54332
-0.34432,0.54203
-0.34432,0.54203
-0.34432,0.54332
-0.34432,0.54332
-0.34432,0.54461
-0.34634,0.54849
-0.35241,0.56917
-0.35241,0.57821
-0.35646,0.58597
-0.37466,0.60406
-0.37870,0.61310
-0.38679,0.61827
-0.41309,0.63895
-0.42118,0.64412
-0.43533,0.65058
-0.47983,0.65962
-0.49398,0.66221
-0.50410,0.66479
-0.51825,0.66479
-0.54657,0.66867
-0.55263,0.66867
-0.55263,0.66867
-0.57488,0.66867
-0.57690,0.66738
-0.58499,0.66479
-0.59713,0.65962
-0.62342,0.64670
-0.62746,0.64282
-0.63353,0.63895
-0.65376,0.62215
-0.66185,0.61310
-0.66589,0.60923
-0.68612,0.58209
-0.69016,0.57434
-0.69825,0.56658
-0.71443,0.53686
-0.72252,0.53169
-0.72656,0.52265
-0.72656,0.51619
-0.74072,0.49034
-0.74072,0.48646
-0.74072,0.48259
-0.74072,0.46837
-0.74072,0.46579
-0.74072,0.46191
-0.74072,0.45416
-0.74072,0.44640
-0.73263,0.41798
-0.72859,0.41151
-0.71848,0.40118
-0.71645,0.39730
-0.69016,0.37662
-0.68207,0.37145
-0.66994,0.36887
-0.62949,0.34819
-0.62342,0.34690
-0.61129,0.34432
-0.56275,0.32881
-0.55061,0.32623
-0.54050,0.32623
-0.52634,0.32364
-0.47174,0.31847
-0.45151,0.31847
-0.43129,0.31460
-0.37466,0.31460
-0.35443,0.31460
-0.34230,0.31460
-0.30185,0.31847
-0.29578,0.32106
-0.28163,0.32364
-0.25736,0.33527
-0.24724,0.34044
-0.24118,0.34561
-0.21084,0.37016
-0.20275,0.37533
-0.19466,0.38308
-0.18657,0.38825
-0.14005,0.42961
-0.12388,0.43994
-0.10567,0.45545
-0.05713,0.48646
-0.04702,0.49680
-0.03691,0.50456
-0.01264,0.52006
-0.01466,0.52006
-0.01466,0.52135
-0.01669,0.52394
-0.01669,0.52523
-0.02478,0.53040
-0.05511,0.54203
-0.06320,0.54720
-0.13803,0.57175
-0.15421,0.57821
-0.16028,0.57951
-0.20680,0.58984
-0.22702,0.59243
-0.23915,0.59501
-0.24927,0.59501
-0.30387,0.60018
-0.31803,0.60277
-0.33016,0.60664
-0.38073,0.61052
-0.39488,0.61440
-0.40702,0.61440
-0.42118,0.61698
-0.43533,0.61698
-0.44140,0.61698
-0.44342,0.61698
-0.44949,0.61698
-0.45151,0.61698
-0.45353,0.61698
-0.46971,0.61052
-0.47174,0.60923
-0.47376,0.60535
-0.49601,0.58209
-0.49803,0.57821
-0.50410,0.56917
-0.51825,0.53945
-0.52230,0.53298
-0.52230,0.52911
-0.52432,0.52782
-0.52432,0.52782
-0.52432,0.52782
+0.44544,0.71519
+0.44544,0.71519
+0.44544,0.71390
+0.43938,0.71002
+0.43533,0.70614
+0.42724,0.70098
+0.41915,0.69581
+0.38882,0.67125
+0.37466,0.66350
+0.35848,0.65187
+0.34837,0.64282
+0.30792,0.60535
+0.29578,0.59114
+0.27556,0.57434
+0.24118,0.53557
+0.23106,0.52652
+0.22904,0.52265
+0.22500,0.51360
+0.22500,0.51231
+0.22500,0.51231
+0.22500,0.51102
+0.22702,0.50972
+0.23511,0.50456
+0.24320,0.49939
+0.28365,0.49034
+0.29376,0.48776
+0.30792,0.48517
+0.34432,0.48129
+0.36859,0.47742
+0.38882,0.47742
+0.44140,0.47742
+0.45353,0.48000
+0.48792,0.48905
+0.50814,0.49551
+0.66589,0.53428
+0.70836,0.54720
+0.76297,0.56271
+0.87016,0.59760
+0.89645,0.60923
+0.90050,0.61181
+0.90252,0.61310
+0.90252,0.61181
+0.90252,0.61310
+0.90252,0.61440
+0.89847,0.61440
+0.88836,0.61956
+0.88432,0.62086
+0.87016,0.62732
+0.86207,0.62990
+0.82364,0.64282
+0.80949,0.64541
+0.76701,0.65445
+0.74274,0.65833
+0.73061,0.66092
+0.67196,0.66609
+0.64971,0.66996
+0.63960,0.66996
+0.62949,0.67255
+0.60724,0.67513
+0.56275,0.68547
+0.54859,0.68547
+0.50410,0.69968
+0.49803,0.70098
+0.48387,0.70485
+0.48185,0.70485
+0.47174,0.70744
+0.46971,0.70744
+0.46769,0.70744
+0.46567,0.70744
+0.46365,0.70744
+0.45758,0.70744
+0.43938,0.70744
+0.38275,0.70098
+0.36859,0.70098
+0.35848,0.69968
+0.33623,0.69968
+0.33623,0.69839
+0.33421,0.69839
+0.33421,0.69710
+0.33421,0.69581
+0.33623,0.69193
+0.34028,0.68805
+0.35039,0.67772
+0.35241,0.67513
+0.36252,0.66479
+0.40095,0.61569
+0.41511,0.60406
+0.45556,0.54591
+0.46769,0.53169
+0.47780,0.51748
+0.51825,0.45803
+0.53039,0.44382
+0.54050,0.43477
+0.55263,0.41927
+0.56072,0.40764
+0.59713,0.36112
+0.60724,0.35207
+0.62949,0.32881
+0.63151,0.32493
+0.63151,0.32364
+0.63555,0.31976
+0.63555,0.31847
+0.63758,0.31847
+0.63758,0.31847
+0.63758,0.31847
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index d6e44d8e11465326921b4ad0e00348f84439a05b..bbcd15e78148acd5a796424e5d1111dd7a2a731e 100644
GIT binary patch
delta 1860
zcmZ2<gz@}&M!wU$yj%<n3=E%lze^LDzLD>YBIAt7w-pr(MN?Q?7^B!y*jgB(*f|+e
z*jpH)I5-(nI9eE@I5`<oI9nK^xHuV7xLO#ZxH%b8L?>%0$?A)x@V78V32-u`2(&On
z38o0PFhmKZ2(>Uo33D=}2)8gqiEuKch_o<7iB_^`icii`Qem92xnD_-gHd(zJ}obn
z8iuU+$%d>lo4K^x85s>WPtvJiWG%8~U|^WcrDw_LwAn?^oSE+mkJSe@22S37u1>CA
zp2>a2p_6YIS~2dKENrxzasTA~M)Hb&$xI-BK`|!-0|PV2n`amq7^XASFg8d{RuB@K
zEMP1rR>BHZ#8AVyni0Y-VVlfoD#KCAP|W1T(8M@7CsuB<f{HFPV+s4@i>C6EFIWms
z4p0`Ed_bLp2~&u3HPlGP$q#+RCz~^iO!iR`W0ahnXr!*ldy6$BzC1p)NRfeo;TB6t
zWkG6@JBZ1imy%dilvp|Wrj>GiAOizKQ3XhtIo~(4sDOciL6fOS2PD8=7@wY5mYP=#
z@}Po3ks(NsBR#bQY;2WiP<~Q=iEm<FVtQ(k9!&TUBLhS66b1%{28J8lLLK%ugvF;@
zO|+VBKhgfOu*wA<l_$dDGuS8FE^%7!wb1Kgy|CT|9=!{~dSAF0cm=NWC|=}IT%fXC
zYoXSPl=Yb_Gk1vJQBqm1u~1`$$$G1mR_kq7+Fn+&++(!gVyDFcm*ZXsy^i}H^u25y
z^o5y`H-zzlu=sS#iI&rCC)zF$SuVR!_M))<bzzf>!X}r6%`Whm6&W)yFeF369uxzU
z=Nor3UfQf}@{XDD;btj|{Y+d%U7#T82Bn0LRwArLJs=jBbr7S#=2+_~jEowaKiRBg
zlAO)Jz)&TG5(e?<sU?9HlegIK-CX40&djz5WYFTt`<?Ygmw=d}AR+@qECmtECNsMz
z>A`I)$^r@If(Ve6;HW5S2gL^qI6jJcK@JIbk(YynzZizyVDYHUQ(WAc8RtyC<1WG2
zvH7ce7bD};$qk+gj1wj=^t57JvH6Z?J|p*3kUe!EV#4GwZ&${3o9B41Ws*V?2FYy!
z5&0lu_2gtfC7D^EB*31VSWu9emtG7?=nBw;Q8ah*LO%tzZA=Ud#fK*^^bXl9?Jvp9
z*frTSNP%(v<gg$k#<P<<gC1~$QY=X0=XDH|Kg8%U6`O!F$Oa8jCb^o)g?yruePTt0
z8B>^2m{x<-F)%PNGE_2@a8F)nCz4djpvmG_1WJ%apwRtgoR?pskeOSMlbV~FSCX2d
zP?=hyqfnf$ke6SQk(rmSkXfvdo)2P_<SS$(78IoBDXwHI0>?x;$X~%A0u(2=SaS2r
zQj63lPcRax=gu!k&2ulxEGS4Vs**qnXe60OAkSJs(z<}ibk2#K*9BBA38<_nSzo!b
z^18nJC4KiBJOceboj%ujWH0i_&PZIqyukS?kH!-o!3#p}S9v@z5+nmCeAq!A#~VNv
zj46yMjHm$wO3xsi!UQW_Kmn)8><132q9Y)m9tC@kCpjm-I2GnUNfiGfOT8f3gUJh&
z7bIWh(Zb_Fc81N_!O?7*pbS*h4005<EC4F0iabEFJ(J%@S~BjKtQlp@xNmZ7lo8{N
z$rGZInHZQie~tRXtYpH>z)+k5i3NV){>skE>-?&h_*L%+h+Ytjzao(EV6t$0B%|2m
z+<1APK9GqMK}0Qx0G0Pe^&l45HLF3aIUr&ah}a4u=7NYI5OEwtoCFbfK*YVt=i(LR
zK&o39K5?WovWk9+VrFEO{lvz|D*u^{o7Mc2!erir1o=-|EUcc4pLjsz2N7OYj!!b`
utfC)+7`a$EK8tZoZb*=3?AkmhL7b8C+2k#Wv4Vx{jLwXo7{C-bfdBwd+6uD(
delta 968
zcmX?qo^jC;M!wU$yj%<n3=D-O@6u*Y+{kxEk+Ey?ZAArrmKKI6wiMPDhA4JUh7`6I
zhA0kBh7|S|hA2)>h7^t#hA1vhh7`^ghA3`Mh7{q+I!dz2A}RbWj8Oud3@HLF3{ird
z3@L&w3{gUy3@Jh_3{k?BT$-YjE0k0it2WP7(&J!Nt6|6rn;a-9Jo&MfF=ORsQ|%5$
zM*YqEbSfApTkBae+H5Y>GiT<w%479`nL$8#@-D-E#+{SBj8-%5nf%R2e)4|9fP|t(
z1_lOArdzBb@#XQUMT!gz3`I>03=GAL3=9kkMTQIv47WJaQ%ihH^7C`5M1%5^@=JUZ
z^Agiji}YZ^`xzM+iiH^%7#bLE@C)}>byoG)ch+C#SGd5TFnO0@=;RJFcg7=|FPOb!
zX1u(4ztw)G$;zh6e9j=dDnS-7=lf<(UT7P{$h7&3?G#2vsm-1CE14vxGB7Yy387dS
zpPpJ0s5RN!X)mMS<UT*K&2r9$%xv>PCM}p8>ZUKc5X2M(5g8z25r|kkd8V6^=yVWs
z28hT45xF2@=Hv@*BAmC_3**x>%Tn_uzjTwAgZNYo!&<Po&t^4ucV@<Elk>bJ7^^n7
zdv!4?^@F@1?3Z8SnOl&Pnwy$elA7XLRFq$&$yB5RvZZUXv5ytwtj&2o`Hb8k)kSp>
z;TOKHj0-mF`K@J=LJ|haZ2}SbAY$d@uK`N7Q$TjG=Oz{uWagz8GlSwr0TMe!vp}IF
zjp5wzqSU<P49`4Q*A<&X0*ja#r%yf}qQJOz@{157#{H9}LmzC4jAau5g>g|c$RbGi
zOwNs$V4OL*E#8uG+vEfB#*BL=e~C9@JU>}IA(`nh)8?LpKg^8clRHxs7;j8ol@ggG
zQWOW$9S<TBK!g#90GrqfVs(IsZV*umB5XlKJ%|9?yb{Ek1|l|qh|M4Z<e{P<5bH9C
zxH>sMRZ(sN0|P?~!zYduMpo-jA<T@dvY*%(S>-{5;wP@j>rxYhKj`qXa(t3eXBGVr
s#K^_U@mY+0vTm9*<Mhp*Y2u8GcPA&N#|j3sGdeSVVgOS`#taM$03{q5{{R30
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc
index 87d3c3d3d34dcdda2552ff64c9b31ada50dde781..8ae2b48b1fa2da019eb463d43cf61b3ecfed7067 100644
GIT binary patch
delta 40
rcmX?9c&L!;G%qg~0|Ntt+{TSuW_qk4@#XQUlU?<`L0Fp|^qV*V0lW;|
delta 40
rcmX?9c&L!;G%qg~0|Ns?VB$tDGd)({lKlLf$*y|eAgs*}`c0ew0l^IV
diff --git a/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc
index 7e831d2ca1f5b2e9c6d95aab6f71489342542efd..4239e1bd51ab18db826e0888d6072ac43e223e4d 100644
GIT binary patch
delta 93
zcmZ1_J6o3fG%qg~0|Ntt_QrP`x#w^*dQV=#t;c+gS9bC}ZW(3<KDo(%xz%Lu2#9pB
z+z}9-&OVWS2ICBq%K{1=tak*(CX`K=-R#N3!N|-YBtJQX*MXTqP;~QjUSUQ6?JyaR
delta 90
zcmbO&yGoY(G%qg~0|Ntt<RQk5+;g}Yqb9H5*5kO$EBlFok=1hYO>P-x5dDK&jpK%Z
t$Y%ywR`JP-JmTz=Wj{0Ub1H7O;^AOq=I2zL?8@uFEXyjsxr0}j5da476*K?<
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index 1bfaf26..9f43931 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -290,7 +290,7 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
# this has forward kinematics in it
- Mtool = robot.getMtool()
+ T_w_e = robot.getT_w_e()
wrench = robot.getWrench()
# we need to overcome noise if we want to converge
if np.linalg.norm(wrench) < args.minimum_detectable_force_norm:
@@ -298,15 +298,15 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
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] = Mtool.rotation
- mapping[3:6, 3:6] = Mtool.rotation
+ 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]))
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 = Mtool.actInv(robot.Mgoal)
+ 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!")
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 8baa74e..bdd49bb 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -434,6 +434,9 @@ class RobotManager:
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
+ # do it once to get T_w_e
+ self._step()
+
#######################################################################
# getters which assume you called step() #
#######################################################################
@@ -444,8 +447,19 @@ class RobotManager:
def getQd(self):
return self.v_q.copy()
- def getMtool(self):
- return self.T_w_e.copy()
+ def getT_w_e(self, q_given=None):
+ if q_given is None:
+ return self.T_w_e.copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(self.model, self.data, q_given,
+ np.zeros(self.model.nv), np.zeros(self.model.nv))
+ return self.data.oMi[self.JOINT_ID].copy()
+
# this is in EE frame by default (handled in step which
# is assumed to be called before this)
@@ -574,9 +588,9 @@ class RobotManager:
self.q = q
"""
- _getMtool
+ _getT_w_e
-----
- NOTE: private function, use the getMtool() getter
+ NOTE: private function, use the getT_w_e() getter
urdf treats gripper as two prismatic joints,
but they do not affect the overall movement
of the robot, so we add or remove 2 items to the joint list.
@@ -585,7 +599,7 @@ class RobotManager:
NOTE: don't use this if use called _step() because it repeats forwardKinematics
"""
# TODO remove evil hack
- def _getMtool(self, q_given=None):
+ def _getT_w_e(self, q_given=None):
test = True
try:
test = q_given.all() == None
@@ -684,7 +698,7 @@ class RobotManager:
# TODO: do something better here (at least a better distribution)
self.wrench = np.random.random(self.n_arm_joints)
if not step_called:
- self._getMtool()
+ self._getT_w_e()
# NOTE: this mapping is equivalent to having a purely rotational action
# this is more transparent tho
mapping = np.zeros((6,6))
@@ -721,6 +735,21 @@ class RobotManager:
qd = np.array(qd)
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ def openGripper(self):
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.move(0,255,255)
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+ def closeGripper(self):
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.move(255,255,255)
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+#######################################################################
+# utility functions #
+#######################################################################
"""
defineGoalPointCLI
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 e68a522252105a4bc3597afb84715b32d856f166..82688cdec6204a4d6ea856a59f397797cb95e1e8 100644
GIT binary patch
delta 18
ZcmbQ{HO-6jG%qg~0|Ns???z5uRRAw}1Tp{s
delta 18
ZcmbQ{HO-6jG%qg~0|Ns?)<#ZVRRAu^1P=fJ
--
GitLab