From 8ff61d1233797883784572eaa8a8897691ec380b Mon Sep 17 00:00:00 2001
From: Your Name <marko.guberina@control.lth.se>
Date: Thu, 12 Dec 2024 01:05:51 +0100
Subject: [PATCH] tried to put 2arm references and it was almost ok
---
python/examples/cart_pulling.py | 34 ++++++++++--------
.../__pycache__/__init__.cpython-310.pyc | Bin 379 -> 379 bytes
.../__pycache__/managers.cpython-310.pyc | Bin 35897 -> 35908 bytes
.../__pycache__/__init__.cpython-310.pyc | Bin 192 -> 192 bytes
.../basics/__pycache__/basics.cpython-310.pyc | Bin 7100 -> 7100 bytes
.../clik/__pycache__/__init__.cpython-310.pyc | Bin 188 -> 188 bytes
.../dmp/__pycache__/__init__.cpython-310.pyc | Bin 186 -> 186 bytes
.../dmp/__pycache__/dmp.cpython-310.pyc | Bin 9311 -> 9311 bytes
.../optimal_control/crocoddyl_mpc.py | 12 ++++---
.../__pycache__/__init__.cpython-310.pyc | Bin 181 -> 181 bytes
.../util/__pycache__/__init__.cpython-310.pyc | Bin 167 -> 167 bytes
.../__pycache__/get_model.cpython-310.pyc | Bin 7638 -> 7638 bytes
.../__pycache__/logging_utils.cpython-310.pyc | Bin 4363 -> 4363 bytes
.../__pycache__/__init__.cpython-310.pyc | Bin 172 -> 172 bytes
.../__pycache__/visualize.cpython-310.pyc | Bin 8744 -> 8744 bytes
15 files changed, 28 insertions(+), 18 deletions(-)
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 5687c6f..0480c90 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -87,8 +87,8 @@ def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
#grasp_pose = T_w_base.act(rotate.act(translate))
grasp_pose = T_w_base.act(translate.act(rotate))
- grasp_pose_left = grasp_pose.act(goal_transform)
- grasp_pose_right = grasp_pose.act(goal_transform.inverse())
+ grasp_pose_left = goal_transform.act(grasp_pose)
+ grasp_pose_right = goal_transform.inverse().act(grasp_pose)
T_w_e_left, T_w_e_right = robot.getT_w_e()
SEerror_left = T_w_e_left.actInv(grasp_pose_left)
@@ -100,9 +100,9 @@ def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
#if np.linalg.norm(err_vector) < robot.args.goal_error:
if (np.linalg.norm(err_vector_left) < 2*1e-1) and (np.linalg.norm(err_vector_right) < 2*1e-1):
isOK = True
- return isOK, grasp_pose
+ return isOK, grasp_pose, grasp_pose_left, grasp_pose_right
-def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
+def cartPullingControlLoop(args, robot : RobotManager, goal, goal_transform, solver_grasp, solver_pulling,
path_planner : ProcessManager, i : int, past_data):
"""
cartPulling
@@ -128,8 +128,7 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
if robot.robot_name != "yumi":
graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
else:
- goal_transform = pin.SE3.Identity()
- graspOK, grasp_pose = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
+ graspOK, grasp_pose, grasp_pose_left, grasp_pose_right = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
# NOTE: this keeps getting reset after initial grasp has been completed.
# and we want to let mpc cook
priority_register[1] = str(int(not graspOK)) # set if not ok
@@ -161,14 +160,13 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
if robot.robot_name != "yumi":
runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
else:
- # MASSIVE TODO: CREATE A DIFFERENT REFERENCE FOR EACH ARM
- runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
- runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+ runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose_left
+ runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose_right
if robot.robot_name != "yumi":
solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
else:
- solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
- solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+ solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose_left
+ solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose_right
robot.Mgoal = grasp_pose
#breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
@@ -181,7 +179,6 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
controlLoopClikArmOnly(robot, clikController, i, past_data)
else:
# TODO: DEFINE SENSIBLE TRANSFOR
- goal_transform = pin.SE3.Identity()
controlLoopClikDualArmsOnly(robot, clikController, goal_transform, i, past_data)
# case 2)
@@ -205,6 +202,8 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
# MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
# MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
# MASSIVE TODO: have two different REFERENCES FOR TWO ARMS
+# ----------------> should be doable by just hitting the path with the goal_transform for dual.
+# in the whole task there are no differences in left and right arm
straigh_line_path = np.linspace(p_ee_l, p_cart, args.past_window_size)
# time it the same way the base path is timed
time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
@@ -218,7 +217,7 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
if priority_int < 2:
# TODO make this one work
- breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, grasp_pose, i, past_data)
+ breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, grasp_pose, goal_transform, i, past_data)
#BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
p = q[:2]
@@ -234,6 +233,13 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
def cartPulling(args, robot : RobotManager, goal, path_planner):
+ # transfer single arm to dual arm reference
+ # (instead of gripping the middle grip slightly away from middle)
+ goal_transform = pin.SE3.Identity()
+# TODO: it's cursed and it shouldn't be
+# goal_transform.rotation = pin.rpy.rpyToMatrix(0.0, np.pi/2, 0.0)
+# TODO: it's cursed and it shouldn't be
+# goal_transform.translation[1] = -0.1
############################
# setup cart-pulling mpc #
############################
@@ -261,7 +267,7 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
us_init = solver_grasp.problem.quasiStatic([x0] * solver_grasp.problem.T)
solver_grasp.solve(xs_init, us_init, args.max_solver_iter)
- controlLoop = partial(cartPullingControlLoop, args, robot, goal, solver_grasp, solver_pulling, path_planner)
+ controlLoop = partial(cartPullingControlLoop, args, robot, goal, goal_transform, solver_grasp, solver_pulling, path_planner)
log_item = {}
q = robot.getQ()
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index 66a7ae1d673b877c2f76b218972970879116ddcc..b346ab48b3eb5b0bdef663970c9cca16de1e2b0f 100644
GIT binary patch
delta 19
acmey(^qYw*pO=@5fq{Wx{icmvg^U0<R|O^j
delta 19
acmey(^qYw*pO=@5fq{YH^6ZUVg^U0<yah7=
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 7c773d8c3aa8cc3f72351d40e311f1dd9e64718c..8344427e05627676833671090b0d83c03dc1c605 100644
GIT binary patch
delta 790
zcmdlvgXzc&Cf<BrUM>a(28Q*UBGYR&^0tRD#%-P&c8-xzcd}c!!sM0+GrJW26oFPI
zNrqb18pafX6ecj6t%hNNWDUzgMn;Ajh6OS;tWcJ04I7A+!W7H^!kU7c-$g8EWPb^=
zL{o0^^r(H4qob=B<0fB=j%MRx<YQ#{y4f@)gpqN_<gQpRw%-g43`Kt?--`8NKg7(y
zQ1qW+vu>OxJ0t7l@>ESm*2xP~?HMm@ewZ4;%*Zp@D)Td=)MUM^+l-2nxw4(vL>L(u
zibN*|XWO!=voJ6eiEo~ky_S(ta`M7fVQ$XSf|SIP)UeDd-N{M0T|%EgHsv!gFlaIr
z1%cR~CjZOTVzi#Dndi+YJ-IZ`Sm+B#7+J}e$^UZICO^nyVY3IBFFW~Fo)e?=WZis|
z$t&_yC%5HGa65w~7#SF<R3^XAlb?JdKi~2n$Q(wH;{-v35Qq>45n>=hf{}sY7E4NI
za!HXCh%W;o<UoY{<bnb(#<0!13*<Q%RVUxB@L}YgtWg=ss4=;z(w;G8@}bHC79~ao
zhRFxAB{%z5O=48g1R2I!Se#jvdW)-|v?M++zdSLsWF^#(N|PCD#2IHzmaK7MwA~z6
z<Il!s2eQc#6hxDcHHk1T-+a4i36qjONYoHS7=cV=PfRW>%`8g2#a@(}lbTqZS_IOe
z$9QV;h1LVU8X$GrAa$&%xdkPax46<%VKx_;gCs$Q6gh)fCSYYb`H3mTMaCcj0}$Z?
zB78u><U4tPhd9?Q_Vm;em&B69+nfKjRWq7_ZO{WLVgTu9E6vNv%*!rv1L?>E5ujKq
z(gG<k0}-HLC~}y*zC(xc>Et^dIid<6(Ik*cClJdOM0ie)?Nns+*j(TFm66e8a%uNr
GMppok{^Cdg
delta 710
zcmX>ygK6gsCf<BrUM>a(1_q_Yk?CF=dE3JnLpDzhJIBbVG1)C#VRB1^nMsLQiA*b#
zBttE04P%K!i7c4SR>QDBu7-6XBO^l%!vgslHV`X?DVPC-H3c^Rh*-|Zq^U4@S=2sO
z#>!ia<&%@6s~KY^-;a)F;bUa^w%H*jgpqOk<f*Y<oPQV?7>fQfFfdH^ja8h?8K=R1
zkePv@h=FmlMVu!)BirPrR82;<$!k*W8P9KilN!Oy$UWIL^E0F5WXr7Ej0%%Qvz^&Q
z85tOg#3sjO+p?*#FfbHJY+jhXmXT3vvVX2Dqt4{q+%BPyAZzj&7#K8}ih@AwkCO%C
zgeS}7i8ESFHp%m5l$qR+XFNG4Uv;ufo-l7tYFddM$S65R28PKFd5V)I^W_+2CR^m2
z2z>)-M>gsk*r<v55^PRjEtA*hPqz9GQpE&vm=K5%1`#44LL5X$GBPmSVoAwNE-8`*
z@nu1TJcv+WoSfGwK6!G15o74)V+HaYjB1mgRroOSO*XEKWYnD8TWQajJo#*80i*I}
zyQ+1Jj9Qc5R?A5$gY;U0>;w~5AXX9s1H;V8Dm5;QcAGP6{Mp#-L5iFv?`u+LT(<dH
z(-I~n1CWRjh%g4Zh&?g6ur#wM^%i?kYEEimacYs%<Tb5&j3+1GZau)J2~wvsd10Hl
zngxgn(o*CCVwr+8v*zR{rW6;MfCLOdgzMycZCZ@CHgmRDGjfA9>w|<DC--+q3uuFw
z<{&}~L^w{~)uF@qWb*Tl98pD(0Lbx0&LEZ>i13=6(W%JjzPYROD<h-n<c99Uj4l9-
CiNO{C
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc
index 043b8caa77c5bae3e06d5fdbbc08ff8f4f781c35..bc4e2818b028c9a713a54e1a7e4d188e056df7b9 100644
GIT binary patch
delta 18
ZcmX@Wcz}^BpO=@5fq{Wx{icar+W;yc1kwNi
delta 18
ZcmX@Wcz}^BpO=@5fq{YH((H*`+W;za1l|Au
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc
index 0249803d684c8194126402b59550e820d3f02aae..bd525f88ef2485c470b94afc01afe489688494ed 100644
GIT binary patch
delta 19
acmdmEzQ>#^pO=@5fq{Wx{icmvo1_6YAq8y!
delta 19
acmdmEzQ>#^pO=@5fq{YH^6ZUVo1_6Yh6Q>6
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
index db758e965347cd0bbe68439b146cea4c58aec265..46908832b061d8500ffd3ddedf0c04ef12d83254 100644
GIT binary patch
delta 18
ZcmdnPxQCG|pO=@5fq{Wx{icarn*b@41jYaW
delta 18
ZcmdnPxQCG|pO=@5fq{YH^6ZIRn*b^71k(Tj
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
index 3ee65b4a22bb546dcb73a46d96079bc3ae5fa78e..e2215d22c3eadbb5fecdbc20c727397dd704569b 100644
GIT binary patch
delta 18
ZcmdnRxQmf1pO=@5fq{Wx{icar8vrRz1i%0Q
delta 18
ZcmdnRxQmf1pO=@5fq{YH((H*`8vrSx1k3;c
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
index 4956a36b4841594d3d70f849609a714bfd194868..e35fbc28222a465baaca0356405c91375c026a23 100644
GIT binary patch
delta 19
acmccbao>Y0pO=@5fq{Wx{icmvp(+48o&|vb
delta 19
acmccbao>Y0pO=@5fq{YH^6ZUVp(+490|kx%
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index ace01fb..f056d26 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -216,7 +216,7 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager,
- grasp_pose, iter_n, past_data):
+ grasp_pose, goal_transform, iter_n, past_data):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -314,6 +314,11 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
+ pathSE3_handlebar_left = []
+ pathSE3_handlebar_right = []
+ for pathSE3 in pathSE3_handlebar:
+ pathSE3_handlebar_left.append(goal_transform.act(pathSE3))
+ pathSE3_handlebar_right.append(goal_transform.inverse().act(pathSE3))
if args.visualize_manipulator:
if iter_n % 20 == 0:
@@ -333,10 +338,9 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
if robot.robot_name != "yumi":
runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
- # MASSIVE TODO: actually have different references for left and right arm
else:
- runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
- runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+ runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_left[i]
+ runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_right[i]
# idk if that's necessary
solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
index 0806428f665ab55390f7bc917ea3c79c554b8034..04a1ccef3f6b4dfc6021b9488af0d53b8e27cf34 100644
GIT binary patch
delta 18
ZcmdnWxRsGBpO=@5fq{Wx{icars{tsV1hD`B
delta 18
ZcmdnWxRsGBpO=@5fq{YH((H*`s{ttT1ib(N
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
index ffdca2fdd15c7955b24c823c089a647bc847f159..addfb91192daa6a5085310358c6bcd33c1802a29 100644
GIT binary patch
delta 18
ZcmZ3^xSWwIpO=@5fq{Wx{icara{(r`1cv|s
delta 18
ZcmZ3^xSWwIpO=@5fq{YH((H*`a{(s^1d{*&
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index dd179f150c733a47bda1f946bd294d5c5f4bb05b..48c5c7d75670106ce36c3db1cfd7fcc17c5eed69 100644
GIT binary patch
delta 19
acmca+ea)IHpO=@5fq{Wx{icmvXJi37hXus|
delta 19
acmca+ea)IHpO=@5fq{YH^6ZUVXJi37>;=*Q
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc
index e97154fba239571a8fc08725ead074ba7acaab1c..4771e5aa53a3f3ac42dd177e6b27f00d8ec1cb6f 100644
GIT binary patch
delta 19
acmeBH>Q>^)=jG*MU|?WaziA^EqaXk)2LvVn
delta 19
acmeBH>Q>^)=jG*MU|?XlJbNP-qaXk)Yy>j^
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
index ad7422e9ee45004d7b00debb9d962d34e75cf926..12f837a1d098d83ea0eba85ab02537512288f3d1 100644
GIT binary patch
delta 18
ZcmZ3(xQ3A{pO=@5fq{Wx{icarivTBN1eO2*
delta 18
ZcmZ3(xQ3A{pO=@5fq{YH((H*`ivTCL1fl={
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
index 31da6f3686755ce93ed26ca5e2cff7bcc988cfa7..d71d90faa69cc38279f576660076700113dcd9ef 100644
GIT binary patch
delta 19
acmZ4CvciQcpO=@5fq{Wx{icmva!LR*>IBjN
delta 19
acmZ4CvciQcpO=@5fq{YH^6ZUVa!LR+PXylp
--
GitLab