From 68a143bcd58ecb26dadb13ba92a82ae77db90c7e Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 4 Dec 2024 23:51:20 +0100
Subject: [PATCH] made the template for the behaviour tree. put in clicking to
grasping and then pulling. that part seems to work just fine. need to
initialize path for pulling for it to make sense. this can be done when the
clik flag turns of (thus the priority_register should be saved in past data).
constructing the path for the gripper does not exactly work, but the code is
there and so it's just the logic that needs to be fixed. i will get more eyes
on that. next up is to make add a controlLoopManager funciton which can be a
callback to a ros2 node, and to add ros2 topic communication into
robotmanager under sendqd and getq. after that we can do vision and tune
stuff.
---
python/examples/cart_pulling.py | 139 +++++++++++-
.../__pycache__/managers.cpython-312.pyc | Bin 54489 -> 54697 bytes
.../basics/__pycache__/basics.cpython-312.pyc | Bin 12790 -> 12790 bytes
python/ur_simple_control/clik/clik.py | 5 +-
python/ur_simple_control/managers.py | 10 +-
.../optimal_control/crocoddyl_mpc.py | 140 +++++++-----
.../crocoddyl_optimal_control.py | 34 +--
.../path_generation/planner.py | 214 ++++--------------
8 files changed, 285 insertions(+), 257 deletions(-)
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 103355a..08d59cf 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -5,14 +5,15 @@ import argparse, argcomplete
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoEEPathFollowingOCP, solveCrocoOCP
-from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoEndEffectorPathFollowingMPCControlLoop, CrocoEndEffectorPathFollowingMPC, BaseAndEEPathFollowingMPC
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import *
+from ur_simple_control.optimal_control.crocoddyl_mpc import *
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse
import pinocchio as pin
import crocoddyl
+from functools import partial
import importlib.util
from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
@@ -26,7 +27,8 @@ def get_args():
parser = get_OCP_args(parser)
parser = getClikArgs(parser) # literally just for goal error
parser = getPlanningArgs(parser)
- parser.add_argument('--handlebar-height', type=float, default=0.5)
+ parser.add_argument('--handlebar-height', type=float, default=0.5,\
+ help="heigh of handlebar of the cart to be pulled")
parser.add_argument('--base-to-handlebar-preferred-distance', type=float, default=0.7, \
help="prefered path arclength from mobile base position to handlebar")
argcomplete.autocomplete(parser)
@@ -43,6 +45,127 @@ def get_args():
args.n_pol = mpc_params['n_pol']
return args
+def isGraspOK(args, robot, grasp_pose):
+ isOK = False
+ SEerror = robot.getT_w_e().actInv(grasp_pose)
+ err_vector = pin.log6(SEerror).vector
+ # TODO: figure this out
+ # it seems you have to use just the arm to get to finish with this precision
+ #if np.linalg.norm(err_vector) < robot.args.goal_error:
+ if np.linalg.norm(err_vector) < 2*1e-1:
+ isOK = True
+ return not isOK
+
+def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
+ path_planner : ProcessManager, i : int, past_data):
+ """
+ cartPulling
+ 0) if obstacles present, don't move
+ 1) if no obstacles, but not grasped/grasp is off-target, grasp the handlebar with a p2p strategy.
+ 2) if no obstacles, and grasp ok, then pull toward goal with cartPulling mpc
+ 3) parking?
+ 4) ? always true (or do nothing is always true, whatever)
+ """
+ # we use binary as string representation (i don't want to deal with python's binary representation).
+ # the reason for this is that then we don't have disgusting nested ifs
+ priority_register = ['0','1','1']
+ # TODO implement this based on laser scanning or whatever
+ #priority_register[0] = str(int(areObstaclesTooClose()))
+ # TODO: get grasp pose from vision, this is initial hack to see movement
+ if i < 300:
+ grasp_pose = pin.SE3(np.eye(3), np.array([9.0 + args.base_to_handlebar_preferred_distance, 4.0, args.handlebar_height]))
+ else:
+ grasp_pose = robot.getT_w_e()
+ priority_register[1] = str(int(isGraspOK(args, robot, grasp_pose)))
+ # interpret string as base 2 number, return int in base 10
+ priority_int = ""
+ for prio in priority_register:
+ priority_int += prio
+ priority_int = int(priority_int, 2)
+ breakFlag = False
+ save_past_item = {}
+ log_item = {}
+
+ # case 0)
+ if priority_int >= 4:
+ robot.sendQd(np.zeros(robot.model.nv))
+
+ # case 1)
+ # TODO: make it an argument obviously
+ usempc = False
+ if (priority_int < 4) and (priority_int >= 2):
+ # TODO: make goal an argument, remove Mgoal from robot
+ robot.Mgoal = grasp_pose
+ if usempc:
+ # TODO: make it not hit the handlebar or other shit in the process
+ for i, runningModel in enumerate(solver_grasp.problem.runningModels):
+ runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+ solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+ robot.Mgoal = grasp_pose
+ #breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
+ CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
+ else:
+ #controlLoopClik(robot, invKinmQP, i, past_data)
+ clikController = partial(dampedPseudoinverse, 1e-3)
+ controlLoopClik(robot, clikController, i, past_data)
+
+ # case 2)
+ # MASSIVE TODO:
+ # WHEN STARTING, TO INITIALIZE PREPOPULATE THE PATH WITH AN INTERPOLATION OF
+ # A LINE FROM WHERE THE GRIPPER IS NOW TO THE BASE
+ if priority_int < 2:
+ breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
+ #BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
+
+ q = robot.getQ()
+ p = q[:2]
+ # needed for cart pulling
+ save_past_item['path2D_untimed'] = p
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
+
+
+def cartPulling(args, robot : RobotManager, goal, path_planner):
+ ############################
+ # setup cart-pulling mpc #
+ ############################
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ problem_pulling = createBaseAndEEPathFollowingOCP(args, robot, x0)
+ if args.solver == "boxfddp":
+ solver_pulling = crocoddyl.SolverBoxFDDP(problem_pulling)
+ if args.solver == "csqp":
+ solver_pulling = mim_solvers.SolverCSQP(problem_pulling)
+ xs_init = [x0] * (solver_pulling.problem.T + 1)
+ us_init = solver_pulling.problem.quasiStatic([x0] * solver_pulling.problem.T)
+ solver_pulling.solve(xs_init, us_init, args.max_solver_iter)
+
+ #############################################
+ # setup point-to-point handlebar grasping #
+ # TODO: have option to swith this for clik #
+ #############################################
+ grasp_pose = robot.getT_w_e()
+ problem_grasp = createCrocoIKOCP(args, robot, x0, grasp_pose)
+ if args.solver == "boxfddp":
+ solver_grasp = crocoddyl.SolverBoxFDDP(problem_grasp)
+ if args.solver == "csqp":
+ solver_grasp = mim_solvers.SolverCSQP(problem_grasp)
+ xs_init = [x0] * (solver_grasp.problem.T + 1)
+ 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)
+
+ log_item = {}
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ save_past_item = {'path2D_untimed' : T_w_e.translation[:2]}
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
+ loop_manager.run()
+
+
if __name__ == "__main__":
args = get_args()
@@ -51,7 +174,6 @@ if __name__ == "__main__":
robot = RobotManager(args)
robot.q[0] = 9.0
robot.q[1] = 4.0
- #time.sleep(5)
x0 = np.concatenate([robot.getQ(), robot.getQd()])
robot._step()
@@ -79,7 +201,9 @@ if __name__ == "__main__":
planning_function = partial(starPlanner, goal)
# TODO: ensure alignment in orientation between planner and actual robot
path_planner = ProcessManager(args, planning_function, robot.q[:2], 3, None)
- #time.sleep(5)
+ # wait for meshcat to initialize
+ if args.visualize_manipulator:
+ time.sleep(5)
# clik version
#cartesianPathFollowingWithPlanner(args, robot, path_planner)
# end-effector tracking version
@@ -88,9 +212,10 @@ if __name__ == "__main__":
# and also make the actual path for the cart and then construct the reference
# for the mobile base out of a later part of the path)
# atm this is just mobile base tracking
- BaseAndEEPathFollowingMPC(args, robot, x0, path_planner)
+ cartPulling(args, robot, goal, path_planner)
print("final position:")
print(robot.getT_w_e())
+ path_planner.terminateProcess()
if args.save_log:
robot.log_manager.plotAllControlLoops()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 0343be7262ccf487703d207bbfbaeb2cdb691335..e02e89e5479c52cad283376b57866178a3e4284e 100644
GIT binary patch
delta 2937
zcmcb)l6mE7X5Q1hyj%<n3=CgO0@D9)<h{wn(Z|ZbFrA@<ak7B2%w{fTYerrkkOB}+
z5lIo9yntPqQEc)B7I|jz6t2k%EXs@$AReRS<PR*Wj8b5>G>9!MQ^En#1;Qz^DRSuy
zDe{vOm{s`|N)RS0rYM0-<X0}?1E~Yy6qOWJkQk#H$Sh`ch%J*f*-Us5YIIU`!D{rt
zYV;v$7!ANYLkMrOB)bVO!Ubk2=3pHbU^SKyHT+gcZm~|W0jUtQMRJQ>iv1dg$rD%=
zC#!P^FoIc%E=cmODQ;`{SF?cJ&A`COP{R<+py|HZgX0)eJ-4oIa!FB+Zdy@lVU;Y1
zEKSWzu2jfRQz*$uRY=ayD=Es)QOL>9FUb7D$iPq~sH>ZnSX`o8oS9pilUS0OpI7CV
zlCMyiU#d`^m{+2J5LYP4S4b>Y0I3Gi1^LCrnMpaR_6izBsfjtdC7HRY3Y){ZJGdBw
zCO;6}DLjLBjq?pbgB{8rm>F0lzA!Md+E1P!CdQX|A-e!BRJ3`En2-SD>CMO0c5ySl
z+1zBr$j#`nxy6>7iE+W^3HB>l85K5LxvpkrsbS2rn9Nuz!V@nG@i+rR3fJU^GsMKI
zxEUB~7~<vN!rV0s@$!?W`zbJG*-buZD$FXyz)-|LdA_GOH&3<;14A*B7ef={<oSN8
zj82medWs_CrIF-Q_-Yv9m0*U}FiyVcCBjt0Fj>({gd-blB3Mk67fBCTL`0~DAzm4#
zF$Lr{mB|NB$uVVlOg`u-%*a1^u9qnzn9V7Wt--)h<X_@FIWSk4R|Tf2h9S#i@<ub^
z$$8$wlc)QN38V<FW`an9;u|8#=s%g!Qhsuuw+PfQA+TZmC4nHr*dT^YF7UCKEZ`%@
zs5x2RN1c_oh9N5m><Zz@`97A6A(I7tWZAUAMuU_>1Q>ND@AQ+CDxOjj4VGbGU`P>J
z%?M#LGL*zl?)TN?)`##J7)lbsd_y=t6~yN?hVg3{veLl5aq<%e#iP^Y#A+Fq8iuUw
z$&8j#EHw=AW|I^BOnJ@V8W^*jKyl3!uQ++9pE6_KWWx!<%(Ef(@WSn>VaQ4XnafxR
zGKZ~}xrRBb800yA9)x3IV&X_*C8{u+vPvc=<_S+OC=p=PoP5^boUt6Ff3ie?5M$+J
zMoSr%N(N2Q$=(5GliLD97;Put4mi%m3CdydpuDwtVPG~R+ayK?hGO&0LctA;oFCX2
zWEAGx&9vJ*HN=6LQE2kD@B*Pt3=9mKOhwBW7#NB?LB!_C5fPe<qMMr{3Yi!;P5u(K
zj8S6q>}WAYQHau8Y~@9%dC3_t)yHBC8MQb6h}q1-xMK2}L~Z`{3=9mn*b~cAixSgQ
zi`GnjmgvT~da`bkETi6L@1#<8&7B}?c7cd$5Rn2R!a)Ss5xYUG*&t%i<d>PsY|BB+
z&69<))ER>|TV)+(<X#C9Sp^~%Oy<gwV_Y>^Ge?23YO_~P5sSbIkoY<f;Q}HSPd-y%
z#<+embD^T-DFz0HDxskKr2G=!#5|DW^y1S~O9ERr+ZJAAW4j1ae`&Hvg_ab$At0H{
zAmYm8hzeO#kPnNn`2xx9Ak`6|uw^Oo2eJ1~UR7bwxMK6GioGlnAoFi=r>B-UCYO{Z
z<^-k`K^?olR+<fL`m)InYNZ)ZPySOY!FYYMXx$SgZm>8=XVH<#ER70``zNb5S~33I
zoX{A?XaSBJ&Z10^Rh1wDNi~SO21M)!#SB|fVqQvqZqZDT$mYrKnvB`lnHd<0gD3m8
zg>0^F_F_`OrMKu2NDo-!&B-rYl^9P>=4q2+JTqCnO@Wbrvqzf)yTl@p60kGsz_t|^
zq^72L70sW#pjQ^+1#m3yoP45Jf${F-m%T=ea+_uPYM2=pPp+FR%g&NtkeXMtdGg+g
zri}iRZ%_8$>@>-Qk>evMDA|iZYE&3EPhLFPoH26q?a6Dnj6tzlv;|}<wirQ#1ybBh
z-ZjsX?HNeRipf9c*)sl~Y(8I;@%QA!`N@pwn~%<CV`bbt`OK0)#>JcEmZmW>Zl2t_
z%#d;8=IzTg7}@TF^lhH}bh$a>j>)ns_RH`xGB9Ye6eWYiL4Gc32C<;Zeyp%&+&5Wv
zh0^A*m9k8Xf|E;D8!;Z4ynMAE<EF{qRx8zmUA_&ZBMC$t01>M}L@bB^r*g0lZ-Tg|
zLBs<P@fbv$0THzz;xmZ&1|rxQ85oK<K?FC5-~$l?j0_C7SW+^RONxX+{BDp!a}Z%V
zdHNb&$udwOF@b@Bp@rcilNc+<C$lO>R?$xmESvYPk>h53I{D7_>%2Qa(aBnzkywyg
zBt7}S4j)G5$-Fz=L^goLc{1}dOX8FBb8{2(Qi|jzr|*nme7E_?P7@|J2}TBnWTwf5
zRicv@?5o+_zbAu{QGW7|y>g5alg0NLFsf|!*eA=*W&(1U`Q+@Q9*l;Yw;au5RtH&F
zqzV#N2N4<|Pp~H@7nWugrQTvMO3g`4EKV&lpX_-uj4@>Mf|I^XkxxOXgs(^rq+SuE
zo;5YMprrB^S9)qmd|rNeVrEH^E=Uq&Sdle|r3F@&lb@JUT%-vSPy-P*AOc(*dVzvz
z^JKR(`i!ZQ%g*d#>{@JimT|JjcHznXr`Q>#Cnru7+w5{Kfsyg)<fZ46y}@>?fV6)F
z1q@qhUQT9SHY9(igUreR5egs$Iv_$GM3{jb!C8`8l$)8CSdv<F9i(RS<lqY??D;GV
z48^RBllv|tD01^(=a#$3Ew@1N61VaV9)asTau<2zuJdSK<k4KAafQctGULS_DGf&<
zXB&pYHja$0Ow30aL1YoQc;KG=Z?%-D2*|=jkOh_?mMw^Intbh|BIBjWKQ3C!^07KI
z7J(CoFDNvPZgCc5=H(|RXJqCV?VKENNuC>A27y|5MJqQ~U7E_ss5M#OiXmg@WTz{s
tM$wFnLf>ncLYNu(KQe&G?>THyoQ(P(89?Os3ZZN!MrX!P3}6au4*&_TC+z?L
delta 2869
zcmZ3vn)&8RX5Q1hyj%<n3=HR&`KOC+<h{wn5yQ&BFrA@<aWelhnay0x){K+?Gs`iH
zq;O4EU{Piio&1kko>6SF9E&QWIG8N~Vhc-_aDa>f;S{M9>2!t^naK&vs{FDgJRmU;
zPLWHIpL~E-nO~uV55x!I6vY%JkQk#f$Sh_Rh%Nl8NGjA))IlmHYqFVuU7`a~!>9|^
zp$Fmd>m%83kYWhdA;}KWVFp!Wj^q}L6ibjAK`SJitW#{(*iL@Hq9|yGBx;}H01=(6
z&LJSkznTT)ZUzQMh8l)o22GdERUF5dCKqxGYkH>SD^%u}DwHSYl_(Tv=9cCpmSpDV
zDU{?Z6qV*F<d>xuDVQ1<cvNlP#66XZ(QLAZ*iPvS653lluA2m1Gzq#P7;_;l`vWrr
ztHc)uM%LWT7UDtzj3+mHtMB4we6^X)n30>&Z8L`*HxuK$%_0sfSs8gY?{i%ZikcdR
zco~SZ7#LExYZ$UDCQtN~l$M2y@zgNH%fWaxj9GS*A4G~W+DxABCCz9z+0j#&pEuiu
zfuWemi=l}TDlam5yQczA3SYJq149u%SkTc^SQbfP3V#hlydsjZE?{Gwz{a{@F%~2*
zA`q_x(~`na!w|1LnbBLG(G8@WT_D?>fuU&5WJfcx$${RI5K&ISYz+p6BL5Q4$&Oou
zd6i+hYZ$WJCMTK;GkQ<n?XAf#w3-Pb1Bx1m%;a6(!jl79xhB8dE)F$I7;F}Qi9g6J
zM&-%zzP6J$cu2F<FvM$2PMj_`d44fFJAVpa4MSD{$Vp5hHj@i{EGB>Q5uUu>mzz-w
ztOLrM9Ouh9InrNDs(4CCB*bzCh7{4&j1V>>LrL`HXg^JEJqVwHp(Gy6H-Ph#L3~~#
z7{7)gD+S~w?sS+41H<H<o^q4d`*N^>{gpX+W0MrfwWgDQ`k6ACPTuLMFj?1&cXEus
z1Y^$R!T@3B*$`WJ;kMK;WF>%1Wy}X@p8VdQm9Y@yH-27(OKX_3z+&P^VkN3DS7a4U
zb_`Hv)R_F*U4HV0nJkk#0z?=~Cr>O95vpO%WJEHCA*&o@2^%9rC4;8e<i`PKteIaJ
z85kx@28J-&OwJ2DzS$uti;-;tBLhRR;pPp&4UEhWq~$hShB`1a@=vacC=lAnz`&r%
zRJ4?VfuYC~L~NS;Cqk1^XtP>mArs@q$upvtF^X=sj}c=Og($tnR$i2vmz)7pofT`y
zsIhrY>}D3m<&y)FwE5RDFfiO=Pb^C<N=#2JT0OZd$&GQ<<cmqNjM|&uCzY~m?f_Y{
z6GT*lh!hYJ4kEyg*ac$E1`)d__hu=xEdwz(P2QHJ&gj4SUe-}Y?iC=Bl^|mN<aIf6
zj4LOf%~4=1-~2A8h(%yINPI1bZ~+mECKnW%F|M1uyiif{Bm)COl~7QAQhteVVjjqG
zdhzM0C4o(w9~NF@W4i!Se{u4T3N0ygLqIZ@K*Z(Ae=1~6K|U<P<_jdZgH%U=!j`4T
zAH?1>*{{-`ap~s1%DpV1AoFi=r>B-UCYO{Z<^-k`K^zMTIqvkL%z}c{qWH4ZoFWg9
z)=Cfowi6UUnoM8=Z!s076f-d}FeoS#nSvCYn!KP+g7M1copn!`xWTGHwiF$nyrNNo
zao^;VjaH1`HnTK^F<OA*k+Uchqzhp-L^VikHHg>;atm8gVqQvqZqZDT$fn7YnvL05
zm>C#~gC~D%3)w8+;>DzbWD`gcTyN1s5ErcR#^l~MCB_qzH?&DHo}PTVO@WbT^P4sY
zc87%^C17XNfo&@;NKH-gDw+opP6LUs7o`?wBo?IJV#&!YE`fvw*g{Z16z!Ot)2G0A
zXL4_!5u^0x1AR5jjEg4QPEuyvG&ymSDWlKi`YHaKpG`7h<oE!J2=*e7tn6g_DeR1!
zCc95DXAIk1KV=P<F(_RWZ3bD2Elv?3gcQ4z<K|nkJq2l5K6&nZTb5r83=ES$t`?d6
zX}%ESugR<nk{MGrXD(o4W!yBmU}+%Zg3X7PrZF;Znyk6pka6AS=;a!WZ1+IgH%;zb
zVa~X9^1&7R8F?l*t`vm{&sk~BxO?)!6-t|bt(0YA6qqcr#)$FYWbZY8j2kD<TBB4C
zcJNk^8A%{wKZsZbB4R-VID3FSb_2vc1tRW)h({peG>E7L5uZTBR}jI*$iPs<0V23S
z1TTo-XJlZw#gdYlTv8+i;&+1-nu7?_$<}LmB}+i%$^-@mh8Bj8Ok%7YpUf&5Sw%n9
zF>g*>E62_FWOBof>%7}RG06&!$0Di8DLZ`_nI>=C=_axsB+iqWmst{@oS&PUn3qx{
zJDGb|4CCv~8M{oF7{xcw-Sdx;QEu{_eR7QAlXvekU{u`vW}hrOn=!~OW|R4kc`)j2
zjyRUbtPZlHNChOU1|rl!?q*L+E-cL~O1;Hil$w*8Se#m9Hu>$zFvh^mE~k8%c%OjM
z1YeOXNWH@3d1u5;bwEsz)*>qqOA}-WYfgS*N^y|}NI(@tSc3>~LFokwhfR}TozZ7Z
znk;p87gG+?<c$$hlRun&y7}|DVn)U%lhZFGdxI@d2I=|&@-SOzUQT9SHYCG?DxRVY
zkU8=o1==7&4n&xO?B*;<Ey~TzODst(x&~6SX|m`=6ZRYy28QDA43h&cCJ1xzT<4Iw
z$RTx|L-itu>Vk|b9Qu<lUhMIdbQE&dWjL(s$mn9se8dPu7J+j;cTpk8qz(|#2_m{c
zL<oow2HBekVp)J#HXy=r^0G^cjOQnxxMVHM%j(Qn1de-OP{<kG;w;F_%TG?u$jmR=
zF<IfVJU6%m0kznQR&4gVJe84AbMk{LhKwPTg|4O=MKUr9eXn2&VP@q2$N(b0=deX`
YGU|V10FmFzgtC|zof$tdfGMy&0QU$HoB#j-
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index e1f51ccc304659f0c256d15a324b24fc9c0a6ee0..5f7f84bdf9b600f8d0c911ebd0919c211ea1c011 100644
GIT binary patch
delta 20
bcmeyC{4JULG%qg~0|NuY`_6!k+^-D*Q7Z>c
delta 20
bcmeyC{4JULG%qg~0|NuYzfy;d+^-D*P|OE7
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 7b53005..c26e75c 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -92,9 +92,8 @@ def invKinmQP(J, err_vector, lb=None, ub=None):
# TODO: why is q all 0?
q = np.array([0] * J.shape[1], dtype="double")
G = None
- # TODO: extend for orientation as well
- b = err_vector#[:3]
- A = J#[:3]
+ b = err_vector
+ A = J
# TODO: you probably want limits here
#lb = None
#ub = None
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index f6731d0..70e8783 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -88,8 +88,10 @@ def getMinimalArgParser():
default="192.168.1.102")
parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction, \
help="whether you want to just integrate with pinocchio", default=True)
+ parser.add_argument('--ctrl-freq', type=int, \
+ help="frequency of the control loop", default=500)
parser.add_argument('--fast-simulation', action=argparse.BooleanOptionalAction, \
- help="do you want simulation to run over 500Hz? (real-time viz relies on 500Hz)", default=False)
+ help="do you want simulation to as fast as possible? (real-time viz relies on 500Hz)", default=False)
parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction, \
help="whether you want to visualize the manipulator and workspace with meshcat", default=True)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction, \
@@ -440,7 +442,8 @@ class RobotManager:
#self.JOINT_ID = 6
self.ee_frame_id = self.model.getFrameId("tool0")
#self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
- self.update_rate = 500 #Hz
+ # TODO: add -1 option here meaning as fast as possible
+ self.update_rate = args.ctrl_freq #Hz
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
@@ -1135,7 +1138,8 @@ class ProcessManager:
self.shm_cmd.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
- self.command_queue.put_nowait("befree")
+ if self.comm_direction != 3:
+ self.command_queue.put_nowait("befree")
try:
self.side_process.terminate()
if self.args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index d0a17c5..6446153 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,6 +1,6 @@
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoEEPathFollowingOCP, createBaseAndEEPathFollowingOCP
-from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam, path2D_to_timed_SE3_base_and_ee
+from ur_simple_control.path_generation.planner import path2D_timed, pathPointFromPathParam, path2D_to_SE3
import pinocchio as pin
import crocoddyl
import numpy as np
@@ -54,13 +54,13 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data):
robot.sendQd(vel_cmds)
log_item['qs'] = x0[:robot.model.nq]
- log_item['dqs'] = x0[robot.model.nq:]
+ log_item['dqs'] = x0[robot.model.nv:]
log_item['dqs_cmd'] = vel_cmds
log_item['u_tau'] = us[0]
return breakFlag, save_past_dict, log_item
-def CrocoIKMPC(args, robot, x0, goal):
+def CrocoIKMPC(args, robot, goal):
"""
IKMPC
-----
@@ -69,6 +69,7 @@ def CrocoIKMPC(args, robot, x0, goal):
a dynamics level, and velocities we command
are actually extracted from the state x(q,dq)
"""
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
problem = createCrocoIKOCP(args, robot, x0, goal)
if args.solver == "boxfddp":
solver = crocoddyl.SolverBoxFDDP(problem)
@@ -78,7 +79,6 @@ def CrocoIKMPC(args, robot, x0, goal):
# technically should be done in controlloop because now
# it's solved 2 times before the first command,
# but we don't have time for details rn
- x0 = np.concatenate([robot.getQ(), robot.getQd()])
xs_init = [x0] * (solver.problem.T + 1)
us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs_init, us_init, args.max_solver_iter)
@@ -112,6 +112,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
+ # TODO: replace with shm
if not (type(path_planner) == types.FunctionType):
path_planner.sendFreshestCommand({'p' : p})
@@ -124,7 +125,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
data = path_planner.getData()
if data == None:
if args.debug_prints:
- print("got no path so no doing anything")
+ print("CTRL: got no path so not i won't move")
robot.sendQd(np.zeros(robot.model.nv))
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
@@ -203,7 +204,7 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
loop_manager.run()
-def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
+def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, iter_n, past_data):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -219,54 +220,58 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
q = robot.getQ()
T_w_e = robot.getT_w_e()
- #p = T_w_e.translation[:2]
p = q[:2]
- #print("CTRL:", p)
# NOTE: this is the actual position, not what the path suggested
# whether this or path reference should be put in is debateable.
# this feels more correct to me.
save_past_dict['path2D_untimed'] = p
+ path_planner.sendCommandViaSharedMemory(p)
- if not (type(path_planner) == types.FunctionType):
- #path_planner.sendFreshestCommand({'p' : p})
- path_planner.sendCommandViaSharedMemory(p)
-
+ ###########################
+ # get path from planner #
+ ###########################
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
- # get the path from the base from the current base position onward
- if type(path_planner) == types.FunctionType:
- pathSE3 = path_planner(T_w_e, i)
- else:
- data = path_planner.getData()
- if data == None:
- if args.debug_prints:
- print("got no path so no doing anything")
- robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
- if data == "done":
- breakFlag = True
-
- path_pol_base, path2D_untimed_base = data
- #path2D_untimed_base.insert(0, p[1])
- #path2D_untimed_base.insert(0, p[0])
- path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
- # should be precomputed somewhere but this is nowhere near the biggest problem
- max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
-
- # 1) make 6D path out of [[x0,y0],...]
- # that represents the center of the cart
- # TODO: USE THIS ONCE IMPLEMENTED
- #pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol_base, path2D_untimed_base, max_base_v)
- # TODO: separate out timing construction so that it can be used for the handlebar path as well
- pathSE3_base = path2D_to_timed_SE3(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
- # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
- if args.visualize_manipulator:
- if i % 20 == 0:
- robot.visualizer_manager.sendCommand({"path": pathSE3_base})
+ # get the path from the base from the current base position onward.
+ # but we're still fast so who cares
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("CTRL: got no path so not i won't move")
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+ if data == "done":
+ breakFlag = True
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+ ##########################################
+ # construct timed 2D path for the base #
+ ##########################################
+ path_pol_base, path2D_untimed_base = data
+ path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
+ # ideally should be precomputed somewhere
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+ # base just needs timing on the path,
+ # and it's of height 0 (i.e. the height of base's planar joint)
+ path_base = path2D_timed(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
- # now find the previous path point of arclength base_to_handlebar_preferred_distance
+ ###################################################
+ # construct timed SE3 path for the end-effector #
+ ###################################################
+ # this works as follow
+ # 1) find the previous path point of arclength base_to_handlebar_preferred_distance.
+ # first part of the path is from there to current base position,
+ # second is just the current base's plan.
+ # 2) construct timing on the first part.
+ # 3) join that with the already timed second part.
+ # 4) turn the timed 2D path into an SE3 trajectory
+
# NOTE: this can be O(1) instead of O(n) but i can't be bothered
path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
handlebar_path_index = -1
@@ -276,11 +281,33 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
break
path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
# i shouldn't need to copy-paste everything but what can you do
- arra = np.array(past_data['path2D_untimed'])
- path2D_handlebar = np.vstack((arra[i:], path2D_untimed_base))
- # now we need to construct timing for this
-
-
+ path2D_handlebar_1_untimed = np.array(past_data['path2D_untimed'])
+ # NOTE: BIG ASSUMPTION
+ # let's say we're computing on time, and that's the time spacing
+ # of previous path points.
+ # this means you need to lower the control frequency argument
+ # if you're not meeting deadlines.
+ # if you really need timing information, you should actually
+ # get it from ControlLoopManager instead of i,
+ # but let's say this is better because you're forced to know
+ # how fast you are instead of ducktaping around delays.
+ # TODO: actually save timing, pass t instead of i to controlLoops
+ # from controlLoopManager
+ # NOTE: this might not working when rosified so check that first
+ time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
+ s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.n_knots)
+ path2D_handlebar_1 = np.hstack((
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:,0]).reshape((-1,1)),
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
+
+ # TODO: combine with base for maximum correctness
+ pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ # TODO: make it work with just translations too
+ if args.visualize_manipulator:
+ if iter_n % 20 == 0:
+ #robot.visualizer_manager.sendCommand({"path": path_base})
+ robot.visualizer_manager.sendCommand({"path": pathSE3_handlebar})
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
@@ -290,10 +317,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
us_init = list(solver.us[1:]) + [solver.us[-1]]
for i, runningModel in enumerate(solver.problem.runningModels):
- runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3_base[i]
+ runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
+ runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3_base[-1]
+ solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
+ solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
@@ -306,7 +335,7 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
-def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
+def BaseAndEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
-----
@@ -316,16 +345,13 @@ def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
are actually extracted from the state x(q,dq).
"""
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
problem = createBaseAndEEPathFollowingOCP(args, robot, x0)
if args.solver == "boxfddp":
solver = crocoddyl.SolverBoxFDDP(problem)
if args.solver == "csqp":
solver = mim_solvers.SolverCSQP(problem)
- # technically should be done in controlloop because now
- # it's solved 2 times before the first command,
- # but we don't have time for details rn
- x0 = np.concatenate([robot.getQ(), robot.getQd()])
xs_init = [x0] * (solver.problem.T + 1)
us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs_init, us_init, args.max_solver_iter)
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index e323d59..09157b3 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -396,7 +396,10 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
NOTE: the path MUST be time indexed with the SAME time used between the knots
"""
T_w_e = robot.getT_w_e()
- path = [T_w_e] * args.n_knots
+ # TODO there has to be a path for the base and
+ # a different one for the ee
+ path_ee = [T_w_e] * args.n_knots
+ path_base = [np.append(x0[:2], 0.0)] * args.n_knots
# underactuation is done by setting max-torque to 0 in the robot model,
# and since we torques are constrained we're good
state = crocoddyl.StateMultibody(robot.model)
@@ -486,23 +489,21 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
runningCostModel.addCost("uReg", uRegCost, 1e-3)
if args.solver == "boxfddp":
runningCostModel.addCost("limitCost", limitCost, 1e3)
- # TODO: implement
-# framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
-# state,
-# robot.model.getFrameId("tool0"),
-# path["ee"][i], # this better be done with the same time steps as the knots
-# # NOTE: this should be done outside of this function to clarity
-# state.nv)
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ robot.model.getFrameId("tool0"),
+ path_ee[i], # this better be done with the same time steps as the knots
+ # NOTE: this should be done outside of this function to clarity
+ state.nv)
+ eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, 1e2)
+ baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
state,
robot.model.getFrameId("mobile_base"),
- # TODO: implement
- #path["base"][i], # this better be done with the same time steps as the knots
- # # NOTE: this should be done outside of this function to clarity
- path[i],
+ path_base[i],
state.nv)
- goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
- runningCostModel.addCost("gripperPose" + str(i), goalTrackingCost, 1e2)
+ baseTrackingCost = crocoddyl.CostModelResidual(state, baseTranslationResidual)
+ runningCostModel.addCost("base_translation" + str(i), baseTrackingCost, 1e2)
if args.solver == "boxfddp":
runningModel = crocoddyl.IntegratedActionModelEuler(
@@ -540,7 +541,8 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
0.0,
)
- terminalCostModel.addCost("gripperPose" + str(args.n_knots), goalTrackingCost, 1e2)
+ terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, 1e2)
+ terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, 1e2)
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index a93d6d8..f48dca2 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -463,14 +463,12 @@ def pathVisualizer(x0, goal, map_as_list, positions):
def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height):
+def path2D_timed(args, path_pol, path2D_untimed, max_base_v, path_height):
"""
- path2D_to_timed_SE3
+ path2D_timed
---------------------
- we have a 2D path of (N,2) shape as reference
- the OCP accepts SE3 (it could just rotations or translation too),
- so this function constructs it from the 2D path.
- it also times it as this is what the current ocp formulation needs:
+ we have a 2D path of (N,2) shape as reference.
+ it times it as this is what the current ocp formulation needs:
there should be a timing associated with the path,
because defining the cost function as the fit of the rollout to the path
is complicated to do software-wise.
@@ -487,6 +485,11 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
trajectory here, it's a man-made heuristic,
and we should leave that to the MPC,
but that requires too much coding and there is no time rn.
+ the idea is to use compute the tangent of the path,
+ and use that to make a 2D frenet frame.
+ this is the put to some height, making it SE3.
+ i.e. roll and pitch are fixed to be 0,
+ but you could make them some other constant
"""
####################################################
@@ -530,28 +533,24 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
# what it should be but gets stuck if we're not yet on path
#s = (i * args.ocp_dt) * t_to_s
# full path
+ # NOTE: this should be wrong, and ocp_dt correct,
+ # but it works better for some reason xd
s = i * (max_s / args.n_knots)
#path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
path2D.append(np.array([np.interp(s, s_vec, path2D_untimed[:,0]),
- np.interp(s, s_vec, path2D_untimed[:,1])]))
+ np.interp(s, s_vec, path2D_untimed[:,1]),
+ path_height]))
path2D = np.array(path2D)
+ return path2D
- ####################################################
- # constructing the SE3 reference from [x,y] path #
- ####################################################
- # 1) we need to add points so that there are args.n_knots of them
- # the -1 is here because we're finite differencing to get theta,
- # so we need one extra
- #path2D = list(path2D)
- ## in case there's too few
- #while len(path2D) - 1 < args.n_knots:
- # path2D.append(path2D[-1])
- ## in case there's too much
- #while len(path2D) - 1> args.n_knots:
- # path2D.pop()
- #path2D = np.array(path2D)
-
-
+def path2D_to_SE3(path2D, path_height):
+ """
+ path2D_SE3
+ ----------
+ we have a 2D path of (N,2) shape as reference
+ the OCP accepts SE3 (it could just rotations or translation too),
+ so this function constructs it from the 2D path.
+ """
#########################
# path2D into pathSE2 #
#########################
@@ -566,170 +565,36 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
# elementwise arctan2
thetas = np.arctan2(x_diff, y_diff)
- #thetas = thetas.reshape((-1, 1))
- #path_SE2 = np.hstack((path2D, thetas))
-
######################################
# construct SE3 from SE2 reference #
######################################
# the plan is parallel to the ground because it's a mobile
# manipulation task
- # TODO: enforce timing, interpolate the path accordingly
- path = []
+ pathSE3 = []
for i in range(len(path2D) - 1):
rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
#rotation = np.eye(3)
translation = np.array([path2D[i][0], path2D[i][1], path_height])
- path.append(pin.SE3(rotation, translation))
-
- return path
+ pathSE3.append(pin.SE3(rotation, translation))
+ pathSE3.append(pin.SE3(rotation, translation))
+ return pathSE3
+def path2D_to_timed_SE3(todo):
+ pass
-def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
- """
- path2D_to_timed_SE3
- ---------------------
- we have a 2D path of (N,2) shape as reference.
- from this we construct timed SE3 references for the cart, end-effector(s) and the mobile base,
- in that order. this is done as follows.
- 0) the path starts at the cart. we align the end-effector and the mobile base to be
- ON this path as well. to we can follow the path, we need to make the path smooth enough.
- that is considered a part of the planning problem.
- 1) when the initial plan is set, the base-endeffector-cart system needs to be aligned with
- the path. we can ensure AN initial alignment in the beginning when we grasp the cart.
- furthermore, we can compute the initial path to see what the alignment should be.
- then we can let either point-to-point clik or mpc align the bodies to this,
- or let the controller-planner architecture run,
- BUT then we need to test the stability w.r.t. initial point.
- 2) the cart is a rigid body, so it doesn't matter which point of it is being referenced.
- (one point localizes the whole rigid body because it's rigid).
- we select the point(s) on the handlebar where we will perform the grasp.
- 3) DUE to rigid grasping, the end-effector(s)'s reference is the same
- as the one for the handlebar. thus we actually construct only 2 references from the 2D path.
- the initial point of the path is this one.
- 4) the path is of some length (it's a variable in the planner). we ensure it is long enough
- to fit the base on it. this does mean that we can not GUARANTEE we avoid all obstacles,
- because the path-planning guarantees this only for the initial path point.
- 5) TODO for extra fun and profit: make an underactuated 2-point model in the path planner.
- 6) timing is obtained from the maximum velocity of the robot and total path length.
- this is a stupid heuristic. the time-scaling of the path should be a decision variable in the OCP.
- 7) TODO for extra fun and profit: make time-scaling of the path a decision variable in the OCP.
- """
-
- ####################################################
- # getting a timed 2D trajectory from a heuristic #
- ####################################################
- # the strategy is somewhat reasonable at least:
- # assume we're moving at 90% max velocity in the base,
- # and use that.
- perc_of_max_v = 0.9
- base_v = perc_of_max_v * max_base_v
-
- # 1) the length of the path divided by 0.9 * max_vel
- # gives us the total time of the trajectory,
- # so we first compute that
- # easiest possible way to get approximate path length
- # (yes it should be from the polynomial approximation but that takes time to write)
- x_i = path2D_untimed[:,0][:-1] # no last element
- y_i = path2D_untimed[:,1][:-1] # no last element
- x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
- y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- x_diff = x_diff.reshape((-1,1))
- y_diff = y_i_plus_1 - y_i
- y_diff = y_diff.reshape((-1,1))
- path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- total_time = path_length / base_v
- # 2) we find the correspondence between s (path parameter) and time
- # TODO: read from where max_s should be 5, but seba checked that it's 5 for some reason
- max_s = 5
- t_to_s = max_s / total_time
- # 3) construct the ocp-timed 2D path for the cart and for the mobile base of the robot
- # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
- # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
- # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
- # TODO make this an argument
- # TODO: we need a function that takes time as input and spits
-# need to have set interpolation to implement that
- # out a path point of distance base_to_handlebar_prefered_distance from time 0.
- # (it's the classic integral for curve length)
- # cart_to_base_prefered_distance as percentage of path length * size of s (again why the fuck isn't s=1????)
- handlebar_to_base_as_s = (args.base_to_handlebar_preferred_distance / path_length) * max_s
- path2D_mobile_base = []
- path2D_cart = []
- # time is i * args.ocp_dt
- for i in range(args.n_knots + 1):
- s_cart = (i * args.ocp_dt) * t_to_s
- # since we're pulling the robot is in front of the cart
- s_mobile_base = s_cart + cart_to_base_as_s
- path2D_cart.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
- path2D_cart = np.array(path2D_cart)
-
-
- ####################################################
- # constructing the SE3 reference from [x,y] path #
- ####################################################
- # 1) we need to add points so that there are args.n_knots of them
- # the -1 is here because we're finite differencing to get theta,
- # so we need one extra
- #path2D = list(path2D)
- ## in case there's too few
- #while len(path2D) - 1 < args.n_knots:
- # path2D.append(path2D[-1])
- ## in case there's too much
- #while len(path2D) - 1> args.n_knots:
- # path2D.pop()
- #path2D = np.array(path2D)
-
-
- #########################
- # path2D into pathSE2 #
- #########################
- # construct theta
- # since it's a pairwise operation it can't be done on the last point
- x_i = path2D_cart[:,0][:-1] # no last element
- y_i = path2D_cart[:,1][:-1] # no last element
- x_i_plus_1 = path2D_cart[:,0][1:] # no first element
- y_i_plus_1 = path2D_cart[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- y_diff = y_i_plus_1 - y_i
- # elementwise arctan2
- thetas = np.arctan2(x_diff, y_diff)
-
-
- #thetas = thetas.reshape((-1, 1))
- #path_SE2 = np.hstack((path2D, thetas))
-
- ######################################
- # construct SE3 from SE2 reference #
- ######################################
- # the plan is parallel to the ground because it's a mobile
- # manipulation task
- # TODO: enforce timing, interpolate the path accordingly
- path = []
- for i in range(len(path2D_cart) - 1):
- rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
- rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
- #rotation = np.eye(3)
- translation = np.array([path2D_cart[i][0], path2D_cart[i][1],
- args.handlebar_height])
- path.append(pin.SE3(rotation, translation))
-
- return path
-
-# function to be put into ProcessManager,
-# spitting out path points
-#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
-# let's try shared memory
def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
"""
starPlanner
------------
- wild dark dynamical system magic done by albin,
- with more dark magic on top to get it to spit
+ function to be put into ProcessManager,
+ spitting out path points.
+ it's wild dark dynamical system magic done by albin,
+ with software dark magic on top to get it to spit
out path points and just the path points.
- goal and path are [x,y]
+ goal and path are [x,y],
+ but there are utility functions to construct SE3 paths out of this
+ elsewhere in the library.
"""
# shm stuff
shm = shared_memory.SharedMemory(name=shm_name)
@@ -764,12 +629,19 @@ def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
# has to be a blocking call
#cmd = cmd_queue.get()
#p = cmd['p']
+ # TODO: make a sendCommand type thing which will
+ # handle locking, pickling or numpy-arraying for you
+ # if for no other reason than not copy-pasting code
lock.acquire()
p[:] = p_shared[:]
lock.release()
if np.linalg.norm(p - goal) < convergence_threshold:
- data_queue.put("done")
+ data_pickled = pickle.dumps("done")
+ lock.acquire()
+ shm_data.buf[:len(data_pickled)] = data_pickled
+ #shm_data.buf[len(data_pickled):] = bytes(0)
+ lock.release()
break
# Update the scene
--
GitLab