From bbec5afa45d24150bef7e8639923a8a007d008f4 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 18 Oct 2024 19:06:26 +0200
Subject: [PATCH] some optimal control added, but it DESPERATELY needs to be
turned into the version with constraints -- this flatly put doesn't work
---
.../basics/__pycache__/basics.cpython-312.pyc | Bin 2141 -> 7093 bytes
python/ur_simple_control/basics/basics.py | 51 ++++++++++++++--
.../optimal_control/optimal_control.py | 57 ++++++++++++++++--
3 files changed, 99 insertions(+), 9 deletions(-)
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 4c97f924d4a9e9eaa656a4c4e56ccfcec555404b..e00e5fe4b51234988fa9300a62849b129253b847 100644
GIT binary patch
literal 7093
zcmX@j%ge>Uz`(F-vQT=G0t3Ti5C?`?p^VQ*7#SF*Go&!2Fy=5sF@kBPC}uFt62$_h
zS)*7}7*d#W*mBvU*cm};nR7UDIiompxuUpoxudwjVvIRFQ9LOusoZHyDXc9lQM@T^
zEeuh7P!aYPmMH!djuwU}0jLOP3rmz>3Req5lu)X03U{^!14GdqCWchb)ljEJiKL2T
zX+t?k6kJ}6i6NCcRctjQL^4V|RXocO!bT=x>KPfB7*Zuu#j{M1Wg%>s8H^06%voj-
zF=R5!9GM4Ur|`gZrSMj<GJteiKxB|fJThDi3{kRRH_5_G0=tnfRV-CJRdzKq#2iM3
zRPiiZ2pgG1_^Xn!l2McIB}mdwlkpaNL1Ix!W@3&e^DW`@)Dqv!yv*Fh9LJ*cfW)HW
z)S_EL&iQ#IMfo{C`S}IDiFt|XsYSPVg7TB{OJF?KA`mZ`1!N)=gB%0GpPw**6AWW5
zOD01tYYk%-H29(Fz!c1BwQMzPDNHHMQVg|hH4IP|Lk&X>TNM`rLoIs=6IeL|149<F
zT~NU+b|?dhs$qu(RSHWs69YpLQwnP>OA1>pM-4{}J1DSg7_&H#G(edknOe>o#w>2A
z01{Qh2{V(Cp@t=sA(J7EIha9{y{ef@K|w(wH@_^^3n|FJg1V>xES8&JT%wSaSe%)x
z5TK9@31fw{{33;{{LH)(h2nz5<Wz+M5L>q-Ul+_!$jvXw%+J$NNK7s%P0WGo%E`|!
z&;uLl=J%3~fq~)qpOqgk`6dO}hrFy{U|`T>yTy{8S`v7RDX-ubTTW(PVov%kmc0C;
z+*?d3CAZj$Q}a>+Q#845aTdm>q!wourKS{tLh}}5<}J>G#Nv|pl*E$6TbxNnsfpQc
zIf?1F_=*$DQsW^~naL%$7z=N)7sjU+73CM*V)VYnRG3l>DosEE0>QuX^fU5vQ}xsH
ziwp8o5{tk*BLp|6G%rQJpeV5-vsk~NC_gJTxujUXv?#tfGq)foH69v1`URCG8Tolw
zB)|b#tPi2|3My|2phrZJ3<CoLF9QQZaR&nfLj%JH9tJ_N2G1L!;thTeIC%THI=Q-e
z8XO;RaCRtl7<RBWINlJDoS`(sZ~||G=M6sb8H$~`4enpK7&y4vD;q1@>l*7Oq+e#2
zTOe|YUG;{DL<i>w4hBx12FDL<3|!nD;hoYGN~c#$thmT2-{ACsg|pqK(Px6t6&8^j
z+#(Z_W++|Ymbt(p^BI(~l0jJo!~kJXz=QaoFMzX72|UBpFu@WY{)_-B8;hB|7@8PM
zKxTo=WGI0r@*1WZMp#)<%Ul9a;5Cdj%ry+4<h&YM+Sjtwuw;P(3~T~|fMr)kh8l(}
zK7=@oRm4*w2xF%(!raWrP{~xuq{&=03!G5Fu@eAhqUH@<uo(H71CkH2N{dSric1oU
zN{SUqGEz};ib8p2Nk)EYi9%vtr9w`AdU|GFIz-nm3tv#~4NyqQ&r7ueX(~%CN>9y8
zPE|-u&d5wHOHI*HC`wIC&PYvBNJ%XQWyVAhq3EZ{d5fi_vLLkx6kxa5^HLIviV`b}
zKoJQ}ctxN<y~SKqns<vQB{iuuJ-(nQGq0rh7Au(6WWB|bSd?B2&UQsY3=9mnxS?qt
zRPNs50cG*{TuAX-3^EIxt%{^UD%l``S){?hz+eDM;0fRa{)LZ$M_>YHxBZ-?`B^iw
zE^%vIV9~h1t#L<4@`{w!3eOFtmxXK_yq^fGE=XMwy+h>y(*dW;!tM<|H&_I&vxr<|
z5xK%5_JNIogXcQC#3go#I~=?h_&lz1cz$4J;E*hmWnf@HDf2;z4dma?n;?w@Xqlf0
zF3s_l`|LFgSs<OTR0E}939Xi+hN%W?nGY$ak&-zlR4YRksQ7_#pmYsq4Py$EBm)CO
z6*mI|EMdavTCN(d5>V`cRo5`4FoRUWWK&o`br2|PLFy2gd<_>!RSi2xwwAkwF@+7x
zhlp~3MR`)#Yk5;RYWdJ)d26^q?yq8CV5s4%;eoMh`D^%77*aT;7-|J-7_#`m&Osq+
z_+hSMWT@e;VN2n%0hz&5!<NDgW<lK!bsJj>4^$M{ZEPvLU{RJ#h8i|-4aVo^RwMvQ
zPfUfyFF~bF$StO_;&h1BAMDd-%>;p+_C=sTzQqnM?BfezjIvwYMX712MWEagUx>^p
z)8r`P0h!ARs)JG>b}(n=l|b?eOLA&v&MoGg)Vv~4v7spl&I7kN;ieS{GcYg|iGT=E
z5FrLKiz6pLJwCG}HMd9{#APhI#aMESy(B&}FD132NEf6|4<y0lU2u!ZJLMK@S$uMC
z$}P6S6cAkuvI(4nkO@d`k_XqnAw`K<ZuvPm`Q@2;=>aZC6?%~!0|SE-D4WSMg6feQ
z0;14b<OaWBgU1a-RRUsicXD_0g6jlMt`66Jk4}$Ho(9J!e1cPyrmIg>pP@J-a|O%H
z{EJe$mxc5$^XWIZgK`}I6y*t-3s@%RUlvkZA$U<p>k_*bxQcneCq9FvGk-?<MLv)O
zpH_oAsA@tN;^b*?x&b0Surct8O-SkVpW%LyM`c0L3ipLI7kP{~B<(QYm~)ZGsloLI
zuh0ae3633+4Q@A}m6Yfe7V!sCiVYqekr!FSZy;E5>L9nSNWUm&*5J_*dyz%@2By#h
zaP_8ffm`VUixQ-iK+fCnQsNf4lt5H(h)S)7v4#n$iUO5(;6#`OYTAHV2m*Wi4O{Mq
z<(U$4%mbxggr#5>Mdo2CohUL7kNZ*>=djeWl)%f88fZmd!ve49S!-CcKsf^JF9ZQA
ze;Gj)J*ZAXNP$^JJSCtE31+9j8z+nml?;^(n#_KhEQowqqz}rGpvtL89@HS^hSZFp
ztOO|qSgTTt@{4b==H{oQ=G<b+E4;;&R|c*$i<CfGl|h6GNGn^BDu@kAM@6O}mKulv
z^}>qOLAer?NFccylAlGe<>w+7Q0)a$z7SkjJ^|H<mOD%jXkHfbY4Cm`A~xM)qQ{KT
z`O!0@=f};AyDXw}2b-9L)O?Pa91BF3OD~jOF1JwbvV_4MEMgxySq0b{JihQSh^WCU
zTaU}azGPRnphOJHR-dOZA$PFAl`Y;*7PMQ%Uc;Qi0;;M~SV3ehM-4Ni>Von?oga|-
z;K)S~up|LBg{y`ER`r#@yJXn1X%;8K3>d418<yl#*ukAM6*A1=LNkLYg`<|GhB<{3
z<SVFOc#yI@mW)@!gJdt3jI@SpHMHZ2tuls%NG(qdPYrhsV-~0ig?SN5gR6F428LRm
z6z*DHB-dccR5d&`yxa^myzl}LWG-t8C%Cd^uVGDL0kgPkSX0<QtQ4MFz7*bC{u(|=
zbE1YHsVN~)!<YrnqEN?xnhFB2P-A4MVZ__?5~yLXVN2ly*;FF{iS-(`6n-!t)Zl=)
z3)Bb!yQYSvh85bJ5U7d=H{}C7T|g~q&=3M~-CJ<`Tp_WbpeVm0GdHm$H3ixwE=$eH
zPtGjKOf7~q*WLUeO%b-j_=2L;vX}q=|NnoBEfvfu0!4JlExydWlGOB~#2iT9RFm-*
zYYM2D{t_e&ZQ95<Kp6}SMOvUX19M7Yu_iaT%~Av!Ap)0BMWCKe5vUZs#R_iQ-(pY8
z$xkdXGXXIYOG;AnAT1k~<eba`SW^ZR!Ql1`wnj`5xX=Q%Uy9s7Wta!Za?Zk(cu4oY
zNCG722@>SVO{|EAd8Nn-B*f&MStJE2z(55zq`X5SAw{1Mq+{TT+<FONU|=W(>1zYG
zUOosQdcolK%Lgt7PCiIyctX&G#0wk}4UQkU82H2|R9xg$SRizfSEa%22Co=0_kn=u
zbnc1VlX+((%_zJepxEH~fs27(0@`Ed!P;Y9qqJUgrRI9Qm3liQFKam;NV%lt1#aSe
z;9?MyzaX!FQPg0A&_z*`2ERL^;?pB0M$E|D;<Vjsqt_Kt`|G0a7e(DKi+VQr-Cz-#
zkiLL*Imbedi((oVSu`5_KCm%}h&A|tw1rO$pAokrXGQD|AF%2VTnrj!4SpAdl`pcW
z+>lpj@S9+9kwxZ)yc(4Cfs28Q7uI;<V&LNGNbi)H!89Z8qJ;KEPVEM#4?GO4T=2g9
z15Unv=}zfx*#@T@qGAmm9U+Z=H^d}n2+dHOkk;VQ5!&c~Ls7rM;{vbjMHaamk_rtT
z9T68<#6V*Jc?&WwaO+)Q(Zkb7;sKZLh(=Nk6QY@fZ4eArQW4w#$3DabE7L&Zxl#<Z
zEG6)oAPY<RSi^{I6aZT}2rG3U=GL&(Fx7y&RVhq!m}^<_+kzNLppH7ST@7<H^Bk61
zwh~a^1YEE}npL3E58kX|uVKel`ocn&5!9>#b!Nc&5kwJB38?Y{3#72X!VKE1Vuh4>
zn#@I@f&o!(XtIFIt|A{;nNb8PO{$;+8lJ?DXyic#G&Ff{ab*@~<`tJD<|RW$HHs8L
zW`mMK5vUM>l#dP|F-H&q>JnjXuyG(Y*udo>sKIuNGczv}H0&H-SOf|KNLdJOz=4LU
zU_}|E9Ak%;V?_}R3=Bs>g%@a8xq;yhx9Eh-B|^)k7D`>=*15o<bAek2Ji4K?B6El6
zWg+_pZ}2GJbgqe9lX+$Y%}Bf;pwQs?g@Zv_cD}|;jRmI5truD^w_9j;S;}aG(FVs0
zQf4=#ly68WFUY(orT>wgMU1V%?~4FTx#4W>J5n-Wok&dVhCoc<WEJ3Q@c1HvX!r45
z2#B~W9EsNO<H9$}a*HEAJ~uHlFFwAAlYxQZC8#x`DSwNzATuvNIXNRU{}yvWX5KB<
zywco)N^oU$izPX~pz;<+Vo`blXcYezXIg1qa!G!EPBFOQeM<^sXjKn3xKspc$U!0!
z)Rex(Qj(dQ3K_!!4KRVbzu@LGBo%<71mwWH#N5>Q_*-mX`-*OHf$4bgm=?Ufev37}
zxFofp2-J>;^xN5r67y2>b8j&fWZq&cj8D%`%(*3*otcy4o0ykbP@0oil3x^-SzMZ!
zlUbEow2}c_R6-I3hfQvNN@-52T~Q7L0|RKhqWA>^1H%VqMn=ZZTnvm7w;6<PGjQK#
z;J?ctd!HfZB16n=2JR0mvW&7Tcs?`8GRl5(W8`P_V*H>Yh@<}-slqw*&wMn15R
zFC(M=3Z2gkAo^1RlQKxqi;;nar=#i;v*b-)h0DB34Q@BM1+R0<T;!Iy%q`d8a+615
zhR9_exdvBIPujcDyT$h=3ulLFzj~*7i{A}i{{G0$$m_ha7kOo8BwprKSYUXOS7kxa
z^3a8$7kRZ-1g#HU8G4b|sKE`SORdHCAq!WB$pq0EY*$$1Z?JG)XHmY$qI{J_^@At_
IixfB{0D~m*fB*mh
delta 589
zcmdmLepf)_G%qg~0|Nttaw%_mBnJb-V-N?1nV^i%2@Df8>?|1>QW#PgbGW0pQ&?IU
zqIgnRTNt8vQ`lM<qWDtSTNtAFD;YI8UV=3FX)@koFGwsZ$xO`AWST6&q_o+QsfUp<
zeeyYGNA_E+MfpkjC6h&1&M``De$3Lw$UT!wK|w(wH@_^^YjOm0@Z=M0HtbdAzF_`6
zajnT_>_U<thu&h(OGzv$N~|nmW?*0_k^&JdlM~t1*aSdK{>i=Un!J(>3=Fqe!G;y7
zPCmk(T<?CB!{Y-p1BYahGy?-eGRTR@7-ZIG76t}}=?s+&D;fP(G8Acmi~}hy0-L6g
zm7kec5>k|y<(8k5lV6^hmmc8h;+&sXQk0+Llb>Huq{+a*;Kabd@R^l?fuVun1`7wm
zt~i{Q6{&;l0b2$p5H^ZpwXsNtfq}sdq(=)xfRt+rOx`Fd!N@iFl%!|HEtZnZ+|(j)
zxTu0ufSd)^gzh5_o80`A(wtPgB2xwi21W)3hGHWI28IvJjEsz*xfmEFZZinoX5hZf
oz<-xP_CACAZ3ga7EV7KUpWGPv89f<4sW38fe&%3g<OF*Y04^|rs{jB1
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 5323bfb..444ea98 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -63,12 +63,55 @@ def moveJP(args, robot, q_desired):
print("MoveJP done: convergence achieved, reached destionation!")
-def jointTrajFollowingPIDControlLoop():
- pass
+def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data):
+ breakFlag = False
+ save_past_dict = {}
+ log_item = {}
+ q = robot.getQ()
+ v = robot.getQd()
+ # NOTE: assuming we haven't missed a timestep,
+ # which is pretty much true
+ t = i * robot.dt
+ # take the future (next) one
+ t_index = int(np.ceil(t / reference['dt']))
+
+ # TODO: check if that's really the last
+ if t_index == len(reference['qs']) - 1:
+ breakFlag = True
+
+ error_q = reference['qs'][t_index] - q
+ error_v = reference['vs'][t_index] - v
+ Kp = 1.0
+ Kd = 0.1
+
+ # feedforward feedback
+ v_cmd = reference['vs'][t_index] + Kp * error_q + Kd * error_v
+ qd_cmd = v_cmd[:6]
+ robot.sendQd(v_cmd)
+
+ log_item['error_q'] = error_q
+ log_item['error_v'] = error_v
+ log_item['reference_q'] = reference['qs'][t_index]
+ log_item['reference_v'] = reference['vs'][t_index]
+ return breakFlag, {}, log_item
-def jointTrajFollowingPID():
- pass
+def jointTrajFollowingPD(args, robot, reference):
+ # we're not using any past data or logging, hence the empty arguments
+ controlLoop = partial(jointTrajFollowingPDControlLoop, robot, reference)
+ log_item = {
+ 'error_q' : np.zeros(robot.model.nq),
+ 'error_v' : np.zeros(robot.model.nv),
+ 'reference_q' : np.zeros(robot.model.nq),
+ 'reference_v' : np.zeros(robot.model.nv)
+ }
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ loop_manager.run()
+ # TODO: remove, this isn't doing anything
+ #time.sleep(0.01)
+ if args.debug_prints:
+ print("MoveJP done: convergence achieved, reached destionation!")
+
def moveJPIControlLoop(q_desired, robot : RobotManager, i, past_data):
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
index 1f5cce5..c8daeb8 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -1,15 +1,34 @@
import numpy as np
import pinocchio as pin
import crocoddyl
-from ur_simple_control.managers import ControlLoopManager, RobotManager
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+import argparse
+from ur_simple_control.basics.basics import jointTrajFollowingPD
+
+def get_OCP_args(parser : argparse.ArgumentParser):
+ parser.add_argument("--ocp-dt", type=float, default=1e-2, \
+ help="time length between state-control-pair decision variables")
+ parser.add_argument("--n-knots", type=int, default=100, \
+ help="number of state-control-pair decision variables")
+ return parser
+
+
+# TODO: use fddp and incorporate torque (i.e. velocity constraints)
+# --> those will correspond to max_qd and acceleration attributes in robotmanager
def IKOCP(args, robot : RobotManager, goal : pin.SE3):
+ # starting state
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
state = crocoddyl.StateMultibody(robot.model)
# command input IS torque
actuation = crocoddyl.ActuationModelFull(state)
# we will be summing 3 different costs
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
+ uResidual = crocoddyl.ResidualModelControl(state, state.nv)
+ xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ xRegCost = crocoddyl.CostModelResidual(state, xResidual)
+ uRegCost = crocoddyl.CostModelResidual(state, uResidual)
# cost 1) distance to goal -> SE(3) error
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
state,
@@ -18,15 +37,17 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
robot.model.getFrameId("tool0"),
goal.copy(),
state.nv)
+ goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
# cost 2) u residual (actuator cost)
- uResidual = crocoddyl.ResidualModelControl(state, nu)
+ uResidual = crocoddyl.ResidualModelControl(state, state.nv)
# cost 3) x residual (overall amount of movement)
- xResidual = crocoddyl.ResidualModelState(state, x0, nu)
- # now we put in the costs to our functions
- # these seem fine
+ xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ # put these costs into the running costs
runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-1)
runningCostModel.addCost("uReg", uRegCost, 1e-1)
+ # and add the terminal cost, which is the distance to the goal
+ # NOTE: shouldn't there be a final velocity = 0 here?
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e3)
# Next, we need to create an action model for running and terminal knots. The
@@ -44,3 +65,29 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
),
0.0,
)
+
+ # now we define the problem
+ problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)
+ # and the solver
+ solver = crocoddyl.SolverFDDP(problem)
+ # run solve
+ solver.solve()
+ # get reference (state trajectory)
+ # we aren't using controls because we only have velocity inputs
+ xs = np.array(solver.xs)
+ qs = xs[:, :robot.model.nq]
+ vs = xs[:, robot.model.nq:]
+ reference = {'qs' : qs, 'vs' : vs, 'dt' : args.ocp_dt}
+ return reference
+
+if __name__ == "__main__":
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ args = parser.parse_args()
+ robot = RobotManager(args)
+ goal = pin.SE3.Random()
+ goal.translation = np.random.random(3) * 0.4
+ print("set goal:", goal)
+ reference = IKOCP(args, robot, goal)
+ jointTrajFollowingPD(args, robot, reference)
+ print("achieved result", robot.getT_w_e())
--
GitLab