From 1390dac8b83f8d2f8fa05b9f4343db3e2db1bd8a Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 22 Mar 2024 16:51:49 +0100
Subject: [PATCH] some light viz work, nothing yet, just need to get something
from main
---
python/examples/drawing_from_input_drawing.py | 12 +-
.../planar_dragging_via_top_contact_force.py | 10 +-
python/examples/point_impedance_control.py | 1 -
python/examples/test_gripper.py | 1 -
.../__pycache__/__init__.cpython-311.pyc | Bin 443 -> 443 bytes
.../__pycache__/managers.cpython-311.pyc | Bin 17146 -> 17667 bytes
.../clik/__pycache__/__init__.cpython-311.pyc | Bin 219 -> 219 bytes
.../clik/clik_point_to_point.py | 7 +-
.../dmp/__pycache__/__init__.cpython-311.pyc | Bin 218 -> 218 bytes
python/ur_simple_control/managers.py | 170 +++++------
.../__pycache__/__init__.cpython-311.pyc | Bin 233 -> 233 bytes
.../urdf/__pycache__/__init__.cpython-311.pyc | Bin 238 -> 238 bytes
.../util/__pycache__/__init__.cpython-311.pyc | Bin 219 -> 219 bytes
.../__pycache__/get_model.cpython-311.pyc | Bin 4008 -> 4184 bytes
.../robotiq_gripper.cpython-311.pyc | Bin 19662 -> 19662 bytes
python/ur_simple_control/util/default_args.py | 129 +-------
python/ur_simple_control/util/draw_path.py | 34 ++-
python/ur_simple_control/util/get_model.py | 7 +-
.../ur_simple_control/util/logging_utils.py | 6 +-
.../__pycache__/__init__.cpython-311.pyc | Bin 224 -> 224 bytes
.../pin_to_vedo_plot_util.cpython-311.pyc | Bin 0 -> 5748 bytes
.../__pycache__/visualize.cpython-311.pyc | Bin 1726 -> 1726 bytes
.../visualize/pin_to_vedo_plot_util.py | 3 +-
.../visualize/vedo_manipulator.py | 285 +++++-------------
24 files changed, 200 insertions(+), 465 deletions(-)
create mode 100644 python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 35e0e69..a0b8d64 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -15,7 +15,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
@@ -295,7 +294,7 @@ def controller():
# to find option 3)
# control loop to be passed to ControlLoopManager
-def controlLoopWriting(wrench_offset, dmp, tc, controller, robot, i, past_data):
+def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -325,7 +324,6 @@ def controlLoopWriting(wrench_offset, dmp, tc, controller, robot, i, past_data):
#Z = np.diag(np.array([1.0, 0.6, 1.0, 0.5, 0.5, 0.5]))
wrench = robot.getWrench()
- wrench = wrench - wrench_offset
# deepcopy for good coding practise (and correctness here)
save_past_dict['wrench'] = copy.deepcopy(wrench)
# rolling average
@@ -387,8 +385,8 @@ if __name__ == "__main__":
robot = RobotManager(args)
# calibrate FT first
- wrench_offset = robot.calibrateFT()
- robot.wrench_offset = wrench_offset
+ # it's done by default now because it's basically always necessary
+
#######################################################################
# drawing a path, making a joint trajectory for it #
#######################################################################
@@ -396,7 +394,7 @@ if __name__ == "__main__":
# draw the path on the screen
if args.draw_new:
- pixel_path = drawPath()
+ pixel_path = drawPath(args)
# make it 3D
else:
pixel_path_file_path = './path_in_pixels.csv'
@@ -523,7 +521,7 @@ if __name__ == "__main__":
'dqs' : np.zeros((args.max_iterations, 6)),
'dmp_vels' : np.zeros((args.max_iterations, 6)),
}
- controlLoop = partial(controlLoopWriting, wrench_offset, dmp, tc, controller, robot)
+ controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
#######################################################################
# physical setup #
diff --git a/python/examples/planar_dragging_via_top_contact_force.py b/python/examples/planar_dragging_via_top_contact_force.py
index 653485e..171b444 100644
--- a/python/examples/planar_dragging_via_top_contact_force.py
+++ b/python/examples/planar_dragging_via_top_contact_force.py
@@ -5,7 +5,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
@@ -153,7 +152,7 @@ def controller():
# control loop to be passed to ControlLoopManager
-def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past_data):
+def controlLoopPlanarDragging(dmp, tc, controller, robot, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -166,7 +165,6 @@ def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past
Z = np.diag(np.ones(6))
wrench = robot.getWrench()
- wrench = wrench - wrench_offset
wrench = np.average(np.array(past_data['wrench']), axis=0)
# first-order low pass filtering
@@ -175,7 +173,7 @@ def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past
#wrench = beta * wrench + (1 - beta) * past_data['wrench'][-1]
wrench = robot.getMtool().toDualActionMatrix().T @ wrench
- wrench = Z @ robot.getWrench()
+ wrench = Z @ wrench
# deepcopy for good coding practise (and correctness here)
save_past_dict['wrench'] = copy.deepcopy(wrench)
# rolling average
@@ -214,8 +212,6 @@ if __name__ == "__main__":
robot = RobotManager(args)
# calibrate FT first
- wrench_offset = robot.calibrateFT()
-
speed_down = np.array([0,0,-1,0,0,0])
moveUntilContact(args, robot, speed_down)
m_goal_up = robot.getMtool()
@@ -230,7 +226,7 @@ if __name__ == "__main__":
# log_dict = {
# 'wrench': np.zeros((args.max_iterations, 6)),
# }
-# controlLoop = partial(controlLoopPlanarDragging, wrench_offset, dmp, tc, controller, robot)
+# controlLoop = partial(controlLoopPlanarDragging, dmp, tc, controller, robot)
# loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
# #######################################################################
# # physical setup #
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 78ad110..d1d06e4 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -5,7 +5,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
diff --git a/python/examples/test_gripper.py b/python/examples/test_gripper.py
index 366a6a5..6613fdd 100644
--- a/python/examples/test_gripper.py
+++ b/python/examples/test_gripper.py
@@ -6,7 +6,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.managers import ControlLoopManager, RobotManager
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc
index 6730f1e95d99f25500f6b1680857af2968134e4f..e6536f859f51b8b8a17c0032e2d11854161023ad 100644
GIT binary patch
delta 20
bcmdnZyqlSOIWI340|Ntto5`Pz+#47HH0A|1
delta 20
ccmdnZyqlSOIWI340|NuYx?9m3xi>HZ062XGqW}N^
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 26120b0cff823e6d4bc7f91a37548db23576a5ab..89732ed96ef8a84e5f69a8c8ee92ef71e3c71bee 100644
GIT binary patch
delta 4669
zcmey>%Glh+$hVxAmy3acfgw8iZ)$1IL_P_|JrmUfBpGvfqj*yoQW$giqWDr6QrL3%
za|NOV7#Su`kZ|K<;ACK8aA!#2Y+*>@TE@h{u$l>E1_(!qLglzy7*cr9<it`KgBdh=
zUxF0+ZRTe@$;8Mp`6jcwf)E1(Ly<6u5Mf|oxW!kTSe6=JkXT$2pOTqeQY1aufn^n2
zDgy&U@!`oISk#$s$ZAcNVcpK;!Myn@>nbLu5{bzZWJT(W%xV~G7_#7wNnx(yVqnMy
zS;tT$R3ZyzfQb~A8pagXWy}l=tC_(93=9m640&QGYS>CxU~&u$DeUN~YZ$UPVDdE#
zSqu!5CvfmEa!j7fAyxm9k%57simSLZIXShsSRpg7iaWC;wJ5P9Ge56bu}a*{(bLCD
zAtf_KAuqo~0jw-VAv5n57ebY$z%Bl?%)G>$c(_=R7$}GYK|#b)l9`(d2{KU-pS>Wz
zASX4gq(}m!L{u6i#*&>{SzH7P+gnU2B}F2WIXIQrWI*!rlMOhn8RaJDaLO_&Ozz@T
zU~2;f?#am;IAz(CI8{F~C{4c1d4`c~@&c~b$h+M9SGZ+9FtBj?Gv44Am|${6SZxL4
zMPbcL{F)c|HSY?DPRLo{xS;l;faw(h(+gnqR6z8CnEqC|12Pu{{H_T2T@dhlz%SfW
zJ0s<aoY4h-qYE5HMM|43xqF$Iic}__;Pn;L0XYoh!COpu1-DqMQj7A7i-aa?^I1o6
zf}}uRDpCWnz-dGvH7~`jC^a>uD6=fpH$Np+leGxsw_9w*ndy0@xwlx;iV|~Ei{wEe
z!Csb{6Q7)$Qq0Q0z@PvEMG7F31STKgQ|8$N^80rN28ITPyOZDZePvXd{FGn2UJ<0i
z1VjihFfgoSC{hBk!TPvCED(_b(&_`&+Q0yTADF~h6+bY*2_tq^jt>Ibti~T0;6xN7
z2P?-1As$xi4-A+D#3DxS$rA)58B-=NWK^DfRzR4Mck)94F_{#u7KSKpPKFfj7KSJu
zPKFeo7KSL^6t-XnO}@#zf+~y|n~em!87Ds$Qktx8Cc#s~5D!WL;DibiWXckrJV8-%
za*i<9<R?OkP%fkB<WNCnX?V^l?yF(U0;OTFzT);0-N}Z+vXkctt231tPxcp<<(2}=
zFfcHbn1lGdps)uE)i5lOoxIRYiKT`iUT$)NqR8Zn!j}BbDCXHrjuz1XnZzrCY7(!A
ztTWuC6t)_Mcu@R-%?0y7p%3A)*D%C`VhF<HsA0&0^QyQfzZI35d|iZ>QF*eisHDC!
zSP=sQLk&Ybg3Xwv0uxW+&IWt7h`WX{3#`WwE?2`4k6<GQihK=27Tm21R40El6=i13
zQkgtKQA~)DA%&*~$$~1@$>w6axy=|D7&5;wGB6aWO#UmT#V9gaL)@H^eR6_$03+Mv
zjpAbUMdqNSWdTaGOnHU37z=MP<(1uHE{iX`#he&lSOm@`pdzct5~L7RK;7agDoII=
zFG@{L%`8hT0wwWV>|mkH{9D|~i8+}`MTsS;ZXvgL%ZpO;k~8A-)6$AlOK$Px#U~c!
z#%JYc=9LuR;_&kK^qVXpA;hRPSzAJ1=@wgVVnuvmiY5~{UloCJT`?$uC@4TOkK*L@
z5>kARK{;R+6DafC5RsUCQ^JdpZ?cM{8Y9<aZ%Mz@uN(|QB2y%0sLautqkB<M<BFh0
z2TKpzS2hL_u_+caT;_Pq@x3UlbwyaKgQbV#sjw)f-~%bSIr<$eJ^VKWM5ge6U=ZW9
zSfO%7%NES~%EiDdH2I>Wwaf<&1|_xYiiVdI4KFGhUr{vfaJj-GcY|N}3ctc+K`A+b
z4_pjFVpjxJJ}@wITHIAInQS7Z%gM(eE7!qtg+p?3rj#@H0~v)YGP)3cr<C&KTT=Rl
zAA}ghrDnKXk<q;<rrW{N!`;E%!41x$$>1~tO+%ny2gS>0o5_XJP0UTTOq2O!RVPc!
zm@qRgU<D-(MwZF{WK5%3YglW*^-?Wc4O<OE78}$6253DcQvxyyDwxF%WiTMCtYHJy
zYs(lJ7*@lbz{t?kJXuCfL#YH*KR|V5f$Me<2UOjyVT07ilOGz12$aBeg0e*xHz?T(
zFoLWBm1`g^3=4R``8b$ClYO$7T#czFFC_Va3h5$sQ1XibrBlK5)Dp+!lG4PS5a$55
z{G#MkNF}5NQlUGUM_$r99>kPnU|`T>2PdXm+-W88MX8A?nR)5O;3B$64`dWmT1k-+
zNUbp_Re}5tDytP>DK3tIfnjpLycD}9BLhS6KZeO0<Q+BHIki79uybm6INlYJnBqCn
zdy035>kUECD}qYb1x+pqnp_k#yCP`T;WRl+Au2*veooX1?j4F3WNj|W+FX&f0Y$dd
zT@K!klq-A+7daHKa425jPy{C%#q0cPm-y8#@@rh-*SNr;ff?zbxC2Le(B!`gp)55F
z3z$GbrozZj0&+Y!6d4$@K)Dpmu3^Z6NABc&BZ<kkO?X6>u`n>KhARO_0o&yHMskyw
z6($$R@JZD$#Pfi4g9|W3ddcFQte_-W&kG7x{#z{RsU?B8SV3gUEzb1Rl5lWdzr|dT
znRkmnIX}0cv?SFrCnqGeD7UyM02Ii<AR+=pM1caIIo~(4C=tX3#ndg<kofZW)LYCc
zDTPJRpbC%?9CgK@bOR{?Zm|@Xq)y(hB*In%ijSJf7nS7eZ*cJR^L6oE<dC?+A#s62
z;wu{iC(jiQX;4;Q!Lo*PgUJ@N9VUA$E^@eB;c&UY;c`P%a!TZs*aap_Y_EtKUKcgH
zBx-h1)Z&V$MF&d{*9SHR5s4|5Q*1%SLkC9(2P6_e<q#}PKoJ1KpKCzLBSE>x1XNTN
zd4qfs3nD-b$0ATo4K2!Uv6dy~l%}E-W~q}ER1_GSCp)SHGAeDJtn!gj2%NaN;E6l1
zNMv%qnvM&&fdww5vp|M}!VhY05r~aqYCfnHC>WHVlwabTn3tHITBHYxph=Vet6gJM
z+I&r&kD0HAAq$kDz`lXxxcXoQO;*2<B9MQIYC)EGg5p^G7IR`w4mb<m;&Uu1DauSL
zElG7PD#|Z{coyW~qB@XzZ~*v%SoI*H0Yre^4RT=-D0+)P^*5xl0ehnelmZ0P@{7t7
zizb&UiJ6pu^ng=CDadjbaOB@&FN{ymEKAM1#Zr=5TmtbhDCvT-X_3O@^V%wmTQ`5v
z&S4a~!7toX)nDILf019|3ctby4u#1@x>_QAj0_CZL8T1XzaXM`+2#$pK8%x3DDp5Z
z-~weIkpG$diol-cMtJQOHzF=lz|~O^$cwkQ(~B|-3Q~*W%TjY7B@ZHYKnWY{UuYEF
zVk%5QiJ<~e3<;sekW^|(V9H}ql3ojL*?|iKr4J0EoKlk)>32zpOLn;Qcz`@u(O=tD
z+gaaJe}O};NNKW*!BabMXo6!R6BL3WpumIFptsl{)i1LBw|L+=H7K#XN^)|dp%_b0
zVmZs?3PV@T8~majypRMB%5|X70%1^qgA@D`22fy@FfvSTR2H5*O<4(4UQC{$C@?uo
zkxK+rN7OLH!wVcx;WK$6n~q~JLlL;pU`L97gp0vtbuY;6;B-<BDv3e1L-Kr4At;l@
z8%l6<Gchm}+kpdU@&_dur4uYyJfbd&MS-eFP}o#o=a;|4FMp9=@d`hvNG(#@e8kX_
zk<o1OKO?cwG>`@0EDCPNSb@07AOc)#fKwx9W?o5ZI;iOaDXu_iSCbi>4vV@$$(5}z
z1>C3yg$1~dn4D>>z$iYs%ebFWX|t5cQzn)grUfFPB*j+C%*jxqJz3jal~HVRpt(A~
zJ}4w1xWtfwVREg7#N_$rqKw9qx0;*tf|~shNrnZ|AOj{#TdZZYoP5$kkx_Q@YYRz6
zMzhVVR&9({az&s_UIcP+Q47ceh@usd3=x?LoHPAF>U1U_wYFr8nf%{cOav6tw^%dt
z3Q9|Eu@)EPWR?_(P1du~wFb2>!MUsmlnaZNfxOL<o}ZX=i!t>UCx{WBlUZB>u0o4J
zy-`p`1mPlu$<u6%*o;Au<gxjjjUyxP2Mz{Nsp(#mye9fg@!2eAC%_1)JNSy6K<;q{
z5iTGC)Q~O$)wM+xATGH01bea_#0BM;qCOA{loX1lf>^6Sge!<Z^pV^_To7>|l!88k
z6AGk<)WGn8BZ`q#?}Hfwn1YZMOq{G7A4CLMH9s)G2{lnxjt@FqpbizBkeGbWUf)ac
z1A{V17*2RW`c&eq@}SNalz?=;SXsqBFu(~uCRU{n3~+*DvWJ6-JJ{VI*J_I2;w;F_
z%TG?u$jk>v>n+y2(%gbdNM->yd%#H+l<JDW*#?v!i@-Gv>*Tu*I^3XE0l3g9;+rhu
zs3r_*3f$sO&d<q7O$HAD6md-Ub=0=u1ceZ&wN(VNxCk};esS33=BJeAq}mm21P@Ly
zFch0HGBA8#W@KdiATW80qw?gJj&n*=7#XcUFkmM?g2lgpNvIkhW)Vh~4_bAsjG`YH
Ru#+Fb;$OfdrW$Z@1OTdxJ#hd4
delta 4431
zcmZqfV*J(0$hVxAmy3acfgxP-cPd-%L_P_|6BE?~B-wI!bNQn97#UI+QW$giqxiwJ
zK$O752@>fk3@MyBqPb#GVvG!&44e#14DJjmTrCVK+{>647*;cZ%md*lai|<m3quMo
znw&%mV=#jz-%F4}KTXD4g6^pWsU;=(VVT9Hi8+~7sYM&#on&I<oV<hCT|t<EfuTqQ
zM2IplFx=uRPAp4}FGwsdiBHK)E-8|kEYGrvEscSJq4?<J^DOGDAJ`b=v?u>$+0Jx?
zY4ax5RZNo~aL6(;PyWXt6{sn2i$5(hFEJ-Rvm~`Bu_QA;uSgtZv=GQ>mXgfeRESk#
zAU=CRenC!ZT1k;4NQn$ch$TC<vbYH3q+3iWB}Jl>*KjJa$%3R6CST#SW|W^Sz$MG5
zI9ZQNL9-L&rWQs9h6aWk@(LX+S2$$u3W`lAxgxH0QBeDep!Np_RZi89462jMxy~^1
zOm^XJmHZ&WARy9HJ0s<aoY4h-qYE5HMam2e49U!sKN^c{w&p2eVtUCqc_W{%m^R2#
zklSxD<rUmwtx7G*FD??EEW&Rs0E*NikaLSvL5f5t$MZ`VfgE{@tvEA1uQc}-Yg$oa
zZfcPN$V=>HsX6h<xhci03=9kkAW)<T(k3{0CBHJy5s)XoGk`qtbn*@UuZ+r*&k1PP
zD}huPg9t$e28NXkU{^x)@qk1@L@G$D4_Iph0|b6xl4Mo<zyK%A*jYI~2pF;&e_((U
zX^db&URLW5444GOB59EM$)Mx|!?1++Ic{<Tqw?f!g2K8yDeNtbQ5>8MDI6^fQJkC%
zDV!|~QCyth^cuy@$&kX`!Vtxi!WPV+$vgRvpbDeNW<{ZH#>sUelDrIAj4-2%<R`BY
zR+=1X!6#S45D)S%SSD{{4PzF}T884h64lAMS~5%}>XQ?Bq$cZ#@G@$HSll%X@u1WM
z(lhzIh$c%7V;1w|I#EYvn-cZO7er+z>x*zPYJ*suFm()*&xvXzrLfg7#DkI^NGk)F
zCk19QFfgRB*D%CO!+9Jv3|Vkq6*mJz4MRLA0e}_MFlK=wAIwhS%m(|bh#Mq2`Gc6G
zK3uMbAs)eI%#s6Z0NKTjuE!8ASHlpGU^61ZtXRH=Axi>g!UFlp2YE!98MEXj2U>`E
zFf!CIW`WWkSiXiK9?nkTtU<E4iWTe;P-+CLOyNP5o!lTMHTjen4_}EVSRPDdsZJK;
zm6%*3zLVROfq^0O3nK$Vk=bMi2`xs^$=MR-j2x2}O9Vu-gOh%8eqLT`a>*@@#N?99
zvc!_qTg(NSdAHc}QcFvU5_4`b78Zf?84D;)TY?BsCb`8^RFaY!UzD1hnpu`w#0EAB
zER>mliz9DxjifxI=Hxk&`k_VOa)muNu_C@OCFK?ygmH^2JwGugKDDSQzo^I@WU2+o
z2;TCd)V$=3`24iA;?xpNrXo;m6oCr8Vo)+uP=MqwrO9t4r35~LGFLMbC|`cyVGxy^
zEGp&2$Tm4aN{vx$a=VnD#RmxnY56(&9V|WkH@Nw)aLavQ5ahJDD=IN1dV$Cisf(gI
zS44F>TzcF(+-?YpPLcn>z|U#%lt*x~m9#bM4SwOCn#mKT<ygP+FesW%-Y>1o!NVXa
z)xpxkJNc`$v%m*723f@`GP<BrP4Wi6@MI|&xyflV`hGX$l&;7bf`p|%a4|?o&j`69
zt9MacuY;wB=Yg2y43{f1x);TCJ6L+SKd`}F_*7PYPQ-%5nbC8iL0Y9QaDY&e^5iu#
zl3WXz7#OBAq%+hqPIlyxpZrcHiuG(QQwi&2Lqn0tm1g{t3uH~08M2rs8*1vZveqzU
zu}xmXCO0`jl#i{Jtp+4ft*OIO%U;7ciCv7ThIR5@SyO$E8jc#~DlP_wTFx5I8ip)T
znF&rm*<i9rrUVqzU^W9o7Wd?fCZgh;Aoa@_85mZB^AJdqk)h|@<o`_KlXt2KCX~Q4
zD+5CoA4nR6YZy~l*RY}1fVEsTTr~{w{2+M{E`b{e&e~k4su>w-xPlopIVQ8q*VI?>
zCzhoaC8nn;q?IUuDlAO_NE!vz1x0G0)D#6uO<d`zC637@rHMI#w*=wr5a$55{G#O4
zB2Z$!#h;#9;+3D7R}zw6R9Ko?d`keX(7CjzC^ZjS^=X1k)d3Oa^&qoZ6N`!xD{rww
zT~Y)p={32*>Gc+OT1k9SYGO)eUV1UOf+^AkX^aHvV@fNz#axnDdW$_7YP=yx&IlyO
zoLH7#%nhr8K-Ca5!QbLePRz+nDoQL#bqlGQEUO^N9>>VQP^`~5*;c_(l#5gQ0|OVQ
zc8B9#5s4|D6TPQ+ceqYos4i+PD0)Ru>AIlFB|(#mf@W6)%{rWJ2#H-0Qob%^d`Zao
zqLAqoA=3`$8)8yd#MG~gSzHpcxF}|IMa-(hee!aJC}Tc>o~#+tD-192>0ad1y~3y4
z!F)p&T#rlL<>0-*C%-`WB8Tb~4%G`BsyFzBI=CR|mC1r}a;%xw<Q7G($&EZ>lUJMZ
zOrE2vGWmm%0B1a?tN=y$<OF?*$>&UzCkyfk*Td6Z7N}YSNi)<iWbuL-3=9m2R8_+e
z4^LJp48aUVAf6`wEtd4ulE7Q6ATk9~pyYtU6_gH&GC_gM1}R#K!a*FtwEUv-#G(}M
z%)Hdx#FEU(nmW90;P_$A_suMd1*u{6Ey>T%(PRY2X)!2uLQ0=oEX5_M1yzDU`APXD
zzKMB>>8V9}Afawhl;lqKP?n#(&RE7Al)vwCa9`n&oslvpYlX@h%?%-2!Y*>yT;Z^}
zz+rPkRB}q>l-LC(OKh)*8g{VsaCLBj!|$Z>N)u3tSL6-yKs1O*1Q7`g3=Gf`@fK@Y
zVoqr)N|~5Ad6|j=<MheLRRS56CwCi3ZZ=ZA!N?5`fDDj+(aAFEIxe7)L<%2}pP_~p
zf!HWUmNGCfp!pLNzRM=}s9$3TwaNUFnI><P5!qa;@r4Og1TZsXiGiv&xnKrOR=<!U
zP);do09g*IoixR7F(>Ba6lqW1uPs@R<S>u|Ul7p<BAP%1*qI;)6@lveB2dMS<Z4hH
z7FB@M1%QZP5K##tKoM6|1qvt@aJ1cGFN{ymEKAM1#Zr=5TmtbTs3ucjRRHC!$yquo
zj3+nu>*O#B-Q^eVshtrrC+Z@<#ua{z3mh7gdG)kJ3K&5}Zw*5hEG;q=FW>B~=fhZ^
z$6mvb#R(G*W~c$R=$ZVAz<%X|L?6hPx47X&N?;1OGAjc4_7-<~QD#9wYEgVyY7V4*
z4Xy}5>Gl>=VG1l-dO+126S!1>)P7~4Xb}ohM^1mBcuDyVN`Py@t<4+!!dLi}J}^jd
zN>5HO=#mhV=y2(A?{NRX#vmdw#k|As2ESkjXAwA|OqMo0;}5d62;^FDZYc@@g(0;1
zEk|>}Ezb1Rl5lWET_uK+3*h3?lkJUM*&pzWcJLM{Z+>HJ$(RFbiM;&(|NnnY9&k+F
zVoA=)EGPoCU?D*TDvpb+K(QDPBESVMD57t1X6BWorh^K<q5zN#C|PJSgX6ad6gaoo
z3RA$XcV=)PDHJJAo@lDTs6Ba;X+NX#W+$_!OnfCE<DgNMB|3Sph6GzJGbcld?&M%g
zRYvj2iM*nd`z(c+4H+0Fe-IIwyxmfj(RA`%OLJZ`s4fNuh6OT{6|K%QT2KCHrN}6^
zS=d^Vk<oOsu1y=Gm3$E>vw#a1^n?kI)FMO@1?O*nkY}_fzqPeA00lf!%@g7r5RhM-
zib(snSTpkqN=t6B78m4XmK2Fk4z$x10Jn`mfdNkOMN235*+~e4+L6T^kbnT!AVrFk
zciI`Txic{^6nky{Xy?et|CNJ5RC0>%0@aHm8dpR#HrqQ0FxG?1QQjgakfWSIgbRoO
zcXEnBtZEPeE{MQB?ErB>DY2*z#F_#kR)Ppu5CJM2Ac@r-#03$rK&j{nIB`NcLJbTb
z_~IE^H9s(bNJ9wq!Ht=dmE(hmD68-X1~{Q7%F6LUhn3an0|T7ko6O>*Z>{-(K^bHO
zobY1gVCDEAB*iN9fdNiffV)#{tYRM+;Do^BS|^i4utP!4)0Di$S&*5RpPZbLnGcS~
zTdaAdxdoNr#0qXy+!BT>&?|#?(?N+2+>|H+l`D`e1Zp=GflF?-$v>QQxIry#NM!O)
z)^}DD2F1@U?&SQOoYZ7c2fVn5b8?2WwiKuqECN-4MIa{?AqC~+wa(()9*hhOAD9^#
z89(q%KJTn7Xn27E1wEMj(RprZ4kM%W2L|lqN3i%8FbP!?$1KXo@<GR)jZyRi19tKw
OSo{l^#8d<BjsgG&%^&yx
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc
index 46c82e11fbe0ff3bb421e7f1e4c8aec6e35c9363..47dfd580761c955ca39175dd03a865d137c0bcb2 100644
GIT binary patch
delta 19
acmcc3c$<-XIWI340|Ntto5`Pv+!p{dtOaoZ
delta 19
acmcc3c$<-XIWI340|Ns?#ES5V+!p{d$OTdW
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 c101839..b20bdc6 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -182,10 +182,9 @@ def moveUntilContactControlLoop(speed, robot, clik_controller, i, past_data):
pin.forwardKinematics(robot.model, robot.data, q)
# break if wrench is nonzero basically
wrench = robot.getWrench()
- # TODO: move this to the robot class
- wrench = wrench - robot.wrench_offset
- # TODO: remove magick number
- # it is empirical bla bla, but make it an argument at least
+ # NOTE: this 4.2 is a magic number
+ # it is a 100% empirical, with the goal being that it's just above noise.
+ # so far it's worked fine, and it's pretty soft too.
if np.linalg.norm(wrench) > 4.2:
print("hit with", np.linalg.norm(wrench))
breakFlag = True
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc
index 6d20d4dc491caffb2381d4d29f6e5fa47875ecf5..94aa9adee31a8c24a5d1149f612b7a8a46762036 100644
GIT binary patch
delta 19
acmcb`c#DyHIWI340|Ntto5`Pv+~)x^hXrl`
delta 19
acmcb`c#DyHIWI340|Ns?%!=@d+~)x^y9HAK
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 4fba119..689d5b2 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,14 +1,13 @@
-# TODO clear out unnecessary imports
# TODO rename all private variables to start with '_'
# TODO: just read the q and update everything every timestep, don't deepcopy,
+# TODO: rewrite all getSomething functions to updateSomething functions,
+# and then make the getSomething function just be return self.something.copy()
# --> just create a RobotManager.step() function, update everything there
# don't do forwardKinematics 2 extra times for no good reason. make that the libraries
# responsibility, not the users
import pinocchio as pin
import numpy as np
import time
-from pinocchio.visualize import GepettoVisualizer
-#import gepetto.corbaserver
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
@@ -102,8 +101,6 @@ class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
signal.signal(signal.SIGINT, self.stopHandler)
self.max_iterations = args.max_iterations
- # NOTE the robot manager is all over the place,
- # there might be a better design approach to that
self.robot_manager = robot_manager
self.controlLoop = controlLoop
self.args = args
@@ -115,8 +112,8 @@ class ControlLoopManager:
self.past_data[key] = deque()
# immediatelly populate every deque with initial values
for i in range(args.past_window_size):
- # deepcopy just in case, better safe than sorry.
- # and we can be slow with initialization.
+ # deepcopy just in case, better safe than sorry plus it's during initialization,
+ # not real time
self.past_data[key].append(copy.deepcopy(save_past_dict[key]))
# similar story for log_dict as for past_data,
@@ -145,7 +142,7 @@ class ControlLoopManager:
# TODO: write an assert assuring the keys are what's been promised
# (ideally this is done only once, not every time, so think whether/how that can be avoided)
for key in latest_to_save_dict:
- # remove oldes entry
+ # remove oldest entry
self.past_data[key].popleft()
# add new entry
self.past_data[key].append(latest_to_save_dict[key])
@@ -159,9 +156,6 @@ class ControlLoopManager:
#self.robot_manager.stopHandler(None, None)
self.log_dict[key][i] = log_entry_dict[key]
- #for key in log_entry_dict:
- # self.log_dict[key][i] = log_entry_dict[key]
-
# break if done
if breakFlag:
break
@@ -174,27 +168,24 @@ class ControlLoopManager:
else:
time.sleep(self.robot_manager.dt - diff)
self.final_iteration = i
-# TODO: provide a debug flag for this
-# if i < self.max_iterations -1:
-# print("success in", i, "iterations!")
-# else:
-# print("FAIL: did not succed in", max_iterations, "iterations")
-# TODO: reintroduce maybe
-# if not self.args.pinocchio_only:
-# self.robot_manager.stopHandler(None, None)
+ if args.debug_prints:
+ if i < self.max_iterations -1:
+ print("success in", i, "iterations!")
+ else:
+ print("FAIL: did not succed in", max_iterations, "iterations")
return self.log_dict, self.final_iteration
"""
stopHandler
-----------
- TODO: make ifs for simulation too
- can have self as first argument apparently.
upon receiving SIGINT it sends zeros for speed commands to
- stop the robot
+ stop the robot.
+ NOTE: apparently this isn't enough,
+ nor does stopJ do anything, so it goes to freedriveMode
+ and exists, which actually stops it.
"""
def stopHandler(self, signum, frame):
- #plotFromDict(signum, frame)
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
vel_cmd = np.zeros(6)
@@ -207,11 +198,12 @@ class ControlLoopManager:
self.robot_manager.rtde_control.freedriveMode()
plotFromDict(self.log_dict, self.final_iteration, self.args)
self.robot_manager.rtde_control.endFreedriveMode()
- #exit()
"""
robotmanager:
---------------
+- design goal: rely on pinocchio as much as possible while
+ concealing obvious bookkeeping
- right now it is assumed you're running this on UR5e so some
magic numbers are just put to it.
this will be extended once there's a need for it.
@@ -225,25 +217,14 @@ robotmanager:
actually run at 500Hz and not more.
- this is probably not the most new-user friendly solution,
but it's designed for fastest idea to implementation rate.
-- NOTE and TODO: rely on pinocchio as much as possible.
- do NOT copy data that's already present in pin.model and pin.data,
- it's just confusing. this lib already heavily relies on pinocchio,
- let's just lean into that all the way and not invent hot water.
-- TODO: write out default arguments needed
+- if this was a real programming language, all of these would be private variables.
+- TODO: write out default arguments needed here as well
"""
class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
# might be changed later if that seems more appropriate
- # TODO: put the end to all modes not compatible with control
- # like freedrive or forcemode or whatever.
- # you shouldn't care about previous states
def __init__(self, args):
- # if you just stop it the program with Ctrl-C, it will continue running
- # the last speedj lmao.
- # there's also an actual stop command, but sending 0s works so i'm not changing it
- #signal.signal(signal.SIGINT, self.stopHandler)
-
self.args = args
self.pinocchio_only = args.pinocchio_only
self.simulation = args.simulation
@@ -253,58 +234,61 @@ class RobotManager:
self.model, self.collision_model, self.visual_model, self.data = \
get_model(args.visualize)
# TODO: fix this
+ # ---> TODO: write your own vedo 3D visualzier,
+ # connect it with the real-time matplotlib line plots,
+ # and add some basic gui control (show/don't show stuff,
+ # define a goal point, choose controller etc)
if args.visualize:
raise NotImplementedError('Paths in the urdf or something else need to \
be fixed to use this first. Sorry. Coming soon.')
pass
- # run visualizer from code (not working atm)
- #gepetto.corbaserver.start_server()
- #time.sleep(3)
- #viz = GepettoVisualizer(model, collision_model, visual_model)
- #viz.initViewer()
- #viz.loadViewerModel()
- #viz.display(q0)
+
# TODO: make general
if self.gripper_flag and not self.pinocchio_only:
self.gripper = RobotiqGripper()
self.gripper.connect("192.168.1.102", 63352)
self.gripper.activate()
- # TODO: this is obviously at least a partial redundancy w.r.t. the pinocchio data object
- # however i have no time to investigate what i get or don't get there
- # so i'll just leave this be. but fix this!!!! we don't want to duplicate information!!
+
# also TODO: figure out how to best solve the gripper_velocity problem
- #self.q = np.zeros(6)
- #self.qd = np.zeros(6)
- #self.qdd = np.zeros(6)
- #self.gripper_pos = 0.0
- #self.gripper_past_pos = 0.0
- # set q,qd,qdd,griper_pos, gripper_past_pos, gripper_vel to dummy values
+ # NOTE: you need to initialize differently for other types of joints
+ self.q = np.zeros(self.model.nq)
+ # v_q is the generalization of qd for every type of joint.
+ # for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
+ self.v_q = np.zeros(self.model.nv)
+ # 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 self.pinocchio_only:
- self.q = pin.neutral(self.model)
- elif not args.simulation:
- # NOTE: you can't connect twice, dw 'bout that
+ if (not args.simulation) and (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
+ # back to the os even though you've shut off the previous program.
print("CONNECTING TO UR5e!")
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
self.rtde_io = RTDEIOInterface("192.168.1.102")
- if args.gripper:
+ # 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()
+ if self.gripper_flag:
self.gripper.connect("192.168.1.102", 63352)
# this is a blocking call
# this isn't actually needed and it's annoying
# TODO: test after rebooting the robot
# self.gripper.activate()
- else:
+ 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")
# ur specific magic numbers
+ # NOTE: all of this is ur-specific, and needs to be if-ed if other robots are added.
# TODO: this is 8 in pinocchio and that's what you actually use
# if we're being real lmao
# the TODO here is make this consistent obviously
- self.n_joints = 6
+ self.n_arm_joints = 6
# 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
@@ -314,28 +298,23 @@ class RobotManager:
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
assert args.acceleration <= 1.7 and args.acceleration > 0.0
- # we're not saying it's qdd because it isn't directly (apparently)
- # TODO: check that again
+ # this is the number passed to speedj
self.acceleration = args.acceleration
+ # NOTE: this is evil and everything only works if it's set to 1
+ # you really should control the acceleration via the acceleration argument.
assert args.speed_slider <= 1.0 and args.acceleration > 0.0
self.speed_slider = args.speed_slider
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
- self.max_iterations = args.max_iterations
# TODO: these are almost certainly higher
# NOTE and TODO: speed slider is evil, put it to 1, handle the rest yourself.
- # maybe you want to have them high and cap the rest with speed slider?
- # idk really, but it's better to have this low and burried for safety and robot longevity reasons
- self.max_qdd = 1.7 * args.speed_slider
- # NOTE: i had this thing at 2 in other code
+ # NOTE: i have no idea what's the relationship between max_qdd and speed slider
+ #self.max_qdd = 1.7 * args.speed_slider
+ # NOTE: this is an additional kinda evil speed limitation (by this code, not UR).
+ # we're clipping joint velocities with this.
+ # if your controllers are not what you expect, you might be commanding a very high velocity,
+ # which is clipped, resulting in unexpected movement.
self.max_qd = 0.5 * args.speed_slider
- # error limit
- # TODO this is clik specific, put it in a better place
- self.goal_error = args.goal_error
- # this is probably the right place to manage this
- self.wrench_offset = np.zeros(6)
-
-
"""
calibrateFT
@@ -354,10 +333,7 @@ class RobotManager:
print("Use this as offset.")
for i in range(2000):
start = time.time()
- q = self.rtde_receive.getActualQ()
ft = self.rtde_receive.getActualTCPForce()
- tau = self.rtde_control.getJointTorques()
- current = self.rtde_receive.getActualCurrent()
ft_readings.append(ft)
end = time.time()
diff = end - start
@@ -365,10 +341,9 @@ class RobotManager:
time.sleep(self.dt - diff)
ft_readings = np.array(ft_readings)
- avg = np.average(ft_readings, axis=0)
- self.wrench_offset = avg
- print("average ft time", avg)
- return avg
+ self.wrench_offset = np.average(ft_readings, axis=0)
+ print(self.wrench_offset)
+ return self.wrench_offset.copy()
"""
step
@@ -395,9 +370,14 @@ class RobotManager:
def step(self):
self.getQ()
self.getQd()
- self.wrench = np.array(self.rtde_receive.getActualTCPForce())
- pin.forwardKinematics(self.model, self.data, self.q)
- self.Mtool = self.data.oMi[self.JOINT_ID]
+ self.getWrench()
+ # certainly not necessary btw
+ # but if it runs on time, does it matter? it makes everything available...
+ pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
+ self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+ # this isn't real because we're on a velocity-controlled robot,
+ # so this is actually None (no tau, no a_q, as expected)
+ self.a_q = self.data.ddq
"""
setSpeedSlider
@@ -488,8 +468,8 @@ class RobotManager:
self.q = q
pin.forwardKinematics(self.model, self.data, q)
# TODO probably remove deepcopy
- self.Mtool = self.data.oMi[self.JOINT_ID]
- return copy.deepcopy(self.Mtool)
+ self.T_w_e = self.data.oMi[self.JOINT_ID]
+ return self.T_w_e.copy()
"""
getQd
@@ -522,11 +502,8 @@ class RobotManager:
# let's just have both options for getting q, it's just a 8d float list
# readability is a somewhat subjective quality after all
qd = np.array(qd)
- self.qd = qd
- # TODO make sure this makes sense
- else:
- return self.qd
- return qd
+ self.v_q = qd
+ return self.v_q.copy()
"""
getWrench
@@ -536,14 +513,17 @@ class RobotManager:
here because this things depend on the arguments which are manager here (hence the
class name RobotManager)
"""
- def getWrench(self):
+ def getWrenchRaw(self):
if not self.pinocchio_only:
wrench = np.array(self.rtde_receive.getActualTCPForce())
else:
raise NotImplementedError("Don't have time to implement this right now.")
-
return wrench
+ def getWrench(self):
+ self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+ return self.wrench.copy()
+
"""
sendQd
-----
@@ -584,7 +564,7 @@ class RobotManager:
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
- Mtool = self.data.oMi[self.JOINT_ID]
+ T_w_e = self.data.oMi[self.JOINT_ID]
if not self.args.visualize:
print("You can only specify the translation right now.")
if not self.pinocchio_only:
@@ -596,7 +576,7 @@ class RobotManager:
# remain with the current orientation
# TODO: add something, probably rpy for orientation because it's the least number
# of numbers you need to type in
- Mgoal = copy.deepcopy(Mtool)
+ Mgoal = T_w_e.copy()
# this is a reasonable way to do it too, maybe implement it later
#Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
# do a while loop until this is parsed correctly
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc
index 0b438269c81061782619270835f25ca18b5845e3..310d0f3ccd9af60dab2a7927182652fe8b3decea 100644
GIT binary patch
delta 19
acmaFK_>z%(IWI340|Ntto5`Pv-1h)AvIUy}
delta 19
bcmaFK_>z%(IWI340|NuY%N5}hx$gl0IWz_h
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc
index d70d22745d75183a65543fa1b8c7a0c8298cd164..87cf9af8fa0a9f776b63d90be581eb2a06f08dde 100644
GIT binary patch
delta 19
acmaFI_>Pf#IWI340|Ntto5`Pv+>Ze^YXzzR
delta 19
bcmaFI_>Pf#IWI340|NuYjdkG@xgP@nIi&^>
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc
index cc7e2ef70b43104b84abbcf2d7964c31e18b8618..73ad86e2073753e1f862d0c21f332c78f73adcef 100644
GIT binary patch
delta 19
acmcc3c$<-XIWI340|Ntto5`Pv+!p{dtOaoZ
delta 19
acmcc3c$<-XIWI340|Ns?(u(kj+!p{e1O;3G
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 4f5e2990e6644b46ae0420a9cac8ef5da6c2e1ed..386fce4af890b63f03d87eb47a3d28d507c0012d 100644
GIT binary patch
delta 310
zcmZ1>e?x(HIWI340|Ns?s`;PPBNKTg>uneq7^X9%Fhnt=Fs3l&Fh((fY33*vFwGjp
z3Z~hj*isl%m~+^3Iiffi8B$nU7@{~+n1UHJS*xaU733EeXC~#ODrDvs6y=wt=BDPA
zSSfHRC@3hTr<N#`WTYx2C+1`(6(yEr=I1G-W#*(R<fnneqZEqrlk!Ux$}>wc@=Hq;
zit>xW+7k0pK+<^%1*IhlnI#GtsYR)JV40GP)Z$cy+{E<EWQDxa+@#c^Vui$_RE50!
z5`{#C^!)r3g`E8SY(1`xDQVn{{F_g6i!gI>-C~cAPsvY?kDvU2FGkUUfq{XMfq|ho
kje&vT12ZEd;|C^IMwSl@AcBX1QDSl+zZE0L<Q{$*0AiU}761SM
delta 133
zcmcbiutJ`9IWI340|Ns?h*VOl-$Y(XI|~K|hUp9`3@MB`3{i|=nkkAIOtVC>fN9n!
z))a;mrX03h_9%8nh7{%&hA55{#$X0bmW}_@xEZ-Nr}2m|GjdLD;g1ouXJBApWME(@
bj$&Y7_`uA_$asT6d@`ee6(iSV8vz*rC9oP+
diff --git a/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc
index fd1cbb042ac4e18c03f4548f12b5ef3c475a0fd8..480d94bcf9e5d9e5d77bb06fefa9fe15d9971f79 100644
GIT binary patch
delta 22
ecmX>%lkwb4M(*Xjyj%<n3=D22e>QR-_5lD<od)Ot
delta 22
ecmX>%lkwb4M(*Xjyj%<n3=BQ`OE+>K_5lD<wg%Dw
diff --git a/python/ur_simple_control/util/default_args.py b/python/ur_simple_control/util/default_args.py
index aea31ea..218de85 100644
--- a/python/ur_simple_control/util/default_args.py
+++ b/python/ur_simple_control/util/default_args.py
@@ -4,130 +4,11 @@ import argparse
"""
-preferably you copy-paste these parameters and add/remove
-arguments to suit your specific arguments.
-the generic arguments are pretty much always used.
-TODO: think about setting the generic arguments somewhere else,
-not parsing there, but returning the parses and then only adding
-stuff you want.
-that way it's a bit cleanear, and you also can't delete the obligatory ones
+TODO: have 2 versions:
+ 1. only the minimal set of argumets (those needed to load robot
+ and run anything)
+ 2. every single one you have os far
"""
def getDefaultArgs():
- #######################################################################
- # generic arguments #
- #######################################################################
- parser = argparse.ArgumentParser(description='Make a drawing on screen,\
- watch the robot do it on the whiteboard.',
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- # TODO this one won't really work but let's leave it here for the future
- parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator. \
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio.\
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize with gepetto, but \
- NOTE: not implemented yet", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
- parser.add_argument('--acceleration', type=float, \
- help="robot's joints acceleration. scalar positive constant, \
- max 1.7, and default 0.4. \
- BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
- TODO: check what this means", default=0.3)
- parser.add_argument('--speed-slider', type=float,\
- help="cap robot's speed with the speed slider \
- to something between 0 and 1, 1.0 by default because for dmp. \
- BE CAREFUL WITH THIS.", default=1.0)
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
- #######################################################################
- # your controller specific arguments #
- #######################################################################
- # not applicable here, but leaving it in the case it becomes applicable
- # it's also in the robot manager even though it shouldn't be
- parser.add_argument('--past-window-size', type=int, \
- help="how many timesteps of past data you want to save", default=5)
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with. NOTE: not used here", \
- default=1e-3)
- # TODO: test the interaction of this and the overall demo
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tiknohov regularization.\
- This is used when generating the joint trajectory from the drawing.", \
- default=1e-2)
- # TODO add the rest
- parser.add_argument('--clik-controller', type=str, \
- help="select which click algorithm you want", \
- default='dampedPseudoinverse', \
- choices=['dampedPseudoinverse', 'jacobianTranspose'])
- # maybe you want to scale the control signal
- parser.add_argument('--controller-speed-scaling', type=float, \
- default='1.0', help='not actually_used atm')
- #############################
- # dmp specific arguments #
- #############################
- parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
- help="whether you want to use temporal coupling", default=True)
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
- parser.add_argument('--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
+ raise NotImplementedError("sorry, this hasn't been implemented yet")
diff --git a/python/ur_simple_control/util/draw_path.py b/python/ur_simple_control/util/draw_path.py
index 0fa950d..4a54dca 100644
--- a/python/ur_simple_control/util/draw_path.py
+++ b/python/ur_simple_control/util/draw_path.py
@@ -1,10 +1,14 @@
-# TODO: possible improvement:
-# - draw multiple lines
-# - then you would just generate multiple dmps for each trajectory
-# and do movel's + movej's to provide the connections
-# TODO: possible improvement: make it all bezier curves
-# https://matplotlib.org/stable/users/explain/artists/paths.html
-# look at the example for path handling if that's what you'll need
+"""
+possible improvement:
+- draw multiple lines
+- then you would just generate multiple dmps for each trajectory
+ and do movel's + movej's to provide the connections
+possible improvement: make it all bezier curves
+ https://matplotlib.org/stable/users/explain/artists/paths.html
+ look at the example for path handling if that's what you'll need
+ - not really needed, especially because we want hard edges to test our controllers
+ (but if that was a parameter that would be ok i guess)
+"""
import numpy as np
import matplotlib.pyplot as plt
@@ -21,9 +25,10 @@ from matplotlib.widgets import LassoSelector
#from matplotlib.path import Path
class DrawPathManager:
- def __init__(self, ax):
+ def __init__(self, args, ax):
self.canvas = ax.figure.canvas
self.lasso = LassoSelector(ax, onselect=self.onselect)
+ self.args = args
def onselect(self, verts):
# verts is a list of tuples
@@ -40,16 +45,15 @@ class DrawPathManager:
# made to save and exit
def accept(self, event):
if event.key == "enter":
- print("pixel path:")
- print(self.path)
+ if args.debug_prints:
+ print("pixel path:")
+ print(self.path)
self.disconnect()
- # TODO: ensure this runs fine after packaging
np.savetxt("./path_in_pixels.csv", self.path, delimiter=',', fmt='%.5f')
- print("TODO: run clik on the pixel path to make it a trajectory")
# plt.close over exit so that we can call this from elsewhere and not kill the program
plt.close()
-def drawPath():
+def drawPath(args):
# normalize both x and y to 0-1 range
# we can multiply however we want later
# idk about the number of points, but it's large enough to draw
@@ -59,7 +63,7 @@ def drawPath():
subplot_kw = dict(xlim=(0, 1), ylim=(0, 1), autoscale_on=False)
fig, ax = plt.subplots(subplot_kw=subplot_kw)
- selector = DrawPathManager(ax)
+ selector = DrawPathManager(args, ax)
# map key press to our function
# thankfully it didn't mind have self as the first argument
@@ -70,4 +74,4 @@ def drawPath():
if __name__ == '__main__':
- drawPath()
+ drawPath(args)
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 4a32e98..3059057 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -1,3 +1,9 @@
+"""
+possible improvement:
+ get the calibration file of the robot without ros
+ and then put it here.
+ these magic numbers are not a good look.
+"""
import pinocchio as pin
import numpy as np
import sys
@@ -6,7 +12,6 @@ from importlib.resources import files
# can't get the urdf reading with these functions to save my life, idk what or why
-# TODO fix paths via python packaging
def get_model(visualize):
#urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 730ff2c..c8f0c5d 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -6,8 +6,10 @@ def saveLog(log_dict, final_iteration, args):
# shave off the zeros, noone needs 'em
for key in log_dict:
log_dict[key] = log_dict[key][:final_iteration]
- # TODO make generic
- # preferably name files after arguments
+ # TODO make generic:
+ # - generate name based on args + add index
+ # - you need to run shell ls to get the index,
+ # there's at least one chalmers project with code for that
run_file_path = "./data/clik_run_001.pickle"
args_file_path = "./data/clik_run_001_args.pickle"
# save the logged data
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc
index aba675302559b05bdf8aa8c563b486bbf526fd1f..ac4eb3273e2b46597521f26cb6adac319d37d244 100644
GIT binary patch
delta 19
acmaFB_<)gnIWI340|Ntto5`Pv+*biKWd(o$
delta 19
acmaFB_<)gnIWI340|Ns?R!_u4?yCSaWd&FO
diff --git a/python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d3f8454db4cce95335a702b67ae9e76fa717ce46
GIT binary patch
literal 5748
zcmZ3^%ge>Uz`$UT^EXwRmx19ihy%myP{!vjMh1rI3@HpLj5!QZj9{86iU~|JN3o<Z
zrLttPLe(;)FsHJmu%t3&!E`gEv8AxKutagBupx`4u%qbWK<1^frEs>eL~*8YrGoT;
zT?jH?5=4M8s-0;pDcmhAQQVviDPpKHDZHsXVAt@1wJ|V&T*KGG8pW5w&%}Ui=Q0Kc
zhSe}d3@HMsj9L6JZVC&s$~3kV!4{S%0g6lzYGH{IOc6#lH$^0sB})j&PNpol&(qja
zL|a&*gi{!U88pRTf)x4PVofVb%uT(;nUr4<U!0Lxka~-)D7B=tC@+}_qyUOR@xaW$
z!0=fO9E&B4P;mwZhAcR{h9L{Cr-m_w5fRcgj44bgJZ1zhg=G!vGFAqL)$rJ_Whw#L
z2el%L1<GJR4c%Ji5;m9&0|P?U0=PK{Sq6q0<{E}9xcX&G3=FH`ZmnghVOoG>BSK#d
za~21TSHqme31=huG=*&q`!W^=hShL)GBVUK1v6-J_!WVycnJ!hmmt=2i1r8enyj~&
z3-XI^vE`)ZrI%#fVlB^1Dap9SmXVs7o>6j(H8H0kBQazp^DUOdip=6$tOfa*c_p`)
zixN{bxo)wPrKaTHVs^@}xW!#kl$ckXlUS0OpLdJ7AT#e4r(<4vPO2kFmrz<!er`~H
z30T%Qv7{)o;uc2{lz)p2%u2n*nh4TRB*4JHpvhVUisB-WKW?$-CuOB3mz3P%1S<fW
z$edVFQ49)Q1rYevpr4VSo2s9lUtEx%l2`=h86mhirFkj(1w|kq=ob{_gG?*dFD;5M
z&de>yNsUj=&nqd)&(SZaEXl~v!y-|ZSzMZ!lUbFjUyzv>Uy>gW@^^eePJT&zX-Q^|
zUP0w8zLcWGa;N+Xx1#*qU{~WJ1qKENP#hQ6GcYhTFx=%6>dBa4dPQ1iL*$OaEioq~
zF9e62k+~3;ejy|CN?g{3qS6a|Wf%F%uJDyLxZf2Jp29nke+qwt=T{B}ZoVGf>s(rw
zxU^PSZb;pdcah8W3YTkx^9MEtR-SghCcg=mQ|wnru94eOdc{8ALReIT-$j<_D=g6$
zSfU?r3pMzC21WKQ=A`_JTf7BDsb!h@rN!|D`NgTXIPy}<!K`FZaD$YAFv#5?{$~$7
zISf4q)G#doB`lDB6kNl&0Av=57>I+ae;FeK!)j3K1W7T}vXp>n2!Y7e3qa`sB8WuR
zFxMcJ*J$~PwT58<$UjKxA<P<PM2U`)w-^z5D}`|l6GnbwSO71@ASOY`8YYB3RNpW%
zq%Z_CXfpeS6oE=$P3|Hw1_p*AaRvs4Tg-{cCAZi!^U6|-ic^auK;od{s7MOLk_Hi=
z>`){MVu7sHWGhkvv6VrD3W$&e8N-#5nU<DXl$w{ES|rE7z)%eGGAL&#C=|(q6bO_S
zq$HN4Iw2CMCIbV5Fi2@LIDtMzB)JEIlG9}-$;`;TE@ym6&iJC7=@mKCi-KlX1kD<}
z?sD-WrN*o)W}X+hysmJ0H8|g3VQ=?p^6IGUsb3JeByNZ873+`-QE?4k7g^%3u*6?r
zi3cTEj&}DZ_YRLMJPH?C6tA!-UVxz@MUXq$5{ruR%ZpJ8e2`y2N$9f^MsiGHOkqNe
za2JNy-dd&_Mns}PE&LJHLJDIIQyNlLP{qK&P|J*>6RB#bVMNl&j7294icS_#EJ70o
zvQ8E(I$4o*)-co{xhWpxZ?G{X@B*KK0ns=CCH)%KbY|29hh_#S{J>`P^nnv1E3`5I
znOy{`Yl=V>izfRm)}q9`^i)V<1G)beOHyKS>Mf?cf?KSKMMa5~x0p-v3pCk^v_J_D
zr2Q6S<}JpGTa1;r7^^VSoCGAzxq&KmM{u;l3P&>r1_p34T?$TncX@>-7)}W6jOvN{
zz`)9?1tvNeZ}13S;gOw@c!ft{f$0*v4T>8aHz;3GHoM4UMv<hX+!aZ!4z`ZM9?l-V
zyAo1!I2I@_a9ohMK>VVF<`oG|s1$Dp?+0#}Yrv`V`K*~Bu+#n)OG#=)2}+sC&A`9_
z3X#wEz=<@SA%$@fV=Z$!s4Rn*YYYr2OexG533g2_YYkG{1*Lt0T)Q*Xup%ee8m1~%
z1_p4lWkb=AocNISv*6Uvj-sCx6#3v3!+_k5N@2yRpQDBadnu12*&_M21m2QmU_dI7
z*=pEpI4}}6a}AQ)*03#OW?)zi3U#n!Q8NJ}L(ke0P{IbwF)$!nPYaN;5>%3bfuV*G
zB@EctaG;uA%UQ!x!<hzZO>z2ZGT&k>y2Y4&i!lk@uqx64<p_9QTFH2eG5LA5$U66j
zRa5LWnTkL~_btX^O|BwckV0^A4hrO3Y>5R0sd*_yppy6&D>#=F>4TIQfJ;MgE4cU;
zTS#g}iLr|&7o=5W0g_`cPEO3p%uBz;mYkoHUsMdqltv)E#vsB3M3{n9a)J^oxV2IY
zatEYZ0FjWY0o-y4b~Sb^2Itfw4+aJXHBiOy6P!6ea4_)6cEoqYH@My4<!^9%ikw+l
zSVI{<Fn~zhIdX^iMP-MJJPuS+!NJ$h+r>LIWlGip(JP8ZTb;Ie>_|MoctCMS_(c=f
zD<-Zd#4ZE`o{_vD7JN}G_=;HYMUIdw93dCL=&69n6z=N+N|ywb7MQFEy`pP(K;?pf
z(nSIHD+2Bfo;SD!Cs<zKmbt(p1J3Y8HlXn11NBEzGILY&iZk=`Q0pd8l!FrB=T8iv
zE+vj;3)Y5F3M2MTLkc5m!Yctq8Z@28!^?7%ZVAX0U==A$sGS{90g=L7!<d3pB!I<O
zYZz15Kq(NcjR7Qvp4m|Bg=ZvW(O?Emc1T?UEg4VRKQ{)0%l43p1(eeuwF@V>(Q}Kr
zsG#x|cTquQNWL$)9{^76x0nhtZ!sks7J*z;1WNkgN@yh$I0A}1L8%cGhOpL*SaE8J
zbADb)VseS0XP#4jN+l%e1~D)&fa;dw7;w_PD<nEa{ko9qB_Y)XiEEkGu&zkFC}eO&
z$e`7!#rv+P_>_nl#S82<C|*%ExhQY4A$f=13BfB?-WSchPXu2GjK7eOcqK6DqHod#
zx#WwYDOW^OTAf<_?sD@sc!E<0N+Sgn1>l5mkBo$n!iW*+@Q7w$h=;d&OF*>@B*u_>
zkW`IyWP3q=0^3#sYJ`GW44{@QG}4Q_K}C!YhybZfhbVZkA34H{d_mHFplrmHYy^p4
zc=-m7<e~tOia<P3Y=j)ep$rTR)*!`Em{GifafRYVA)PBiI)o!Q<U&xwg~X&QLCF_=
zlP}1nTog^cBASXC!M9jL;>+WcZZVg|C!s_(sN)Ha>~rAAPGPKNs$p0F@jn__!-!ff
z)iS3r)w0wuBig4W@aSb=$O4%PHUU>x2cZwu+$wGch8h-7KCWfOo=j?xt5?<<rZvo{
z<xDMG4O=jSCX3%K)})-o<m@7QP{i1RdgB~LpbFs@TTW(PYGM&2%YrzGIqA1p^74yv
z!Sz>>6G#K7ZqsBh3InmjLGjKCuAe}GSX|@^767;LZ?S`^__AV9;sI4h3b5?W3+YRz
zRECsi7MBzyGB7YWgQ6Ugt8efKH@M#5;_uh&(wyMjsn?^|0O|_!3pcpm;1%eJnyWNN
zbH3gzy%oZ1q%KOCT#+)l$ZN8L{UVQjgDa>To;5@Cilo*G*A1x`_-ron*<9hXX>h**
z>L-J9wI*YcImpi<w^;K^a|<fLz6Ja57H2_bUVd_NMrJ;=Fbj4yzQvLQa-#^yyP!UQ
z5vX4U2?<b1bc?yTr05nKxMD8`SFN|$^HRW#=^{{RRRl`Ux7d91LE|Q1pRQy8I|E!d
z|KhO8%}*)KNwq7=W?*0dwIho=85tNpFf%eT-e3^A07EwzxEsLm27~wo82Z4*%_uy>
zc#io825v^-4J=@4hsYiYFaw>C;$&2sVKT?!0|O_c(uUwI;b6uI;WOf3209_e#;CO*
zWrfKat1D_|9~jsewGJ>JVFxqN2_Xgz?GMa+jItjX_!wm&4&q~!y&)wF7QLZwyrvq=
z0?Q*23e1drGs<Ta&ncf-Ij8ai12ZGv4x>F*V8#jMGwNUlI>AMbCIJpc;Tg(v)ITtA
VFbabb4w$inWe*pafs+776#zdam-_$!
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc
index 1b8fab368fc8d0086e4066c6ee5f220f2bd77329..44f507b6f1ebdeda62574ea70174ab2afce3bc34 100644
GIT binary patch
delta 20
bcmdnTyN{Q9IWI340|Ntto5`Pz+?&||Hp>NA
delta 20
ccmdnTyN{Q9IWI340|Ntt{o7v~xi_-`06OLd2mk;8
diff --git a/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py b/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
index d400cf7..a891387 100644
--- a/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
+++ b/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
@@ -36,13 +36,12 @@ def updateFrameArrowsFromSE3(arrows : list[vedo.Arrow], frame : pin.SE3):
arrows[i].top = np.array([x,y,z])
def drawSE3AsFrame(frame : pin.SE3,
- text : str) -> vedo.Arrows:
+ text : str, scaling=0.1) -> vedo.Arrows:
#print(text)
#print(frame)
colors = ['r', 'g', 'b']
arrows = []
#print(frame.rotation)
- scaling = 0.02
for i in range(0,3):
x = frame.translation[0] + frame.rotation[0,i] * scaling
y = frame.translation[1] + frame.rotation[1,i] * scaling
diff --git a/python/ur_simple_control/visualize/vedo_manipulator.py b/python/ur_simple_control/visualize/vedo_manipulator.py
index 5c3972b..cf03cc8 100644
--- a/python/ur_simple_control/visualize/vedo_manipulator.py
+++ b/python/ur_simple_control/visualize/vedo_manipulator.py
@@ -15,222 +15,95 @@ import vedo
import pinocchio as pin
import time
from ur_simple_control.visualize.pin_to_vedo_plot_util import *
-from friction import ReducedFrictionModel
-from multiprocessing import Process
-from scipy.integrate import solve_ivp
-
-
+from ur_simple_control.managers import RobotManager
+import argparse
+
+
+def get_args():
+ parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!',
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator", default=False)
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio", default=True)
+ 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('--goal-error', type=float, \
+ help="the final position error you are happy with", default=1e-2)
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.3)
+ parser.add_argument('--speed-slider', type=float,\
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.", default=0.5)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tiknohov regularization", default=1e-3)
+ # TODO add the rest
+ parser.add_argument('--clik-controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
+ # maybe you want to scale the control signal
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ 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
#######################################################################
# IMPLEMENT THIS ACTUALLY #
#######################################################################
-if __name__ == "__main__":
- args = getArgs()
- # change this after you add the manipulator
- box_dimensions = [args.box_length, args.box_width, args.box_height]
-
- ###########################################
- # vedo instantiation, frame definitions #
- ###########################################
- vedo.settings.use_depth_peeling = True
-
- # need to manually seed lmao
- #pin.seed(int(time.time()))
- T_w_J = pin.SE3()
- # TODO: check how friction behaves when we're perpendicular to gravity
- T_w_J.setIdentity()
- T_w_J.translation = np.array([1,2,3])
- T_w_J.setRandom()
- #T_w_J.rotation = pin.rpy.rpyToMatrix(0,np.pi/2,0)
-
-
- model, data, OBJECT_JOINT_ID = getPlanarJointModel(args, T_w_J, box_dimensions)
- q_init = pin.randomConfiguration(model)
- #theta = np.random.random() * 2 * np.pi
- theta = 0.345321
- q_init[0] = 0.1
- q_init[1] = 0.1
- q_init[2] = np.cos(theta)
- q_init[3] = np.sin(theta)
-
- v_b_se2 = np.array([0.1, 0.0, 0.5])
- v_b_se3 = pin.Motion(np.array([v_b_se2[0], v_b_se2[1], 0]), np.array([0,0,v_b_se2[2]]))
- print("v_b_se2", v_b_se2)
- #print("pin.exp3(v)", pin.exp3(v))
- #exit()
- #tau = np.ones((model.nv)) * 0.1
- #a = pin.aba(model, data, q, v, tau)
- # tau_q_control does not exist in reality, those are the actuators
- # of the planar joint
- #tau_q_control = np.zeros((model.nv))
-
- pin.forwardKinematics(model, data, q_init)
- T_w_b_init = data.oMi[1].copy()
-
- q_pin = pin.integrate(model, q_init, v_b_se2)
- print("q_pin:", q_pin)
- pin.forwardKinematics(model, data, q_pin)
- pin.updateFramePlacements(model, data)
-# for vvv in data.v:
-# print(vvv)
-# for fr in data.oMf:
-# print(fr)
- T_w_b_pin = data.oMi[1].copy()
-
- q_3 = np.zeros(3)
- q_3[0] = q_init[0]
- q_3[1] = q_init[1]
- q_3[2] = theta
- print("q_3", q_3)
-
- in_SE3 = False
-
-########################################################
-## JUST USE THE EXP FOR THE LOVE OF GOD
-#############################################3333333
-# omega = v[2]
-# cv = np.cos(omega)
-# sv = np.sin(omega)
-# R_omega = np.array([[cv, -1*sv], [sv, cv]])
-# vcross = np.array([-1*v[1], v[0]])
-# vcross -= -v[:2]*R_omega[:,0] + v[:2]*R_omega[:,1]
-# vcross /= omega
-# t = vcross.copy()
-# T_next = np.hstack((R_omega, t.reshape((2,1))))
-# T_next = np.vstack((T_next, np.array([0,0,1]).reshape((1,3))))
-# # let's try inverse
-# #T_next = np.hstack((R_omega.T, -1*R_omega.T @ t.reshape((2,1))))
-# #T_next = np.vstack((T_next, np.array([0,0,1]).reshape((1,3))))
-# print("T_next", T_next)
-
-# c_theta = np.cos(theta)
-# s_theta = np.sin(theta)
-# R_theta = np.array([[c_theta, -1*s_theta], [s_theta, c_theta]])
-# T_J_b_se2 = np.hstack((R_theta, np.array([q_3[0],q_3[1]]).reshape((2,1))))
-# T_J_b_se2 = np.vstack((T_J_b_se2, np.array([0,0,1]).reshape((1,3))))
-
-# print("T_J_b_se2", T_J_b_se2)
-# T_J_b_se2 = T_next @ T_J_b_se2
-# print("T_J_b_se2", T_J_b_se2)
-
-# T_next_SE3 = pin.exp(v_b_se3)
-# T_next_SE2 = np.hstack((T_next_SE3.rotation[:2,:2], T_next_SE3.translation[:2].reshape((2,1))))
-# T_next_SE2 = np.vstack((T_next_SE2, np.array([0,0,1]).reshape((1,3))))
-
- # wrogn notation last term lel
-# T_J_b_SE2 = T_next_SE2 @ T_J_b_se2
-# q_3[0] = T_J_b_SE2[0,2]
-# q_3[1] = T_J_b_SE2[1,2]
-# # not good but w/3
-# q_3[2] = np.arccos(T_J_b_SE2[0,0])
-
- # this changes the no rotation problem
- #q_3[2] = omega
+def drawStateAnim(self):
+ toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
+ drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2)
+ for j in range(len(self.joints)):
+ toBase.append(toBase[-1] @ self.joints[j].HomMat)
+ drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth)
+ drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link)
+
+
+if __name__ == "__main__":
+ pin.seed(int(time.time()))
+ args = get_args()
+ robot = RobotManager(args)
+ q = pin.randomConfiguration(robot.model)
+ q = np.zeros(8)
+ pin.forwardKinematics(robot.model, robot.data, q)
+ print(type(robot.data))
+ #pin.computeAllTerms(robot.model, robot.data, q, np.zeros(8))
+ # no need to use AnimationPlayer, we can update stuff on Plotter as we please.
+ # so the comm logic + updating will be implemented here (ideal circumstance)
+ plt = vedo.Plotter(axes=1)
+ plt.camera = vedo.utils.oriented_camera(center=(0, 0, 0),
+ up_vector=(0, 1, 0),
+ backoff_vector=(0, 1, 0),
+ backoff=1.0)
- #print("q_3 + v_b_se2", q_3)
-#################
-# q_euler = np.zeros(4)
-# q_euler[0] = q_3[0] #+ np.cos(v[2])
-# q_euler[1] = q_3[1] #+ np.exp(v[2])
-# q_euler[2] = np.cos(q_3[2])
-# q_euler[3] = np.sin(q_3[2])
-# print("q_euler:", q_euler)
-# pin.forwardKinematics(model, data, q_euler)
- #T_w_b_euler = data.oMi[1].copy()
- #T_w_b_euler = pin.exp6(v_b_se3).act(T_w_J)
- #T_w_b_euler = pin.exp6(v_twist).act(T_w_b_init)
-# T_J_b = T_w_J.actInv(T_w_b_init)
- #print(T_J_b)
- #T_w_b_euler = pin.exp6(T_J_b.act(v_twist)).act(T_w_b_init)
- # THIS IS THE ONE!!!!!!!!!!!!!!!!!!!
- if in_SE3:
- print("SE3 version")
- v_w_se3 = T_w_b_init.act(v_b_se3)
- print(v_w_se3)
- T_w_next_SE3 = pin.exp6(v_w_se3)
- print(T_w_next_SE3)
- T_w_b_euler = T_w_next_SE3.act(T_w_b_init)
- #T_w_b_euler = pin.exp6(v_twist).act(T_w_b_init)
- #T_J_b_next_euler = pin.exp6(v_twist).act(T_J_b)
- #T_w_b_euler = T_w_J.act(T_J_b_next_euler)
- # let's try to get this within J and SE(2)
- # get v_b to v_J
- else:
- print("SE2 version")
- c_theta = np.cos(theta)
- s_theta = np.sin(theta)
- R_theta = np.array([[c_theta, -1*s_theta], [s_theta, c_theta]])
- # needed later
- T_J_b_SE2 = np.zeros((3,3))
- T_J_b_SE2[:2,:2] = R_theta
- T_J_b_SE2[0,2] = q_3[0]
- T_J_b_SE2[1,2] = q_3[1]
- T_J_b_SE2[2,2] = 1
- Ad_T_J_b_SE2 = np.zeros((3,3))
- Ad_T_J_b_SE2[:2,:2] = R_theta
- Ad_T_J_b_SE2[0,2] = q_init[1]
- Ad_T_J_b_SE2[1,2] = -q_init[0]
- Ad_T_J_b_SE2[2,2] = 1
- v_J_se2 = Ad_T_J_b_SE2 @ v_b_se2
- print("v_J_se2", v_J_se2)
- # get v_J_se2 to T_next_SE2
- omega = v_J_se2[2]
- c_omega = np.cos(omega)
- s_omega = np.sin(omega)
- # note that this is the same as exp(omega)
- R_omega = np.array([[c_omega, -1*s_omega], [s_omega, c_omega]])
- omega_abs = np.abs(omega)
- t = np.zeros(2)
- if omega_abs > 1e-14:
- V = np.array([[s_omega, -1*(1 -c_omega)], [1- c_omega, s_omega]])
- V /= omega
- t = V @ np.array([v_J_se2[0], v_J_se2[1]])#.reshape((2,1))
- else:
- t = np.zeros(2)
- t[0] = v_J_se2[0]
- t[1] = v_J_se2[1]
- T_next_SE2 = np.hstack((R_omega, t.reshape((2,1))))
- T_next_SE2 = np.vstack((T_next_SE2, np.array([0,0,1]).reshape((1,3))))
- print("T_next_SE2", T_next_SE2)
- T_J_b_SE2 = T_next_SE2 @ T_J_b_SE2
- q_euler = np.zeros(4)
- q_euler[0] = T_J_b_SE2[0,2]
- q_euler[1] = T_J_b_SE2[1,2]
- q_euler[2] = T_J_b_SE2[0,0]
- q_euler[3] = T_J_b_SE2[1,0]
- #q_euler[0] = q_3[0]
- #q_euler[1] = q_3[1]
- #q_euler[2] = np.cos(q_3[2])
- #q_euler[3] = np.sin(q_3[2])
- print("q3 + v_J", q_3 + v_J_se2)
- print("q_euler:", q_euler)
- pin.forwardKinematics(model, data, q_euler)
- T_w_b_euler = data.oMi[OBJECT_JOINT_ID]
+ for i in range(robot.model.nq):
+ plt += drawSE3AsFrame(robot.data.oMi[i], "",scaling=0.02)
+ if i+1 < robot.model.nq:
+ # plt += vedo.Arrow(robot.data.oMi[i].translation,
+ # robot.data.oMi[i+1].translation - robot.data.oMi[i].translation)
+ #plt += vedo.Arrow(robot.data.oMi[i].translation,
+ # -1 * (robot.data.oMi[i].translation - robot.data.oMi[i+1].translation))
+ plt += vedo.Arrow(robot.data.oMi[i].translation,
+ robot.data.oMi[i].act(robot.data.liMi[i+1]).translation)
- joint_arrows, joint_text = drawSE3AsFrame(T_w_J, "J")
- b_init_arrows, b_init_text = drawSE3AsFrame(T_w_b_init, "b_init")
- # pin
- objectt = drawBoxFromSE3(T_w_b_pin, box_dimensions)
- b_arrows_pin, b_text_pin = drawSE3AsFrame(T_w_b_pin, "b_pin")
- # euler
- objectt = drawBoxFromSE3(T_w_b_pin, box_dimensions)
- b_arrows_euler, b_text_euler = drawSE3AsFrame(T_w_b_euler, "b_euler")
- plt = vedo.Plotter(axes=1)
- plt.camera = vedo.utils.oriented_camera(center=(0, 0, 0),
- up_vector=(0, 1, 0),
- backoff_vector=(0, 1, 0),
- backoff=1.0)
-
- plt.add(objectt)
- plt.add(b_init_arrows, b_init_text)
- plt.add(b_arrows_pin, b_text_pin)
- plt.add(b_arrows_euler, b_text_euler)
- plt.add(joint_arrows, joint_text)
plt.render()
plt.show()
- plt.close()
--
GitLab