From cda718002392d28edd06ec6307ae21e01ce81b16 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 31 Oct 2023 14:49:58 +0100
Subject: [PATCH] wrote todos, put the reference dmp code in place
---
TODOS | 23 ++
clik/.robotiq_gripper.py.swp | Bin 16384 -> 0 bytes
dmp/DMP.m | 128 ++++++
dmp/dmp_node | 140 +++++++
dmp/matlab/DMP.m | 128 ++++++
dmp/matlab/NoTemporalCoupling.m | 19 +
dmp/matlab/TemporalCoupling.m | 109 ++++++
dmp/matlab/TemporalCouplingNoPotentials.m | 100 +++++
dmp/matlab/main.m | 55 +++
dmp/matlab/plot_result.m | 110 ++++++
dmp/matlab/run_simulation.m | 65 ++++
dmp/matlab/trajectories/g.mat | Bin 0 -> 1937 bytes
dmp/matlab/trajectories/omega.mat | Bin 0 -> 10525 bytes
dmp/matlab/trajectories/psi.mat | Bin 0 -> 3893 bytes
dmp/matlab/trajectories/s.mat | Bin 0 -> 3204 bytes
dmp/matlab/trajectories/z.mat | Bin 0 -> 2094 bytes
dmp/motion_planning/__init__.py | 0
dmp/motion_planning/dmp/__init__.py | 2 +
dmp/motion_planning/dmp/__init__.pyc | Bin 0 -> 314 bytes
dmp/motion_planning/dmp/dmp.py | 166 ++++++++
dmp/motion_planning/dmp/dmp.pyc | Bin 0 -> 6785 bytes
dmp/motion_planning/dmp/temporal_coupling.py | 80 ++++
dmp/motion_planning/dmp/temporal_coupling.pyc | Bin 0 -> 3407 bytes
.../online_traj_scaling/__init__.py | 2 +
.../online_traj_scaling/__init__.pyc | Bin 0 -> 315 bytes
.../__pycache__/gmr_oa.cpython-36.pyc | Bin 0 -> 11530 bytes
.../online_traj_scaling/example.yaml | 38 ++
.../online_traj_scaling/gmr_oa.py | 363 ++++++++++++++++++
.../online_traj_scaling/gmr_oa.pyc | Bin 0 -> 14523 bytes
.../trajectory_generator.py | 93 +++++
.../trajectory_generator.pyc | Bin 0 -> 3744 bytes
dmp/msg/DMPState.msg | 8 +
dmp/msg/TrajGenState.msg | 6 +
dmp/todos.md | 2 +-
dmp/traj_scaling_node | 113 ++++++
35 files changed, 1749 insertions(+), 1 deletion(-)
create mode 100644 TODOS
delete mode 100644 clik/.robotiq_gripper.py.swp
create mode 100644 dmp/DMP.m
create mode 100755 dmp/dmp_node
create mode 100644 dmp/matlab/DMP.m
create mode 100644 dmp/matlab/NoTemporalCoupling.m
create mode 100644 dmp/matlab/TemporalCoupling.m
create mode 100644 dmp/matlab/TemporalCouplingNoPotentials.m
create mode 100644 dmp/matlab/main.m
create mode 100644 dmp/matlab/plot_result.m
create mode 100644 dmp/matlab/run_simulation.m
create mode 100644 dmp/matlab/trajectories/g.mat
create mode 100644 dmp/matlab/trajectories/omega.mat
create mode 100644 dmp/matlab/trajectories/psi.mat
create mode 100644 dmp/matlab/trajectories/s.mat
create mode 100644 dmp/matlab/trajectories/z.mat
create mode 100644 dmp/motion_planning/__init__.py
create mode 100644 dmp/motion_planning/dmp/__init__.py
create mode 100644 dmp/motion_planning/dmp/__init__.pyc
create mode 100644 dmp/motion_planning/dmp/dmp.py
create mode 100644 dmp/motion_planning/dmp/dmp.pyc
create mode 100644 dmp/motion_planning/dmp/temporal_coupling.py
create mode 100644 dmp/motion_planning/dmp/temporal_coupling.pyc
create mode 100644 dmp/motion_planning/online_traj_scaling/__init__.py
create mode 100644 dmp/motion_planning/online_traj_scaling/__init__.pyc
create mode 100644 dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc
create mode 100644 dmp/motion_planning/online_traj_scaling/example.yaml
create mode 100644 dmp/motion_planning/online_traj_scaling/gmr_oa.py
create mode 100644 dmp/motion_planning/online_traj_scaling/gmr_oa.pyc
create mode 100755 dmp/motion_planning/online_traj_scaling/trajectory_generator.py
create mode 100644 dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc
create mode 100644 dmp/msg/DMPState.msg
create mode 100644 dmp/msg/TrajGenState.msg
create mode 100755 dmp/traj_scaling_node
diff --git a/TODOS b/TODOS
new file mode 100644
index 0000000..2efde79
--- /dev/null
+++ b/TODOS
@@ -0,0 +1,23 @@
+IMPORTANT
+-------------
+1. urdf
+- understand how it works (read the docs)
+- use the calibration data from the robot and put it in the urdf
+- ensure that gripper urdf makes sense
+- do test to see if the calibration is ok
+- recalibrate if necessary
+
+NICE TO HAVE
+---------------
+1. other inverse kinematics algorithms
+- standard list of them, for fun and to practise eigen and pinocchio a bit
+
+2. (ur) simulation
+- put the gripper on the robot in ursim
+- possibly fiddle to get communication with it in the sim to work
+
+FUTURE PROSPECTS, CURRENT URGENCY ZERO
+--------------------------------------
+3. hooking up with a real simulator, ex. mujoco
+
+
diff --git a/clik/.robotiq_gripper.py.swp b/clik/.robotiq_gripper.py.swp
deleted file mode 100644
index 6f387ee01160141069385d952c68d35d5a67715d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|<Ni<&ZkfvXUW2lYt>UzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jbQrWM=CZ<tOEr
zWERG!7iAU{q!#HFR6@)gB}YSGGz5qZ0bT}UBSQm_YGoxw1!1935OWldhQMeDjE2By
z2#kinXb6mkz-S1JhQMeDjF1p0DPUo!XJBApg8DZcN;9I-|DhlnDjor)Ve0-t<-MTd
zo1rvJo*BwlhthwcG)x}K9i>J?U^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU
z1ZWTfi75;WLJSNHU7(J*00RT8|9^m=fnh&C1H(ps28IRv3=C8F85rvL85jcj85jci
z85pei85oTC85s8QF)(c7V_=xT$G~95$G~uhmw{mcF9U-cF9U--F9X959tMUZJPZu$
zc^DY7co-N|co-P=a5FGeax*Yga5FI2a5FGib2Bi=a5FHh<zir%&Begb!o|Q~%*DW<
zz{S8I&&9yN$i=`gnUjH`fs=tDo|A#Wj+22wi<5ytlaql#kduMo8wUf!Z4L&8bsP)~
z{TvJoc^nK3K^zPWCL9b55*!Q+zt|ZVezG$#EM{k5$YEz-aAs#<&|qg^c*w@UaFvaL
zp_h$;A)bwa!GMi{L7$C*ftQVe;W{e=!){gvhFz=-3`MLA48K_z7*?||Fxap#Fvzno
zFbK0SFnnNUV7SA~z_6N`fuV+(fx(QKfkBv=f#EX~1H%p`28KQ+28J9a28L`V1_muA
z28Jz+kZ|q<g*8T8DnNl%L1Iy2u0mO2QD$ONPO6nc7?h<@lCMylTB64QRj8n(q!gT*
zmr|@yl98&ASWr-uUr>~pSdyxcoS&PUn3tkZmYJwfoS&SXS_0JzR#}#qQ<|!fpQexq
zw?sz)q`W*av!qxdEx$+sM5`wzXR9j|r4|?D=M|?yY)wf`Q;084Er~BnEYc`W%}LXN
z8)2nTTv7z$=9H#dDTL-_=I2Fc=9TCuq~+u%mc(jWF~Hqjlv+|+l&1jFuNR+L9G{q+
zt&x&glBfw+0rDY8M8OuK3gQX9qSWLv4MPJX6I7L8gP=-^Q}a?1b8<A2b5rzE^OEyZ
zQo-)jbM<rfck%Rd*F<P7&n(G+7!M7v_?-OYY%91Ulob+7@^dqjVZm9f05V*^D7g%-
zBsn)lLEBbAJtj|GK|xs|FST4DCo?ZqA+uPaD7COOvnVwM9G;*Ef?5KKiL}hT%;F5V
zDIniMO-fTzsD{OStwMEiNs$IPx-@H*;L1S}1XBYIaD~h~P(;M1WG0vBWtODo7HeoC
zJOVP?RzWG)H3VUZvO;oxUU5lLX>y4|X<lYwDLmD{?NL%fD9=bNOI1ipP0dqCO-oBn
zF3Bu|rU;PRtsoI<r4UjCN?3Uc#ihx~sl~-<r8x>ksmZAYC7Jno-~^YPt)q~Zm{Xjp
zkeQ~Cm#>hRoUKrv2nwv^)XcKf6di@kyp+u3#FEUsbZ}ZNPAyT$O{`SN%P)bu3z0#<
z{<TtY$xH^>kXTd+vIG`1#h{P|>DPdX#OEdErb0tZ6Oy1o&ZjgRfrB2LSBuHXN5$X-
z3UXEv)Ia`3DXB%NDIjk}gEJLIZbHdU$jLuBC%-sV1Chp+l$4x7LXa{dIX^EC6nXi1
z3gE;KN|vw%Rj}9wW$~2EVyIHE+cXt)?G*g-^HP!WFtVk^sU;<uxvBZ3B^r=i7!Tzl
z+<~kdX0%2|esKvTN(%CeN|16Ul4?*104~AwAVx!N&~tQ)_w;iO(Seq4dcpqA-toa9
zL9ULz2wP!(0>v^k)j&*vInYWW1mbXTE=>aEe^APUC{oBTNG(bPrL1B^Vu2b1a<G*`
zKz<QYaSWA$`N~Sc1L{8oP>EWAFas1RkeC3c(L@Dk94I7~fD?UsW?5>ULSjlvQED;N
zYEZ1e!VMHUItmDDz)1w+AaH>TaXrF|-~w2|R>4Tm0B20b=O$Lfr{xzVr-Brkn!+<1
z$TGe7+{`?<tO2@QVnuv$L27CWPMt7WY&r|_i!(t%j8ij`0yO>50u@|pE7&TOWE7<)
zrex-&>-pp-XCvhSuoobKqF@V7$8aA&VggbO<`?O~(hDfG<Kr{)GE3s)Avpjk<vWJN
zySN62DA+0(!_5f}@ec@ab&2=%^K%V~_jmGgbp|Og!cgEJ8iK6A5U#-2Kg`q5UBOlX
z;YlSWCI6(X)Z`L{;*!LYQcx)gisjT4g``SII)Sc*R0zpPRmjXMNli~JQUF&M#R}j8
zyCh#BH?br+L!mqamb4&Rz%^Mh+`q{=iN(bV5X*wWc4@fgmF6PX*^bU3o?)&io>o=}
z^$QJlbpb^ksP+rV$Sj6f532u?ON)w9^Gb5SWw%0UF^01h(lU#3%M*)I;b8!_!!ZOB
z4v3%$at(F`g(gbKxWhaGcCJEEDouk1Y8#fYfwe0XY!%c)L)>&NzzqS%U}sNHaGMG&
zrjV2mZfd0Fg6gF5{Gx0~W5~@1<S%tMpAfLFw8YY!5(Nzd+x%=Dh2qpyh1|rv(!?Ba
z*_2vTlwVW~sWwX#s#1&cA;p}(lNZQpe<v@n5&4h=lag8jE$1~16l@i8^UE^x(sdLJ
z6>JsqOG{FVz$K23f{}u)LS|kboNugP3#yf&h9s7NT7M-Fdjf($k*^*Q<O;S2o-0Ak
zoYdkH1&!3?jC@e_o0C{v0&6j)rYOJ^LQDw`aRj+GIK&Zb3N&4V-3V?n6{nU!T$u=J
zVt|_gItn0Hf%&NrLzESQTpe8$!W@G<9i4n!gTd)1z&{vda)5s@*yMuzVg)O><q8@G
zx<;m^nmP)g(8w=H%>(NR4gmWpH~`|O;(}BKD+Nel1=Rpn=H?#+GTO~Q2y8T{`DF!)
zljKyGxnO0EE?{MjE)ZpjDMboa3W=p9`MO1^IjM=osR}8XMX*=_xk1BF6I3%ms%!&I
zg_P1FQ00TH1>#}H5QxbkP?JkAOx8%vO)W}K%}cISD9*_*2SqqEu-rrZL5_D1@drCT
zy(C}3N+CTTlnyn@Gjnnj3Q~*G@{4kzV(?}GD8TdcKuz8fP~);7zgP$4XB|-ZKwSkc
z;?y0TA&ECJxdh~2NW7&gXc#J#XJqE2Du4_sODqAk?UEDo6p~Uwi3-voNzO@4EK-1^
zdPq`$n5?W29^@I~iilwa4QEF`1xKG?e{j@8RKW^s=ltB<(mYV3F|`<@SpyC8oJt+!
z&Jfs_#U<e2hSnm*Itn?dWvNAp>7Y1(%7GdukPZ>72??!^L3Tq5%^=9SbZ7x##lVnO
zl%K0mQds~pCNmdQDnU9!Itn4B1v#lY3P`Oxh~m_|(p&|oQc$tW0A+(3QE)b_a)Sv#
z>N5rgB_$=_{FKt1R8WjUSIjFw9i?6j&gTWGMa2rb3MHw<C8;Tp5b|*JbJ2AL`2y7c
zmu6>RxXcb&{|}p=|Ig3BaEYIRVLm?t!#rr8pPQe7;TRtS!%;p4h6X+c20mzi|28iJ
zLpCo1!(Sc-h6_9l3~f9N3?4iT417Ec40E^{7}U5K81`{7Fl^#tU?}5aV2I~pV36iw
zV36WsVA##cz|hIbz>v+!z>vksz~IHnz@Wg%z;KL%fnf#*1H*I<28I|81_lET1_luh
zh&wNV+=|hE9u*i3fzc2c4S~@R7!83D9Ri@y4iGMa46~+X<|XFDgBx(rb^)}pqKDGq
zgDA^S%7V16KnB2t6l@ieON-+3ld`}qU4$H{nGGGHjZa4!7S;>!4@Q`y4DFtQJFSVy
zC7?bHtX}}`B!R}Epq&m#I}p-60@(w?P`g1xL8&=udZ;$}J9!~&f;2htc@t`urh=_4
zteJ!@IP@U>ailR+i2sRjgR%l>99^L_uOu@EG;RrMjA!Phqk0L{w$m%lNlh)#FwiqF
zFhutPc+3LTqX9v#nhJ`x3dy-Cpx8t>4aGuGP{N0klC$&j%X3mw(o;d>e~BdunI#I~
zMt@0BC8)g!=?8$cK)Z2B9X3?=7A0mDrz!-2Je8a33hIk$D7htO=A@>8%mIzE!du9o
zZUV^9sd`EXUx9~vOF+{X@%czY?4YRxP-hqFCs^p_=9i^vz)}df%K_>>fQP3LMuJ+e
zpwt1vC7|(*%o5PJLqUFVab^;zJq!{9`3+_ec%(TmAJnT&OwItAn3<-Kn5O_ud7yD<
zNWVEX1=bhK%?FK5Ax7@N_JGU;xlkb^F$L7qEyzg)cW)A5<1|QP*ctid;8YJ*pPHAF
znxdcq>e(Y_Jq2(F0MxBaNi9jt%qiB?Q}D|#L5z#Q$JZSdN<e)GNE<#e4?gBolv+@f
zTAZ2(A2tVf!r}gg6a<(9Fo|iPp@7U1P%6z#%S=sy=WggwCOE{c6x_h$f+hK&Q5=P6
z_*kHh0({Ug7F{Q}7j2~w3?3H*4MXLZrGkbx!Meec5Zz!VhHjY4K_jft;X#-gCHW8+
zz?49Yf$<O~m1Lymfiohc{|1k<l6-iSA)*yN0|4s`f)jN~Vo?bwA%l8yCHduvMbMcx
z$nY%8N*zdSfGj93NKFQZBdC*{ngTA`z(EL}761nkXk-zT7UC20QsP171ZY?ll%2uD
zj5_cU3oB6ELQ_7J4K@kP29M%`dy|kEHsr(<YXusL%gZlJL}Xw{{6nU?V6%6{pji>{
ze1oQrLUK-K0mu<xCxY@*St4XELctcfyQ&clb|t8AN9fVi0j21iG`-*eSEy)lK`LC-
z%|8e#nwDP#7j+Nu*HJLkj78*TWl%_iim7~XenAamL=Z#7!Knrjya=CyJp#%}wh%v+
zCFVe!j2OWN4`U>!f(F<?bqPEPBb)*@1T+|g)ex}V(3n9s1T2nb2&8%hn}_NWMB+iP
zAVogNJuop%gvUUU0*%Jp#0m{i9xO}D(NW0F%mYmjLUKV_Vh++I3Mi?;t%hrW=t``J
K2Pw8<U;qFa2#}Ki
diff --git a/dmp/DMP.m b/dmp/DMP.m
new file mode 100644
index 0000000..176c3fd
--- /dev/null
+++ b/dmp/DMP.m
@@ -0,0 +1,128 @@
+classdef DMP
+ properties
+ % State
+ x % [z; y; s]
+ tau
+ tauDot
+ % Settings
+ K
+ D
+ alpha_s
+ n_kernel
+ n
+ g
+ y0
+ nominal_tau
+ x0
+ mu
+ std_dev
+ w
+ demo_traj
+ end
+
+ methods
+ function obj = DMP(dmp_params,demo_traj)
+ obj.K = dmp_params.K;
+ obj.D = dmp_params.D;
+ obj.alpha_s = dmp_params.alpha_s;
+ obj.n_kernel = dmp_params.n_kernel;
+ obj.demo_traj = demo_traj;
+ obj = obj.fit();
+ end
+
+ function obj = init(obj)
+ obj.x = obj.x0;
+ obj.tau = obj.nominal_tau;
+ obj.tauDot = 0;
+ end
+
+ function obj = step(obj,tauDot,dt)
+ xDot = [obj.h();obj.z();-obj.s()]/obj.tau;
+ obj.x = obj.x + xDot*dt;
+ obj.tau = obj.tau + tauDot*dt;
+ obj.tauDot = tauDot;
+ end
+
+ function ref_pos_val = ref_pos(obj)
+ ref_pos_val = obj.y();
+ end
+
+ function ref_pos_val = ref_vel(obj)
+ ref_pos_val = obj.z()/obj.tau;
+ end
+
+ function ref_pos_val = ref_acc(obj)
+ ref_pos_val = (obj.h() - obj.tauDot*obj.z()) / obj.tau^2;
+ end
+
+ function yVal = y(obj)
+ yVal = obj.x(obj.n+1:2*obj.n);
+ end
+
+ function zVal = z(obj)
+ zVal = obj.x(1:obj.n);
+ end
+
+ function sVal = s(obj)
+ sVal = obj.x(end);
+ end
+
+ function hVal = h(obj)
+ psi = exp(-1./(2*obj.std_dev.^2).*(obj.s()-obj.mu).^2);
+ hVal = obj.K*(obj.g-obj.y()) - obj.D*obj.z() + obj.w*psi' ./ max(sum(psi),1e-8) .* (obj.g-obj.y0)*obj.s();
+ end
+
+ function traj = rollout(obj,dt)
+ dmp = obj.init();
+ traj.t = 0;
+ traj.pos = dmp.y();
+ traj.vel = dmp.z()/dmp.nominal_tau;
+ traj.acc = dmp.h()/dmp.nominal_tau^2;
+ traj.s = dmp.s();
+ while norm(abs(dmp.y()-dmp.g)) > 1e-2
+ dmp = dmp.step(0,dt);
+ traj.t = [traj.t traj.t(end)+dt];
+ traj.pos = [traj.pos dmp.ref_pos()];
+ traj.vel = [traj.vel dmp.ref_vel()];
+ traj.acc = [traj.acc dmp.ref_acc()];
+ traj.s = [traj.s dmp.s()];
+ end
+ end
+
+ function obj = fit(obj)
+
+ psi = @(s,c,d) exp(-1./(2*d.^2).*(s-c).^2); % Basis function
+ obj.n = size(obj.demo_traj.pos,1);
+ obj.y0 = obj.demo_traj.pos(:,1);
+ obj.g = obj.demo_traj.pos(:,end);
+ obj.x0 = [zeros(obj.n,1); obj.y0; 1];
+ obj.nominal_tau = obj.demo_traj.t(end);
+
+ lastKernel = 1*obj.nominal_tau; % Time of last kernel center
+ ct = (0:lastKernel/(obj.n_kernel-1):lastKernel); % Time means of basis functions
+ obj.mu = exp(-obj.alpha_s/obj.nominal_tau*ct); % Phase means of basis functions
+ obj.std_dev = abs((diff(obj.mu)));
+ obj.std_dev = [obj.std_dev obj.std_dev(end)]; % Standard deviation of basis functions
+
+ svec = exp(-obj.alpha_s/obj.nominal_tau*obj.demo_traj.t);
+ obj.w = zeros(obj.n,obj.n_kernel);
+ for varNr = 1:obj.n
+ if abs(obj.g(varNr)-obj.y0(varNr)) < 0.001
+ continue;
+ end
+ fGain = svec*(obj.g(varNr)-obj.y0(varNr));
+ f_target = obj.nominal_tau^2*obj.demo_traj.acc(varNr,:) - obj.K*(obj.g(varNr)-obj.demo_traj.pos(varNr,:)) + obj.D*obj.nominal_tau*obj.demo_traj.vel(varNr,:);
+ for i = 1:obj.n_kernel
+ Gamma_i = diag(psi(svec,obj.mu(i),obj.std_dev(i)));
+ if fGain*Gamma_i*fGain' < 1e-8
+ obj.w(varNr,i) = 0;
+ else
+ obj.w(varNr,i) = fGain*Gamma_i*f_target'/(fGain*Gamma_i*fGain');
+ end
+ end
+ end
+ end
+
+ end
+end
+
diff --git a/dmp/dmp_node b/dmp/dmp_node
new file mode 100755
index 0000000..9a72514
--- /dev/null
+++ b/dmp/dmp_node
@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+
+from motion_planning.dmp import DMP, TCVelAccConstrained, NoTC
+import numpy as np
+import rospy
+from robot_ctrl.msg import CtrlTarget
+from std_msgs.msg import Float64, String
+from motion_planning.msg import DMPState
+
+
+class DMPNode:
+ def __init__(self):
+ self.dmp_active = False
+ self.rate = None
+ self.dt = None
+ self.dmp = DMP()
+ self.tc = None
+
+ # Load target trajectory and fit DMP
+ self.load_trajectory()
+
+ # Load parameters from server
+ self.load_params()
+
+ # Setup ros topics
+ self.target_pub = rospy.Publisher("robot_ctrl/command", CtrlTarget, queue_size=1)
+ self.dmp_state = rospy.Publisher("motion_planning/dmp_state", DMPState, queue_size=1)
+ rospy.Subscriber("motion_planning/command", String, self.command_parser)
+
+ # Go into spin
+ self.spin()
+
+ def load_trajectory(self):
+ if rospy.has_param('motion_planning/trajectory_loadpath'):
+ # Set demo trajectory
+ trajectory_loadpath = rospy.get_param('motion_planning/trajectory_loadpath')
+ data = np.genfromtxt(trajectory_loadpath, delimiter=',')
+ t_demo = data[:, 0]
+ path_demo = np.array(data[:, 1:]).T
+ self.dmp.fit(path_demo, t_demo)
+ rospy.set_param("motion_planning/init_pos", np.squeeze(self.dmp.y0).tolist())
+ else:
+ rospy.logerr("Could not load trajectory. Parameter " + rospy.get_namespace() +
+ "motion_planning/trajectory_loadpath not found")
+
+ def load_params(self):
+ # Load parameters from server
+ update_rate = rospy.get_param('motion_planning/update_rate', 125)
+ tc_type = rospy.get_param('motion_planning/tc/type', 0)
+ if rospy.has_param('motion_planning/tau0'):
+ self.dmp.tau0 = rospy.get_param('motion_planning/tau0')
+
+ # Set temporal coupling
+ if tc_type == 0:
+ self.tc = NoTC()
+ elif tc_type == 1:
+ gamma_nominal = rospy.get_param('motion_planning/tc/gamma_nominal', 1)
+ gamma_a = rospy.get_param('motion_planning/tc/gamma_a', 0.5)
+ v_max = np.array(rospy.get_param('motion_planning/tc/v_max', np.inf))
+ a_max = np.array(rospy.get_param('motion_planning/tc/a_max', np.inf))
+ eps = np.array(rospy.get_param('motion_planning/tc/eps', 0.001))
+ self.tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
+ # Set rate for ros spin
+ self.rate = rospy.Rate(update_rate)
+ self.dt = 1.0 / update_rate
+
+ def command_parser(self, msg):
+ if msg.data == 'start':
+ self.dmp_active = True
+ elif msg.data == 'stop':
+ self.dmp_active = False
+ elif msg.data == 'reset':
+ self.dmp.reset()
+ self.dmp_active = False
+ rospy.loginfo("Reset motion_planner.")
+ elif msg.data == 'step':
+ self.dmp.step(self.dt)
+ elif msg.data == 'load_params':
+ self.load_params()
+ rospy.loginfo("Reloaded motion_planner parameters.")
+ elif msg.data == 'load_trajectory':
+ self.load_trajectory()
+ rospy.loginfo("Reloaded motion_planner trajectory.")
+ else:
+ rospy.loginfo("Invalid command string for motion planner.")
+
+ # spin -----------------------------------------------------
+ # -----------------------------------------------------------
+ def spin(self):
+ print_flag = True
+ while (not rospy.is_shutdown()):
+
+ if self.dmp_active:
+ # Step DMP
+ self.dmp.step(self.dt)
+ # Step temporal coupling
+ tau = self.dmp.tau + self.tc.update(self.dmp, self.dt) * self.dt
+ self.dmp.set_tau(tau)
+ # Send target trajectory state command
+ pub_msg = CtrlTarget()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.dmp.acc
+ pub_msg.velocity = self.dmp.vel
+ pub_msg.position = self.dmp.pos
+ self.target_pub.publish(pub_msg)
+ else:
+ print_flag = True
+
+ # Publish DMP state command
+ pub_msg = DMPState()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.dmp.acc
+ pub_msg.velocity = self.dmp.vel
+ pub_msg.position = self.dmp.pos
+ pub_msg.theta = self.dmp.theta
+ pub_msg.s = self.dmp.s()
+ pub_msg.tau = self.dmp.tau
+ self.dmp_state.publish(pub_msg)
+
+ # Notify when DMP trajectory is finished
+ if print_flag and self.dmp.theta > 1:
+ rospy.loginfo("DMP trajectory finished.")
+ print_flag = False
+
+ # Sleep
+ self.rate.sleep()
+
+
+# --------------------------------------------------------------------
+# Here is the main entry point
+if __name__ == '__main__':
+ try:
+ # Init the node:
+ rospy.init_node('DMPNode')
+
+ # Initializing and continue running the main class:
+ DMPNode()
+
+ except rospy.ROSInterruptException:
+ pass
diff --git a/dmp/matlab/DMP.m b/dmp/matlab/DMP.m
new file mode 100644
index 0000000..176c3fd
--- /dev/null
+++ b/dmp/matlab/DMP.m
@@ -0,0 +1,128 @@
+classdef DMP
+ properties
+ % State
+ x % [z; y; s]
+ tau
+ tauDot
+ % Settings
+ K
+ D
+ alpha_s
+ n_kernel
+ n
+ g
+ y0
+ nominal_tau
+ x0
+ mu
+ std_dev
+ w
+ demo_traj
+ end
+
+ methods
+ function obj = DMP(dmp_params,demo_traj)
+ obj.K = dmp_params.K;
+ obj.D = dmp_params.D;
+ obj.alpha_s = dmp_params.alpha_s;
+ obj.n_kernel = dmp_params.n_kernel;
+ obj.demo_traj = demo_traj;
+ obj = obj.fit();
+ end
+
+ function obj = init(obj)
+ obj.x = obj.x0;
+ obj.tau = obj.nominal_tau;
+ obj.tauDot = 0;
+ end
+
+ function obj = step(obj,tauDot,dt)
+ xDot = [obj.h();obj.z();-obj.s()]/obj.tau;
+ obj.x = obj.x + xDot*dt;
+ obj.tau = obj.tau + tauDot*dt;
+ obj.tauDot = tauDot;
+ end
+
+ function ref_pos_val = ref_pos(obj)
+ ref_pos_val = obj.y();
+ end
+
+ function ref_pos_val = ref_vel(obj)
+ ref_pos_val = obj.z()/obj.tau;
+ end
+
+ function ref_pos_val = ref_acc(obj)
+ ref_pos_val = (obj.h() - obj.tauDot*obj.z()) / obj.tau^2;
+ end
+
+ function yVal = y(obj)
+ yVal = obj.x(obj.n+1:2*obj.n);
+ end
+
+ function zVal = z(obj)
+ zVal = obj.x(1:obj.n);
+ end
+
+ function sVal = s(obj)
+ sVal = obj.x(end);
+ end
+
+ function hVal = h(obj)
+ psi = exp(-1./(2*obj.std_dev.^2).*(obj.s()-obj.mu).^2);
+ hVal = obj.K*(obj.g-obj.y()) - obj.D*obj.z() + obj.w*psi' ./ max(sum(psi),1e-8) .* (obj.g-obj.y0)*obj.s();
+ end
+
+ function traj = rollout(obj,dt)
+ dmp = obj.init();
+ traj.t = 0;
+ traj.pos = dmp.y();
+ traj.vel = dmp.z()/dmp.nominal_tau;
+ traj.acc = dmp.h()/dmp.nominal_tau^2;
+ traj.s = dmp.s();
+ while norm(abs(dmp.y()-dmp.g)) > 1e-2
+ dmp = dmp.step(0,dt);
+ traj.t = [traj.t traj.t(end)+dt];
+ traj.pos = [traj.pos dmp.ref_pos()];
+ traj.vel = [traj.vel dmp.ref_vel()];
+ traj.acc = [traj.acc dmp.ref_acc()];
+ traj.s = [traj.s dmp.s()];
+ end
+ end
+
+ function obj = fit(obj)
+
+ psi = @(s,c,d) exp(-1./(2*d.^2).*(s-c).^2); % Basis function
+ obj.n = size(obj.demo_traj.pos,1);
+ obj.y0 = obj.demo_traj.pos(:,1);
+ obj.g = obj.demo_traj.pos(:,end);
+ obj.x0 = [zeros(obj.n,1); obj.y0; 1];
+ obj.nominal_tau = obj.demo_traj.t(end);
+
+ lastKernel = 1*obj.nominal_tau; % Time of last kernel center
+ ct = (0:lastKernel/(obj.n_kernel-1):lastKernel); % Time means of basis functions
+ obj.mu = exp(-obj.alpha_s/obj.nominal_tau*ct); % Phase means of basis functions
+ obj.std_dev = abs((diff(obj.mu)));
+ obj.std_dev = [obj.std_dev obj.std_dev(end)]; % Standard deviation of basis functions
+
+ svec = exp(-obj.alpha_s/obj.nominal_tau*obj.demo_traj.t);
+ obj.w = zeros(obj.n,obj.n_kernel);
+ for varNr = 1:obj.n
+ if abs(obj.g(varNr)-obj.y0(varNr)) < 0.001
+ continue;
+ end
+ fGain = svec*(obj.g(varNr)-obj.y0(varNr));
+ f_target = obj.nominal_tau^2*obj.demo_traj.acc(varNr,:) - obj.K*(obj.g(varNr)-obj.demo_traj.pos(varNr,:)) + obj.D*obj.nominal_tau*obj.demo_traj.vel(varNr,:);
+ for i = 1:obj.n_kernel
+ Gamma_i = diag(psi(svec,obj.mu(i),obj.std_dev(i)));
+ if fGain*Gamma_i*fGain' < 1e-8
+ obj.w(varNr,i) = 0;
+ else
+ obj.w(varNr,i) = fGain*Gamma_i*f_target'/(fGain*Gamma_i*fGain');
+ end
+ end
+ end
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/NoTemporalCoupling.m b/dmp/matlab/NoTemporalCoupling.m
new file mode 100644
index 0000000..bec7d37
--- /dev/null
+++ b/dmp/matlab/NoTemporalCoupling.m
@@ -0,0 +1,19 @@
+classdef NoTemporalCoupling
+ properties
+ v_max
+ a_max
+ end
+
+ methods
+
+ function obj = NoTemporalCoupling(params)
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ tau_dot_val = 0;
+ end
+ end
+end
+
diff --git a/dmp/matlab/TemporalCoupling.m b/dmp/matlab/TemporalCoupling.m
new file mode 100644
index 0000000..68caf44
--- /dev/null
+++ b/dmp/matlab/TemporalCoupling.m
@@ -0,0 +1,109 @@
+classdef TemporalCoupling
+ properties
+ % Settings
+ v_max
+ a_max
+ nominal_tau
+ gamma_nominal
+ gamma_a
+ eps
+ end
+
+ methods
+ function obj = TemporalCoupling(params)
+ obj.nominal_tau = params.nominal_tau;
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ obj.gamma_nominal = params.gamma_nominal;
+ obj.gamma_a = params.gamma_a;
+ obj.eps = params.eps;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ % DMP prediction step
+ dmp_next = dmp.step(0,dt);
+
+ % Formulate matrices
+ [A,B,C,D] = obj.tc_mtrcs(dmp);
+ [A_next,~,C_next,~] = obj.tc_mtrcs(dmp_next);
+
+ % Compute bounds
+ tau_dot_min_a = obj.tau_dot_min_a(A,B,C,dmp.tau);
+ tau_dot_max_a = obj.tau_dot_max_a(A,B,C,dmp.tau);
+ tau_dot_min_v = obj.tau_dot_min_v(A_next,D,dmp.tau,dt);
+ tau_dot_min_f = obj.tau_dot_min_f(A_next,B,C_next,dmp.tau,dt);
+ tau_dot_min_nominal = obj.tau_dot_min_nominal(dmp.tau,dt);
+
+ % Base update law
+ tau_dot_val = obj.gamma_nominal*(obj.nominal_tau-dmp.tau) + dmp.tau*obj.sigma(dmp);
+
+ % Saturate
+ tau_dot_min = max([tau_dot_min_a,tau_dot_min_v,tau_dot_min_f,tau_dot_min_nominal]);
+ if tau_dot_val > tau_dot_max_a
+ tau_dot_val = tau_dot_max_a;
+ end
+ if tau_dot_val < tau_dot_min
+ tau_dot_val = tau_dot_min;
+ end
+ end
+
+ function [A,B,C,D] = tc_mtrcs(obj,dmp)
+ A = [-dmp.z();
+ dmp.z()];
+ B = [-obj.a_max;
+ -obj.a_max];
+ C = [dmp.h();
+ -dmp.h()];
+ D = [-obj.v_max;
+ -obj.v_max];
+ end
+
+ function minVal = tau_dot_min_a(obj,A,B,C,tau)
+ i = A < 0;
+ minVal = max(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function maxVal = tau_dot_max_a(obj,A,B,C,tau)
+ i = A > 0;
+ maxVal = min(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function minVal = tau_dot_min_f(obj,A,B,C,tau,dt)
+ Ais = A(A<0);
+ Ajs = A(A>0);
+ Bis = B(A<0);
+ Bjs = B(A>0);
+ Cis = C(A<0);
+ Cjs = C(A>0);
+ tauNextMin = -inf;
+ for i=1:length(Ais)
+ for j = 1:length(Ajs)
+ num = Cis(i)*abs(Ajs(j))+Cjs(j)*abs(Ais(i));
+ den = abs(Bis(i)*Ajs(j))+abs(Bjs(j)*Ais(i));
+ if num > 0
+ if sqrt(num/den) > tauNextMin
+ tauNextMin = sqrt(num/den);
+ end
+ end
+ end
+ end
+ minVal = (tauNextMin-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_v(obj,A,D,tau,dt)
+ tauNextMax = max(-A./D);
+ minVal = (tauNextMax-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_nominal(obj,tau,dt)
+ minVal = (obj.nominal_tau-tau)/dt;
+ end
+
+ function sigmaVal = sigma(obj,dmp)
+ a_normalized = dmp.h()./(dmp.tau^2*obj.a_max);
+ sigmaVal = obj.gamma_a*sum(a_normalized.^2./max(1-a_normalized.^2,obj.gamma_a*obj.eps));
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/TemporalCouplingNoPotentials.m b/dmp/matlab/TemporalCouplingNoPotentials.m
new file mode 100644
index 0000000..de4002e
--- /dev/null
+++ b/dmp/matlab/TemporalCouplingNoPotentials.m
@@ -0,0 +1,100 @@
+classdef TemporalCouplingNoPotentials
+ properties
+ % Settings
+ v_max
+ a_max
+ nominal_tau
+ gamma_nominal
+ end
+
+ methods
+ function obj = TemporalCouplingNoPotentials(params)
+ obj.nominal_tau = params.nominal_tau;
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ obj.gamma_nominal = params.gamma_nominal;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ % DMP prediction step
+ dmp_next = dmp.step(0,dt);
+
+ % Formulate matrices
+ [A,B,C,D] = obj.tc_mtrcs(dmp);
+ [A_next,~,C_next,~] = obj.tc_mtrcs(dmp_next);
+
+ % Compute bounds
+ tau_dot_min_a = obj.tau_dot_min_a(A,B,C,dmp.tau);
+ tau_dot_max_a = obj.tau_dot_max_a(A,B,C,dmp.tau);
+ tau_dot_min_v = obj.tau_dot_min_v(A_next,D,dmp.tau,dt);
+ tau_dot_min_f = obj.tau_dot_min_f(A_next,B,C_next,dmp.tau,dt);
+ tau_dot_min_nominal = obj.tau_dot_min_nominal(dmp.tau,dt);
+
+ % Base update law
+ tau_dot_val = obj.gamma_nominal*(obj.nominal_tau-dmp.tau);
+
+ % Saturate
+ tau_dot_min = max([tau_dot_min_a,tau_dot_min_v,tau_dot_min_f,tau_dot_min_nominal]);
+ if tau_dot_val > tau_dot_max_a
+ tau_dot_val = tau_dot_max_a;
+ end
+ if tau_dot_val < tau_dot_min
+ tau_dot_val = tau_dot_min;
+ end
+ end
+
+ function [A,B,C,D] = tc_mtrcs(obj,dmp)
+ A = [-dmp.z();
+ dmp.z()];
+ B = [-obj.a_max;
+ -obj.a_max];
+ C = [dmp.h();
+ -dmp.h()];
+ D = [-obj.v_max;
+ -obj.v_max];
+ end
+
+ function minVal = tau_dot_min_a(obj,A,B,C,tau)
+ i = A < 0;
+ minVal = max(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function maxVal = tau_dot_max_a(obj,A,B,C,tau)
+ i = A > 0;
+ maxVal = min(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function minVal = tau_dot_min_f(obj,A,B,C,tau,dt)
+ Ais = A(A<0);
+ Ajs = A(A>0);
+ Bis = B(A<0);
+ Bjs = B(A>0);
+ Cis = C(A<0);
+ Cjs = C(A>0);
+ tauNextMin = -inf;
+ for i=1:length(Ais)
+ for j = 1:length(Ajs)
+ num = Cis(i)*abs(Ajs(j))+Cjs(j)*abs(Ais(i));
+ den = abs(Bis(i)*Ajs(j))+abs(Bjs(j)*Ais(i));
+ if num > 0
+ if sqrt(num/den) > tauNextMin
+ tauNextMin = sqrt(num/den);
+ end
+ end
+ end
+ end
+ minVal = (tauNextMin-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_v(obj,A,D,tau,dt)
+ tauNextMax = max(-A./D);
+ minVal = (tauNextMax-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_nominal(obj,tau,dt)
+ minVal = (obj.nominal_tau-tau)/dt;
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/main.m b/dmp/matlab/main.m
new file mode 100644
index 0000000..56e6400
--- /dev/null
+++ b/dmp/matlab/main.m
@@ -0,0 +1,55 @@
+clearvars
+clc
+close all
+
+% Create demonstration trajectory
+load('trajectories/g')
+% load('trajectories/omega')
+% load('trajectories/z')
+% load('trajectories/s')
+% load('trajectories/psi')
+
+% Nominal trajectory functions
+dmp_params.D = 20;
+dmp_params.K = dmp_params.D^2/4;
+dmp_params.n_kernel = 100;
+dmp_params.alpha_s = 1;
+dmp = DMP(dmp_params,demo_traj);
+
+% Simulation setup
+sim_params.dt = 1/1000;
+sim_params.T_max = 100;
+sim_params.kv = 10;
+sim_params.kp = sim_params.kv^2/4;
+
+nominalTraj = dmp.rollout(sim_params.dt);
+sim_params.a_max = 0.5*[max(abs(nominalTraj.acc(1,:))); max(abs(nominalTraj.acc(2,:)))];
+sim_params.v_max = inf*[1 1]';
+sim_params.s_v_max_online = 0.65;
+sim_params.v_max_online = [inf;0.15];
+% sim_params.v_max_online = inf*[1 1]';
+
+% Scaling parameters
+tc_params.nominal_tau = dmp.nominal_tau;
+tc_params.eps = 1e-3;
+tc_params.gamma_nominal = 1;
+tc_params.gamma_a = 0.5;
+tc_params.a_max = sim_params.a_max;
+tc_params.v_max = sim_params.v_max;
+
+% Create temporal coupling objects
+tc = {};
+tc{1} = TemporalCoupling(tc_params);
+tc{2} = NoTemporalCoupling(tc_params);
+tc{3} = TemporalCouplingNoPotentials(tc_params);
+
+% Simulate
+nSim = length(tc);
+res = cell(length(tc),1);
+for d = 1:nSim
+ disp(['Running simulation ' num2str(d) '/' num2str(length(tc)) '...']);
+ res{d} = run_simulation(dmp,tc{d},sim_params);
+end
+
+% Plot result
+plot_result
diff --git a/dmp/matlab/plot_result.m b/dmp/matlab/plot_result.m
new file mode 100644
index 0000000..50571b3
--- /dev/null
+++ b/dmp/matlab/plot_result.m
@@ -0,0 +1,110 @@
+close all
+
+
+figure('Name','Velocity')
+
+subplot(2,1,1)
+plot(res{1}.s,res{1}.ref_vel(1,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_vel(1,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_vel(1,:),'m-.','LineWidth',1.5)
+plot([0 sim_params.s_v_max_online],[sim_params.v_max_online(1) sim_params.v_max_online(1)],'r:','LineWidth',1)
+plot([0 sim_params.s_v_max_online],-[sim_params.v_max_online(1) sim_params.v_max_online(1)],'r:','LineWidth',1)
+yticks([])
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)]) max([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)])])
+ylabel('$\dot{y}_1$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+set(gca,'FontSize',16)
+
+subplot(2,1,2)
+plot(res{1}.s,res{1}.ref_vel(2,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_vel(2,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_vel(2,:),'m-.','LineWidth',1.5)
+plot([0 sim_params.s_v_max_online],[sim_params.v_max_online(2) sim_params.v_max_online(2)],'r:','LineWidth',1)
+plot([0 sim_params.s_v_max_online],-[sim_params.v_max_online(2) sim_params.v_max_online(2)],'r:','LineWidth',1)
+yticks([-sim_params.v_max_online(2) 0 sim_params.v_max_online(2)])
+set(gca, 'YTickLabel', {'-v_{max}','0','v_{max}'},'FontSize',16);
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)]) max([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)])])
+ylabel('$\dot{y}_2$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+figure('Name','Acceleration')
+subplot(2,1,1)
+plot(res{1}.s,res{1}.ref_acc(1,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_acc(1,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_acc(1,:),'m-.','LineWidth',1.5)
+plot([res{3}.s(1) res{3}.s(end)],[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{3}.s(1) res{3}.s(end)],-[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],-[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([-sim_params.a_max(1) 0 sim_params.a_max(1)])
+set(gca, 'YTickLabel', {'-a_{max}','0','a_{max}'},'FontSize',16);
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)]) max([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)])])
+ylabel('$\ddot{y}_1$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+subplot(2,1,2)
+plot(res{1}.s,res{1}.ref_acc(2,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_acc(2,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_acc(2,:),'m-.','LineWidth',1.5)
+plot([res{3}.s(1) res{3}.s(end)],[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{3}.s(1) res{3}.s(end)],-[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],-[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([-sim_params.a_max(2) 0 sim_params.a_max(2)])
+set(gca, 'YTickLabel', {'-a_{max}','0','a_{max}'},'FontSize',16);
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)]) max([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)])])
+ylabel('$\ddot{y}_2$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+% Path
+figure('Name','Path')
+clf
+tmp = 200;
+plot(res{1}.ref_pos(1,1:tmp:end),res{1}.ref_pos(2,1:tmp:end),'g*','LineWidth',8), hold on
+plot(res{1}.sys_pos(1,:),res{1}.sys_pos(2,:),'b-','LineWidth',2.5)
+plot(res{2}.sys_pos(1,:),res{2}.sys_pos(2,:),'k--','LineWidth',2.5)
+plot(res{3}.sys_pos(1,:),res{3}.sys_pos(2,:),'m-.','LineWidth',2.5)
+xlabel('$\xi_{1}$', 'Interpreter','latex','Fontsize',30)
+ylabel('$\xi_{2}$', 'Interpreter','latex','Fontsize',30)
+xticks([])
+yticks([])
+
+% Tau
+figure('Name','Tau')
+plot(res{1}.s,res{1}.tau,'b','LineWidth',2.5), hold on
+plot(res{2}.s,res{2}.tau,'k--','LineWidth',2.5)
+plot(res{3}.s,res{3}.tau,'m-.','LineWidth',2.5)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([])
+set(gca,'FontSize',16)
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+ylim([0.9*dmp.nominal_tau 1.05*max(res{1}.tau)])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylabel('$\tau$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+
+% s
+figure('Name','s')
+plot(res{1}.t,res{1}.s,'b','LineWidth',2.5), hold on
+plot(res{2}.t,res{2}.s,'k--','LineWidth',2.5)
+plot(res{3}.t,res{3}.s,'m-.','LineWidth',2.5)
+yticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+set(gca,'FontSize',20)
+xlim([0 1.05*max([res{1}.t(end) res{2}.t(end) res{3}.t(end)])])
+ylim([0.9*res{1}.s(end) 1.05])
+xlabel('$t$', 'Interpreter','latex','Fontsize',40)
+ylabel('$s$', 'Interpreter','latex','Fontsize',40)
+
diff --git a/dmp/matlab/run_simulation.m b/dmp/matlab/run_simulation.m
new file mode 100644
index 0000000..1ce873a
--- /dev/null
+++ b/dmp/matlab/run_simulation.m
@@ -0,0 +1,65 @@
+function res = run_simulation(dmp,tmp_couple,params)
+
+% Parameters
+dt = params.dt;
+T_max = params.T_max;
+kp = params.kp;
+kv = params.kv;
+a_max = params.a_max;
+v_max = params.v_max;
+s_v_max_online = params.s_v_max_online;
+v_max_online = params.v_max_online;
+
+% Initialize
+dmp = dmp.init();
+t = 0;
+k = 1;
+sys.pos = dmp.ref_pos();
+sys.vel = 0*sys.pos;
+sys.acc = 0*sys.pos;
+
+while t < T_max
+ % Store values
+ res.t(k) = t;
+ res.s(k) = dmp.s();
+ res.ref_pos(:,k) = dmp.ref_pos();
+ res.ref_vel(:,k) = dmp.ref_vel();
+ res.ref_acc(:,k) = dmp.ref_acc();
+ res.sys_pos(:,k) = sys.pos;
+ res.sys_vel(:,k) = sys.vel;
+ res.sys_acc(:,k) = sys.acc;
+ res.tau(k) = dmp.tau;
+
+ % Final position convergence condition
+ if norm(dmp.ref_pos()-dmp.g) < 1e-2
+ break;
+ end
+
+ % Perform one time step
+ k = k + 1;
+ t = t + dt;
+
+ % Add velocity limit online
+ if dmp.s() < s_v_max_online
+ v_max = v_max_online;
+ tmp_couple.v_max= v_max_online;
+ end
+
+ % Step trajectory generator
+ tau_dot = tmp_couple.tau_dot(dmp,dt);
+ dmp = dmp.step(tau_dot,dt);
+
+ % Controller
+ sys_acc_ctrl = dmp.ref_acc() + kv*(dmp.ref_vel()-sys.vel) + kp*(dmp.ref_pos()-sys.pos);
+ % Saturate acceleration
+ sys_acc_ctrl = min(max(sys_acc_ctrl,-a_max),a_max);
+ % Velocity integration
+ vel_prev = sys.vel;
+ sys.vel = sys.vel + sys_acc_ctrl*dt;
+ % Saturate velocity
+ sys.vel = min(max(sys.vel,-v_max),v_max);
+ sys.acc = (sys.vel-vel_prev)/dt;
+ % Position integration
+ sys.pos = sys.pos + sys.vel*dt;
+
+end
\ No newline at end of file
diff --git a/dmp/matlab/trajectories/g.mat b/dmp/matlab/trajectories/g.mat
new file mode 100644
index 0000000000000000000000000000000000000000..e03f71a1b5d2900da911de5751cec75588386db4
GIT binary patch
literal 1937
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NS<NN=+<DO;O0t
zvr-7jC{=JtO;#{8S1>fPGB&U>HdQb(Ffvde5-`93qo*%F0|NsmI|D<-oU>usAt9k6
za#JoiR`M-aloqj%$uQ&y`;PU`nl>%fjO?7rp|S4(`;rJR)g-$MC*)?iEtu)HFCaLP
z?I91Bmc+3KJ;pX{ZHWT1H7&Q8lr|NgxBMQEeKSh;wfpS%>muc==kMLW_xs-RV}WXu
zIr)rc%Km%Pw_eXE%il2X^K$RL^{m{<;Vt`x#Wzkle)Ez2tMX<$#%AREpb{me`}C
zq~-Oj<EQWJbAQVEPs$&#OI_pE=d!`KKCkX@=y|3E^Q9tek5zt~afa*IYlWf;zp5Y?
z=HLHyZ!mSot|@<SP;V^J_RYq=<Po#jcW#rPx)I@X?;Lt``@zK@#f$BCUOIQKw4r7}
zZTIvTwF{rk4y8ArkBfeO*>yqXF0Q|OmHl7DZ57Sfx4gw!S*E<FW4mz1y}U!*i$lG)
zy?dp1;ZKpHshQP|xJ>;ER<<pgj_b~|Nmg@hnb#a(rTPD&-pt7kudTT4&i~TSzx(pc
ztKA&h>`TwsRvc)3Ss;3jXXR|Es$)}M26F7@<CggTVo}q}BH;t+$0saiGhcQ-*5ljS
za{kS`jOWTZedK<e^S17DLRB?q-{PS6^NU`bE%`1M_c`C7mH)-zFW&{r*nNJiTs~j@
z_2J+Za@&plEqCg_UcUZ;=#2`MsxbR&;r5PqzVBOCzyG~n|IP1SGW++u%E`GJX{_UI
zs^e=K@$i08?{mW*zc}4p$<{lg=CuVLuiRX1(Z*R-6PxhAv{AI?)7g7J)0Dq-cy8<W
z+}8BzbGX5(1)8h6MVW&(MQu;rDwPoN-%?`x0>vxh&iqBE@4cJ0*F5nJ8|$&s8<T=}
zST5fnculQiS@VRJ-h-Ud`3}ENGD=uwNUYnIu%RZZqv%9u$qB>5?{*|gSZQ#a3t>;6
z_2Aj=m%Z<oy;hhwLohGCO8&8x4~J<Fw`oYj|2fPORT8VM&E3l1SzUjA-=w<aa*pmA
z{VL(bLXUiR7B0I}B;m&ret5;Eoc)_}csnlM-jwxV-}L;=)7P`bDe}&?$XEVn)iFO!
z+WhP`8MD6gM+^>UPWi&@wTn%%chP|=iOHsC?Xzd`X$4vHtd#w_!7OB+fYx>A`nM`P
z+*_`;ZN1henQ;8;Q3<h(>&v!e25U8(xSQ5kS||DbXKIL(<<0r5LhRYsc&~*VT)0z}
z{rR+r1<B9K_n-Z<PRdU3vb2tX%ICudMfVbCwkNhk6xm1o*b}GT_V-<;gj>O;cg0)Y
zt!_W^Sc~W7o|(nF<c<6L#jkJN-S_;o3iFF6mwmTM%)R5hXYMTDIr1vMd?$qOkv+_z
z5<7>pNvS_U;-SROMBfX7F^!2VMb)da6*q2Ub$+#6?9-mzPpivUS0>Fn|6{V<^8Z^^
zkNNzJFg4`#+;b%T$b@S4{=$6^=apZ(^5pcK^KtWcd^_{3Xp2(evFW+l`y9-k*e-Y8
zo~Bp*Z1$JM8h@T#XmFl*J>Yv@;pFZ5=iHC~{r)FJW~t+C{r!86y}ZFXW$(X*IS=|(
zUzGpNzJI^=wUU+I?$<FJU3<Nx?yu9^s_*vF+tqe?KxEDbr}L+up4h&1dFFHX`-}d5
zKeM`7Nqgr1V3Qe7);k`YT~g=0?p@Q<<?>T*gkD{c_<K?5-e<Cm%L4-aO=k&Bd-B*g
zjx{bgzsAI930G<A{q*~NHNm0!F9O$<<@axC7k{97w0kq#)$9qpE5kX2%g*dfk|>cp
zE1`F8vB;9TXYEt8x%;yY<i|ZY!CC94V8%b;*<|-$n#{NQh4M~+cDgOH^g)8cU#8jW
zcf6uMRBzkHGV76O>eQn@1<WiTo%>mR^h4{a)VD{iBL)1Pvu?ScpTKr8{J<Z{1qxrU
zZqS@wbh6&u`OSZAi&L+H=N)?FUobbP{!-<({cjzfS5EspVW0e*dv|ts6sM+j#Qyl>
z{iiO#H|O4@iW$EJueMv53;dUq+kf4(cJYIcw$CrwslMCT^X2rp_3giPAKCeR{v|a1
z%jVi*`?KQ5&)dH&PV}#nf5Q4Ne%@u4aPb`VpL2e#72JRR%Zv4a_Qi1?e|leZ9O8{u
z%$lLPufEh{r@?hgtL;3Qt9oyEJv{u+KY6vIdhe9?oy-1twn#5L**b4~pG@moxu825
z{bxkia$Vv|s+!xM`Q+z?qoIHG*6!+k@q1s#^Zj1u;+y-!MJm_nSe<#DyP&G!)m3Nf
zv;33(t5(S^so5gWy~U#I^9}x2OS8VGzG6F3CRQnHGPy=gY<;u2$6haquTSN2yiO$k
zR9X=?#rbQqukO?3=X8tCMyIgtojk+8dDhH*ih3&clfKyd|8|cmit86i`el3c#gC$k
z>zzW**O}CcK8=>|ye4<Tf@Mcwef*{M)8>YM`s*?0+iKT1(~$Wg@k<wdU0NIeL8{~I
znV)%!wl^GIy`)B=Fw-vR&z}8n+>idcTJT?GTmSLr_B^X^{#7m1^n1j9;!pR_f9~oR
ZYQ5*2?fzW<E9SyZGuD5MDw_kp0|2w3u`>Vw
literal 0
HcmV?d00001
diff --git a/dmp/matlab/trajectories/omega.mat b/dmp/matlab/trajectories/omega.mat
new file mode 100644
index 0000000000000000000000000000000000000000..8cd3b2b1561142fbc2ee4b9bd768e1773333c6bd
GIT binary patch
literal 10525
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NS<NN=+<DO;O0t
zvr-7jC{=JtO;#{8S1>fOGPbZXF;y@!Ffvde5-`93qo*%F0|UcU4F-maIdi60T7+J_
z{heRZxPM|=pq8eK(Vgg%AyOWmUWYt2KF??gSi;lUF=?TOsN}2-XM){jKEIJE_IM`r
z>D_1R=d$a*SIw)RSG%tE?dO}>Z@0et{jcu&)^E>L1vu)z32@~6n96KFlk-dS)Pp|%
z&!6m^A9n4!?3viPx;g*-?Zy7M=RZI0AO5GLq(E^;^lf1s9bL{nCTYL-Ki>RT{BgMP
zI+kNHH~znCk^k3t{_MNjPUc^x%sN3feSGq#6F;OY)Jp_Z>cpJc5zl7azr#=a{>qds
z*BcsUH~Ahn>Abj}@yPu>uCF3f-2SVbs95#$?L!aa$2-FUII}a~-oA3KVjpu>?Ygh;
zt#2QdST60ee*K>>^%qV{^fP^azv`o$$gv$E0rjFsbza;)FHz6XlySI&-{_fS%dUvb
zxYK(SkJTJj5Hf82FP(VCnoIoSy`92~dw$j*@V37--_T9wvBAqqr>@f9EXwmZEgq<}
zo&Pgg@Vd&6OS@u!9PP;GVk_8HTihl1Tx7%PRd&XgW~Uvo=G~pTEuehO?8GCs%<3O6
zPG?V-_;|kQ^fkkz!|jo7a>W9>H`*y*&F`CdUZ&>g;;%M_=TBtIX($yIu}O<b{M2RW
zUa<c2wT-N9^Y$mQuf2Z!eoxK+0~@#Qs?TcrXYlDCGhfaY264%s`c5Xsxefb!tp2q6
z{}=1pTK_loN;2oJ|7B|@YQ!X6NtF!CJiG98&o7<kCpl3bGf#3Y_S0VQTYHL#_TT=%
z_4Nt%?+&EffA0_69@-&cncOjTozW}z16Iq0zg&MMWKbo`d4px`Z)+yE_3Dhj-HV$!
zgx%I_3m@y>9`@TH@yjox#M>E9r4tUlC`~&i@^OXGmzjoDduJNHeVFW2zImo$)!mtf
z!mE{B)i;_-?ERN*5FHh<{dz))?6m~D>6yIQ2D{{RZXAw_Iv8oNOHU_4=xv!Ex7+o0
z*54OAwFR`@u6K%Rn=e#7*LCr0($QNPdGe`8U+mhLXrJ+Ne#+q&zcwa%_dXSy`7=i%
z*4?a!eZTbPiL#4dpX|$B<STum?_z1{@o$njE4Ij{AAj+yD53qM#yN?igp#?I8;q-b
z=6smRy!iFmM#0rFWhZkU9(l2=IBA}HChNl^FMbs#Ell0gU}?CE|5{S#mM!Pj9DPw2
z(Y<&1+Gp#t4R`5ZOHwttbFB5^--E5DwRhXB8@Lz$ZsC?b6xuuE(V-W0508o2ZPKos
z|M=L8eGiXW6l~IZbnL~yhsUm6KBacF`C{#n=C>2g`|Tx;HDCODq&f4#ve{1$zS#HZ
zsNegmuO6B`J^JF`qoX>;pFf_cFx+*%!f<=6iSmcUlKmeO%P(g-eM~I5|1q)ff23QD
z#NGNoHzH@rRR5H;dvg4YvUs8E56QYG$8)4IWPeKTdvaVRb9z=?oBdD2_5XFa>)QNT
zH#D%D9Of|iqt$SCxofWGfe&jL#C#{)x-~qEXVeLtuQrF_@NuRarv+vyF!9)PCq-__
z{KjciquWutZ0cpN2@mrf*M!UtlW=T1&$=<+aZ3xUWS#htS()E{sh{|;SKwOn>MhZ6
z9S`djb6yk_$4=TRkSxoQIC+c7qZWzRjvT*zC-<rxi4b>^sJ$+5EaSJI*!TMV?+yNo
z|L&8&7a!PIHf5pd|DE29y7}A9w_2<&J#VPL_FH4r&b|ZMM~zJ4C&k!wZ(eb1=Y$o^
zsT!x$9^AJ$SKixXDl_HOg$-`y?-b7UPD?tsd+O;6obyF@$IZ!q(WSTivCdELa$ogx
z-v0ORe>l8zzfr-|V|86$efxe?JpJqzu3lKTJ9pO4H}{OT%(AMlSC%^;a=ymB-9=I<
zZT=nEzT)s5=Wm?88T6%B<n6ynolMqdGuaK7PG7XO?3Lo>Y>fkYJIwREmWj-L8uD`D
zqXh><{Xa6M{uV0nP5yp_OP_VeC7rY91-mCcnz(k|oJS%WU!LB1vwJ&-ZAg#i*GbC`
zxIg{OmbE8duvMw1CaRN(ZJx?OZGL;DdObt=Y+J59arXW<ZqIJc+H&c4>RF8s<u4zl
z{{I`9G5@Ek+T+(E8QO-Ca)sv{>Q|qW&fnw7cqwLW=6W|SD_e<I73LF|;**Wf=RWpv
zcz@yEk`@6jmDwKF&#u}#-MN0&eC~e##&fZpPrc^9Q_kdEottH$+?>Dq_Of>iN-pa%
z|6C{%FnLz#e)9+2|1$DKG7nhZxOdd)%6;j|*|kZ*9E%S)YRxa5+94h#A<<ti;K%6g
z?mAidY1|9*!YiG>W-u`=<+rH*TO-g|QR40(#M-u=oogoFPsNu%u9Z)X6q#V6Z9hv(
zvZc&w=aW4v7d3?c2wwF3;O~r|N8T@e?ksXcZS(#6k1|z0$<475-ZYKt(f6kIPfSSy
z{>L@mKVj55^7re;>y{H5{3d*znz}BiaZljK{>F3MC70Odc6%;qn&R+ni+h}w)vf;r
zbl1-1`gs0Nv2V|1!H1<qAHSFW{ZM_r_};wInH`qTzs_jRwEt0DxW-quzW*7|6*lAf
z-<khEx&7?Hyqqgi(#xxIcgTy~t*iAvdEW8C^d-rP)(l_Es%~H3^>pD|_dA@^W7r-X
zauiH0+t=Sw@N#y;<>QPNLOS=PUjF8ky?)`x&bmT|X-{0Q&7NN4)*8KL`Bpx@L&Cei
zCfxYPT05KFnRC121-{zM$e5p^OQQ=t`}WGW?Dksv>E1-v7gzfhFaBfC=D6Qc<#60y
zfrYo1zYW{Q!}v6F*1vP_TQkbF=bv($tXSc~YbJMH%jw=}5vR+C6+Z~tY&fZ~lB0B=
zt%_KXA<MeQ#j}^}>b~%-xBGPSX=i~auXL-+cX3#1a?OvkG2q!W)2+@e*VW^4d6CQA
zN(nK!C+1u4YC7GH;bos%-1hL3{^9SNC%1N#a(}P-w*T;g3HBCibVE9}`7b+t`en}n
z>#mHX+vnG>c>PBD_?sVsFM@gBZQA{nbz=YP*Qd_+9yl-P_j%eS|BQe$`~OUTs9|Hs
zty-&h(tTbjd(NcKZi)>$6MtRM7w^?unWXr--r?=Sv~Nb=_+Py0O?+s+z20GI|I4{^
zm%hKCRqESw<N$xc*58xvZ8K;7nG(LGOIDKkE_di|?RCo);@Xv-)psUuh^Tnc{`lYW
zj5dGQ{Oe~VbneRXDdqFJPyg2wCTMc(VQbjMU5@tdPXu4fAAD)fx9Xx{rnZ-zLZwNy
z!<WYv5^mF%+x!vcbo#ztP5N8EbK<)FZ~DVI4}7gV@xtITOH1^7bMa1}#@UM2N3B}b
z6z+!n{FoGHRrCDc1c7aJOeH05xp86pTBXi^cVWHAxxs4p!}D80&0M6fYhT~&-@sL`
zU74uB-lOc!_u8+QIdqcs$Bt0D7catA%H~V7`>TdroO01WVW0b#CmX*MO89&#epUF3
zYrg+!!}iL)!#PaT&wcVz<f-FdW3kSXFI7F+tTRird1JrhM~=@HPwdN9eB;(>wBOA<
z-&eG+vDdBj;??y_6(rwm`n}?`#EfGy`wB7^@NB86**bGuzoW>_N|U06GVeS?=G*)I
zWBi!IdTjP`?FVmO-F?_=XYX(!>G0Xssy~12dv+)6zQTVaZuPG|^-Wx#_MZE@TD|ws
zu}h*!3u;V{h#i|1S0LHi9R7Xkq4ul`&o({G7L{gwZXMYF|HBoBZx)%63kz7@&G^mf
zBDb<pSoxl4xL*H(D{;rCNH5QBc(lf5>3JI_)y``^-=}6JoZrwM>G{dvM-i(_#`%!u
zWlwf|j6It;A-YqHkAF>@bF8Y{_3HKql`8eOzMs^x_@6n^`VjYjz8iYePt`8qGBW;N
z)yB>1xHBVl$&IC69n0U{l>J^NAauTx^-jsU2HQ5BGhS8A1%E#o=ii(CR6)7s(!1{`
z7_R7kTo7BADv)9;yR@jD;nefGA1durn$A4CYu_r$zF=v4cG!bT&WaTpe6i9m+Jz5p
z-=(s;+wp{dXo|)<`Atn7j;VK!Ce+<>t@;>azvcK*yAx8{O`ngM=LMR5W1e4k`C`mO
zC68|fmn+TvS|8u%*3wTAYYM9JY^~%J&D$WR_gqYb>1Joe%72og7Dwx<Wh~-Y-rh~>
ztSetpkaef_{bFMdrn^l3)^`^)&2MvicvAal!Ip1t{`7f=yhxt-(YksE%Z<ptMv?x(
z3om}Z?C01U<nhQ>=HD|7(Qnr7b@Q+4Gv8M|{aR~Ph(Uff@3c)C?T<ZQ_WQqC?sO|N
zoBi`_(F4&R3by96i-tWF+gETb<j~q#x<PYhubiM`b7k}ASW%DPCVPIXdbQqo*<sZF
zC@Uap@6UI_@}hY={raYaUvFTYeLL9RPwT{L>s3FeUTZ4%d|S&bruD}l{&}tGW9b_;
z6NFRM0vglZRDVr3=lsXFDm*4;*?~*j<Ydd9T4lU?AO9%&$oKB2`)}q<v5L6%NAJ3c
zf8(Ey0}XQP-(Pq<<J*^OJp1IA+26Q-Dv&?wy2q*0lSL{jtBMv!y4cPskh5Q5>BlH)
zd+<%^9A1OXH$7uZTbSJ59os5hDz)H2x`My_zqu=4$4z|l^ghGOwsWga?fvm@+gX)r
z#r=%CzTY$xwEpndAok*kgdL2Z!lm{dF%MSAe{Lr(A7{j}{ImSr)yo}HCV%|AwJ2r9
zgZ+0)UQ4sS`hD#R=c`E%zdo9o&i>)=Tb6CdDmQCNe!FXY_I{)r<LVWT2UD|+cv9{x
zXHn#8`6QR%y-mION%+*IE10-mlxWEZ7AYJ))&BYR`}&0rqR-nWyp&E!xOhM2?f;of
zr|fp0I{x9`oBhUJ`j2&1TwY^W*vpeItk<>n!JUOC%I?hcXj`uy?6G@)P{Z!6=T?8=
zlHA2^_T%cK$M54enJ1d5JIgG88y(`p`2FyHk*)d@9B-!lyD~qCYmHEF=;Lz}AHG=s
zLn+%PPF?q+(lX-|h0Qq=V%IHjczb>MwsOCT3lqY2Sni(1^orLyX1m0MM2~+<x9ZQ^
zzkA{Fdw#Y?Pg0hO%&1%5Q26ej_RU)<AC7Unl$gmRbc%KQvc3rqYxgI{)d{oC<2-t&
zVufeh%$M(PYej23cpG$Ho*`wz#qDc8W-VsO-{s-6(n`|odXfE$tmOxK_Rr{8Cnb4j
z)9&171+{$r5;KZcPI`FY-E_t!eq38bc(-kqQ)=G(Rjzcm-k-m#=1tlV-^!zJb*uDt
zL0#p88Ez*Wjc%xPJnJY6c6hNqmD~E9wa~irZ;#%5;PY~QNvhEv=Xct-3-c}-@N7F6
zna3&{cX0mvq`Nld66x1x?OC<n;yg?F-;g7ghN`*CMbGUz`{>>(i}dv~JtB`hz9@6o
zF0IA*^I`AXd5_m@eR82h_<8lDGasb;?TfYM7)w8Qb?p=mv)G<@uGXk`b5YT(RMUCu
z69Y;%YRSnzlq!qkdVW9JbHCh!J?DOUPUerBe{iXUNzdm)GiLfZYqQ2}?f-x2(C6M&
zD`Rdp8DFe9^GWpF@pGabmXE%^eb;^SNb~jO-=0sNp>*eb=04j8ZN@j$B|krybNI@!
zEA7S?J+kh)#?0R-U-h%9qW0&pb!>aGmo)sjGWAo;oatXh%MBjCDKvcfG%bd|xk%Su
zIOhI!CgH`37V`UxYwdP7G97v*@-lKx@U|mgK0I2ZwtUAorgiC8Co0&p*ZkFaZ@DbI
z>2uUqhK*@=^dpTAX9h{7e3+D%5p=lnT5{qay>l)`+1YNg^X51Gzrt}YnbAE|r68^H
zm)awZchff>e*QzrQ=}s|_l(z}o4)FQxx~-?zx;mM|Nj9J9?|vIKJ053^`-wx;d9xg
z!<!uKzj#Nsc)ZI;u0=b(*>mZ#wRK$S-I6CO?sWXOep~k&n_iKaJLj(C3c9FluU@>Z
zoi`xnPF3I9s2Y!lp(_?-KM-7!u%+?!6aBC)Rm*#SPv68dp>MIdo<F-uOx%WB`#)}H
zTJ)lS_Rrg8PUVe%_BbxHU$bgm0^?h&2NP>oJ^Jazb42rQ@9odGm@dh$@=aN{?C^)5
zY3Fa+m}{8p7VeSzBDiB|cctFs=EQ#f*$;bn7)kJjZTM>?#%sgORhhi6^g#al_@xKG
zG2Pi;uwr&xM%$(ZT!(%?E&1?1W%c$a?^;%_+LDn~C$>jdmCrurKKp6M=QCULo;le^
zs;oZV#uc-sb@`{X&x)ak&-}dgvu;N2?wDse-vn(URi&*<e<@Bc{?NI)P)_6B?TXg-
z2BJaR)>W4#R-65hnRn*<%Xh6U$*G!?%GN({E3M%*FIG?S)3aB7b+mEilb4CS+tWUD
z+h<Q*{g%I2*@^x5{A-O(ua>M}yq&m3D>ZOs{X!lOL$_mD@@oTJnpPa&#r?D5?~x_n
z=dc?1)kN%Dt;pN)?;*c#QvRVs(=?}<eKimWpDn)sqaD+)%KO5$+wZspy2gCAymwwP
z|4`n4sjJIZv~5YPo_RQX$Ej<ltG+$Sp7Ky_{ggGcxMlYIw|kv`!uc;R)1;MgtO<W>
z-d}M%tF%Pz>=#A*l_F_5wYp1xpS}=t^Qi9fXIcxBgDZ{;zRsHRVs}_@?M^Y7Emu9K
z`+9VI^(v_Ic~s7^`OkxMLc#q^atY1pYElcHRQ>rZS7)><)uTeWZlmC#L$B7od7j~7
zTzA)1Q`7gr>n}f31M-xuJS482`hSBby-B=h?KLyS=lh#jHt*KmaPHC6=eKvdT?s$<
z#rjV?_m9<w{Pj<}J-MSSy32Kes7=xB-23y?7q52VKe+UgScUlYu&9gC{p-ZakJsG1
z%Jkht_Dstn506qSgPjMrNnZPZrXpt3I<~)6Ro?sA=kNE6S>*MnmUY(SM_fMg*)6)K
zw_lrfSKC$IHkMcQi_}5;Tm4UjJ0@tf_e@M<-6wZ_^1h#Sf18Z5EYG(`O?r6w^v;dm
zbw5|!+H95OFp1~-)i>8`%l<c%J-vU!>Yxu>`fS!pwc9>d3|C)2yQz(5`H%ROZZ^Jd
z>sE8lUp(bRLEoa;g%3S7gdQgv^E#y%+}iH><IP46fvO2JvqNrhJi4~NFY8=-L*()e
z=faMr{jqkxvNrsc@|)6sRz-7{iaxnr|9`J`2vgrLajW?CTO40s_;~H}kLtDYnnHH}
zb~D!qEqib8rFc;Hy$bL5XL=WG4h43IoG>`GIIsTJTK2U5CH}X4%^PzgdggnE>G$-=
zWasR<$aOxrsVscvcSipMlOC>AfBRsufcT!1JNakSt?88CbB9y%d6`o8$G97%+iq8`
zSit>*`EI@F*6$3<)&-U7uaw^B+s-9tcZqjm{?(sq*S;K3xSjHEo$cHOUP{$@^Io)R
zt>xc(^J4A=zb>xH1+Uw<8rO9hf1kB5+i_FWu6^7suiwPy-*;*2_`EvbwY<!+z3yXH
zVZE-!Cl$@N{qI_qd<u!Q`)X8hKK9(b1NS)(AG&1udA9u_zvHo+zb9EMI4@Yf-`bY*
z+fm(^+0)-MC<@J<@AWA;B~i|JQ|2mJCSC0|(YDQkw;$!(eZ5vz;o+<PNk-FUlkKHP
zr~kfv@V_bPbMXS6wcj7E<b3doVZH1^-hPd%8a4eBY$LZa$A!$e8GmM-^F`-NT!m_~
z2kIuznW*UP_VrHq6n(k34CVe!wM-j1uZlz*d)>e7aHe<m|B&TkQ`Ussve~y<c2;W2
z>Q7T{UpOhSe75rcD9202+sqW!%9aYnZ1IoDWsG~+V0FD;R%1os{IB-w)eJQEKY#G#
za_6~%BU)_IkABy^d(7+<6YH_>fm%}1U%uHp+@374-F1QOa=Y-X*Vzk7FO<D0Wxn@s
z+51n^*BM2WRVlvKx!N0&s=AMZMf5=I?rF#W=ank&eZBPil(_VRlg|9ITC-c)_0(67
zkW3e*hp&<x_NMV$EfFtW`6XEBSMM{`)kW18c;A1JdFWi}@!)`}AMe#Tl}lG{exA9V
z<?Y1hXTD^<a4ixpnl!s)^@TmbpD$Z&wtJ$@mY%voWy||xX8hg#wJ!t}Ke>j)F>ktO
zw?9^IJAdxe%e$tQuM!k|B=jVD%C8H(NBkM5uJX-Top<sf&(^pphq7GGe#~|XV@ua)
zkJ=qLM<lVzbs48-)$SQKg0G!6^`AZ#6(T(`Bzeh&xQTpURO}wLve^CGwPnKfq`PZ9
zo;jy}UA(sR#Uw@X8F%yCYK3iX?{Qi!@Za^N5dS~7EtdO2zTG;Uv_zrC(8+bD+}ym+
zcO$DTxlbOk^R}<pB*y#Z-PD~4G8|{x)qXvy(Vj5l{ld!6lYc&#bF1#bryxTm>9!wV
z+NU;uJGS5D_uu${EncTz^sxxN*$~nYayBO9?;7W-Ze2+;mfB5O#^;=-UF3gNqcnTL
zzt7h$RBrY8VrQVVtzd_wSoX}h?$eAWTtAwB{^#VV1pjv9_Mfv$p3Hcynr(2E$#?U|
z{*t@#ia)<UoH?gk_rd2I<-2Mvc{%mm)NX}tb2xmDeJ5+sN)6xH=S!Otxdr5=-@JKl
zkHaC6wewFUo}Hk%^17_$SDw8m1;3uzagF7??&fzJzaLYqo>cek5s%~&`Hgp0?f)Th
za&~9Yp3B(`@7iJyyPLZz3QhhgF4$|eL4I#U$YX)VDXF*GRJLR<d|5Pk$21p~SAP;t
zdP%Kjnz~=InLkPC!rhzMUhaIMhvr!vereq|Q6uW{gp$P$tb20ewnod#wK5q;Kic`^
zlw*z78UHp_sm=|ldxb7*Gv~3MlInYNT%+pe&I_z7gCzbbPOf)S6I;2%`0U>LK%Pen
z_bg>kPoDVVo2GB>ai@xxb)MT+sf#U575?JCriQ7Nc@MAR?W924W6N$Xn8-b~yRGXb
zm!Zad-Qu(Ja^F8JmdK3VbTHXu`iez=F8@?LBlh-0otA8`+4>jjRDP&CEju~?jo?KM
zlY>czMdgJURW?7X_IfU-@TIr)yv<?mBYW-Z)V0`@SX-sd?56iEnzM86^Lr{gm7F3B
zy{GnQE9E7}1#n+=GCrLu`*`bifs4n!^c?1tj<^!C<%89;{zpkCPVtu8Dol1()_oAe
zpjB7?XyP|D`yXq5bbU{)V&D>1KL4chKI7t)e|NWDWYlo~>vDchg~5s?CNf4BxgECu
zx1Ah(tB^m2sma2&>aU>TwzM33u12-q_dDHn{|Kt=kI$LQ-|F*kqRdG?`<9^fb}wJ;
z;P+i9^)LME-wPjNUgu1$mhi9_o6J$Vqt_u%etU)Xzk3J1oY^+NLU`fNT?NykzdKkt
zxNo<=&b;?c&svF{vK(AT&hPaTTlXUMgkt~s$#o7d3V1hdE^=DU-e^?yTWyWrCr<fI
z`};(ut!#IVzN66h%O>Z~OkO3Gsjcy<mQ6dtv!3mlD5^BKTu@=(Rz|01YMlMIrwcAz
z+a5WyMAE}3GWK<<ub`*3(!QsQ1qDp*o@dXoYVP27-S_>?C5E5(FV~d*`|&%qx67g6
ztlyFXcj?K=*PRMCEn8pr<dxE-vvJoii%$)_^Y@?qth982Oy*e|OKx>_+<2e<Qc7GP
zbJ3ztQ|0UhzV^$MaqRC2yT0_)xyZMytg{#1a?<m5c@!?VbP~VQo%fIKt3|Jw*tDeO
zN%wDwBX=cE9K6-((RluFYDfUL$I{Mwdaukp9y)o7bcVBgJU^-uc`n|0HJi}QE9_MQ
z>G7YP!w!laT^lX1XIcf1h}!;gq1gptS37pDOJ`7-e8b884ByH19lqaH<}`$E{q)?d
zt}LVUK}`0m?#T{St`jmUliEENi0AHmFLK0ie({OQjP^%zy6<kkY$<khJ0YudZl}<C
z>tw%&_YF7Qal8F?LQ|D%SgzK+Jgdz3uV=+3_UJ4X7ykHd`J`9YdbgDx-96$h<hVTH
z>X#$0wVSUC1kTv}t5x#eJJpryg%72aHcr_n?)g|IzTkuJQr=1D1*fZBpVzYE)fV&J
zsW}24gVH{kAD#SRKLZ29|Np6zDosSYq>lPZI<-#9JLG!Gr_Jf>+$ohOdQ-eyL;_QG
zwCNeCBwJ0~$)o73lHB%Q;d4QXkCKx}kxcQF>(TH3eg77<f9v&o|KI6`R~H<zJ|Sm4
z$KS5I_fmVVQ1ZE%{AD}#s%g2{uVLq8E!y?^QQ3?Dy)9D@tY(*$6<nUScYR`e-P5u^
zuCX2yE>3J0i@xLP>>PZ=&H3<;+cPu&J^z2Tuip4>epH?F@BH(hr0dOQ{lDziy5ZWQ
zFs@_Yqb{B8iAeHVn0Ec#90QZjQm4M!F6h3yXOY%)-Zz1J`$`nnywT_i^!Gpd{kG$;
zudC1cTDdnEKDYeMo2ajG!+uHi?(Zx$v0t{Y594fIaOq~0q$}@?UHSrsn_f#6%v6`Y
zEfRT<;r2|g<sXW6M15`geA0fOfVps_++qi|?dRjmvgJFO^{p01uNOLg@ciNJYSXtf
z6s<nZv*(tk!I~*g9v^jRI$^y^OqFl;1T&eM6^qsh*KNARI_1|z;a|~~@5R%2ojkY3
znP+PmKRl8vF>#*-+nd0u_42-(g`S<99jF;A{NTe&?Va};dKDH;+3};OTIfdWJG0+i
z8w4t3lE2u8)w4;hJ13YIZ`HK-Z1m(yJH89dnX7Le<ipx$#<w)*tdhZ=>lSLp0bQb+
zN)zpm{JwDOw{!T+mqyn#Wd2V2oG4@)KTFfZ^WwyFKO{bQ8k|*{yH39QKw?@@{HDx$
zujl4A!qLm$&OhT7WgcQ<8gA6N{*d$iMh5AVF|LcxWhOD$hDv^Y{WsDhWbKbY#fw5y
z1Z^g1Wgl|5B$lTA@Z^eCHolGWkMBj_xZrp7v!r^n*Uj$%yun)EJf_8;H>u>WUn^&9
zeEVNl%xz(P*|&DhEwR2Y#GkL%khK1}+4iOMnKWrNeL?XL?jLr%X?m?BvETUqfA`x$
z=j3i*oc3}*&qnh;*LMd)?_AtjaHpt%)g$e@@A*9uhrjdY9Q>uIePQusk>EGGy7sm`
zd;a_8^u|L0a~DnB+Hy#3*1N{tD;L<TnDX$Lzhmd&oS36?L=;S(__|5e9{u}eX-?hF
zVub~dr}~G@I^a{ZeZzyR>kk;6&-}RSEwkE*APKg+%N=exZhLX%wb_~TUGkHJ<e0ZA
zh;3)QWyA4i?&gaeZy6cGBK4~tF_^^exb3>%Ui#9qq*d$p&S+_R_q?mTBzi)M{x<!%
z5<V7DKVzHI(k_n=Fr7+e=W@~BaN2{XC*hgmea7Uyt{pdOPliAJtr6Fsbm!$pr^ZVW
zpJac2>yKDd9?<&v64Mqj%{l6_QchECIdyMpZr)(4yy5@UczLn?7w#wDG36Cx=kxp!
z^Y+li>CxLB<^*q;QQU93d@bYc?;hLY9?xTG`ox*b`(G#|#(<ssq-Mgl{U0VRi{rYK
z6qTWsn|{FKp{BE>Fz223HL6w*Y-X^BOR{HwZ?3VL+Ievv7w?q~&rS2y7QDzjI#+jM
zXos)Wi_Z}Sa{mHDCsnQ3%(#~`Zu#uUl>xHF`<}g>>bs!sLFu#85lx8;pH2Rz82#Xd
zW_^=lHK)b<f|uWO@(<oso&WP%fz}(p6G~I}-D!IJ^Pj9}k;sf>n~yfZxx!(qUmd&o
z?wrF@zKgyumzBPFBcy(6H9zyqko9wY9&kz2)GxMkIoEWSCFx7nTctnFCZ1gjzCC2U
zKYRDNjpv<oO)dWPycGGj<;Bgdf3sxP{rS5!(5`|}TQB1DD&O+PXY-ZJ`^t14Z|;|#
zzgd`T!au2cZ<p6>v!<Mon)tW6KyU3s$F7U|XDpTn9lmGTawvLd=<m8q6VC1St@^Xk
zuthj<*5~J@FFl<9-hK9JKih}YcMoa~^Hl|P7ys$pcEN7W#fSZZPZp|woO8i$d))12
zt~>k^*>7HWOl)?Pw_iDN;?Zpx6At)#TY1i@U2yx87<<QFwRX$x7kdN_yge1%lDhBD
zIo|Z`1$F<fygWB)`G<FZ%`UpUVHaB}JALK!)h(Q>Bm29blz!ndt$VY7v)DYl)Ctd{
zBb{g8oma#hy#Bz4&fXuJ?nqgwSW2EgpPD>xC4=<og`s<1Eq*89{?g&)?>T3Z+)kgE
zd02ZxK{d;EX{Pu!_VGtf|9qWzOnh14%7U#AudTJ5v$WScR3pY=VuILHhq;<HpOzlF
z=bOaoDq3duu*$5<tK{sRKik6$cK7KxPm(xrY`)|=&ysA5<FBIfZr$v4dCgYG+g}>;
z!CGwo_4|EBku{0Gzx_YpwDxLJaktr&s70bezczH<+a^}@ZsFS(m61nW|Go2<N!&2$
z-^%-)yVXN}OW*r*NLuLbGHDm1U3-N#`taX#OBH>#?~(ue50CEc-*YG<!$b0u^?6ah
z?+bgYN`DKhNeL-uPM)n~-tk+0U*);|-?&AatxS@3|Gs$gy5Whk4B@v^+jGoZtP3pG
zm(+dvsCCa`_NrBvpR&vUlK7^!UPs~h^7iD}`}S^Flofb$@i)oSOMQK1V$>B}-?ceU
zDpr2=_P@!MKZRl^c3*#fB>c9*hh4oQ$6H+$zP%Ek$Nz^Xx>`Om(pbOw(mmVXwd-oy
z4TJmYviF7Vuq`dv_3{tbhiU5tellNM&}13BY}y`9v8Kq$XSrTK*t2c=?$=zpj@qYh
zc3n@EUBhb=uzq&q0yDW!?ytU<dR%>4w46UxX+=auSBCETMq63_i2}!cPIwzj+1I9v
zs>CiadZs&*qfY(5&9k&EEIhH%cg%8lTG;1B3Eg*kuIPL~(Q5uDZntw>fy<fI4m_H5
zF0Smk=!vYoKHdrP4f{=Jc!@dfYYgs=aoZRyT;U(gopwTgVf1r7-L)^I3+{WSy<fcT
zfO})a{O>)Y7d9QSTpuaQ{oU#5*E)&o4LesonX}D2$*Xrt#gU&?0(_5)t#_<vpV{Ll
zwb9q~P)_c<O$TlV)_gqCVfnF=JLInBtcN|no7RZ`ziCmqN%Ym;O-kpv&O8o0Dd?7a
zTkVH{?Dgo9`AsX6X0h*=QEhArs{45^f~)98`D}?W(Sz$_dY}Aa|6K7Tao^ERZW3oU
zipEBk%{`XCj#u2iQ}APq_S?@zpCn$n%1$YZs%fj7?tL~jf_<XYoZSZ&Pi);h(Q*5)
z`Fjs5iWc7Wzx^}A_P~nmUw?;tN49oO2yZbxwys2ymAOLyf=4MA*N1a2XS)ltx_><3
zI9W0C_VO7ICOI8GVbU10?d|vC&oT{<UcF7y{K4vNb|t-~BKSc;?z^+hotGt!mM>dv
zr6$|**mZXNO8>=WjM1yES4jQI==R$;!J<|4ZOOIU_cuRSx1l3>-v^deU$Ua@_J=do
zZIRSJ_s_D;F-!8)$&ylmvYqzp{fam$H&6e0OMCVK^CXLJfv>D<mdsiqGvx-;BEh)A
z`s=2PaxVow^_KG$h@C(E)UKGmdU?&A$3Is0hy3IF7_EGBhneiDt^Vt!?zLP$e)gNa
z8}~fD(`(-Z-MDaZvsc)+L#n@j?cFKr8nE5}ug>>u;e^iofH{xOwXV3CRIgs{zhZVu
z<fWn$EaLN~%RLdP=g?3&oN+Taz<9=%GoLwHs`czFYug_yrZp=a(G}xSdt2pnajM}5
zAIH3$2sM@?aZ^g#<GES0wAY_q*C&v`zh2aSUZHqa_1e34ER7PsX9WFRyd`MLkJCTr
z+`L*+aXsbw@jw4eH*MADIoo32TzY-8P|Oc5=bf@gwyzF)5O!h8C9gcie_ayK6`!g%
zU0>I^&hchV&R>_!>x&<8W~58iOi!(C{C%c!X3v@AgC84`pDrsAGg~FZ9d@I0!>adl
zV`oojJQ%%ZtLbX~#FJH*jC@RaJ9ou|b~OcVu!);@WBJsscjhm*#qZ~0p0@0OtmwSX
zjH2)DGY>ufeL-7USZ3p6&Pm6k=1h9kykOVHbNPRy1uxsRy3W$N*ZhjX`IJ>XPf?!K
zu5aIeG^VY4#T-7>qvvf`Pv5EUjW1)XPPVKHFbOmD(2D-ha&Mki^z-YE5|cik$oV``
zdPiHpqH<Xdhj~UXEsB<K=ji-m*z@&Z_Dz17Q{8hG{5be`<<YrvGxmPXt}A(9HO1YY
zr#yCNZ++<I-L+r0x7L&d|28O=?Z27&+k9#C!HC6`eqSev&O5W|`-}ZqY)?-Iwj~R<
zNr<RzKJ0v!gYPSMacjg}xAogThpT&kUGQ$vn_er+zz3&ZKFt=hV}I**`qWx`Q$@QU
z%O>`oF4Wk+{rtuU8wFE7?D!oP`$zbS>(W!A&Sx9-%OX1~OZnYeLkw6$A{%dBPC0-3
zbC5*IV~)$OG^I1lIi!5V!kym#xzq5-`+7oa?rPPgeGPAA-U`~rd=pqw6!m-JK}#Qd
zImH)ms%0aVSkGzK&sf0N+@6y7i1)*(MH}tRR<ybw`L@Nc(!$}_ixsDjnai43)Ya%r
tJTAz0(Ys)V+AGD+lQy_Tbto_Kk1H}<{NnMZ{=07vpY8h~?-nJc0sxA9tY-iK
literal 0
HcmV?d00001
diff --git a/dmp/matlab/trajectories/psi.mat b/dmp/matlab/trajectories/psi.mat
new file mode 100644
index 0000000000000000000000000000000000000000..c1b6372952d1262d5ad2bc6ac09f37ceafd74776
GIT binary patch
literal 3893
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NS<NN=+<DO;O0t
zvr-7jC{=JtO;#{8S1`1&GB&a@vQRKGFfvde5-`93qo*%F0|UcaJ_d%0Icp<0V|p`h
z`(F~csN{Of`=*rlwv{pFfs?jw<hd+y=u8A#<fh|3J+)Gs&L0VwWbW#uS{dRrGi3QC
zOZC!Q*6x!GCknj(Z1cV7UUA&#-*xwE?iZfZ|G)0{zG?se?^?I~)vCpdg_D;qUVOg(
z?TPx5(hsYd55E2P_;Tleov1&5Pp!YcE^fn=xtAaRPuwl|Kk{^a*xH!I!h0%qo@;95
zF5UG>;^oS}uj`Ne?|-tIuQPGU^22c#&zf#B`Yv)HesiMtrA@7+{;Dje73x3i$vj#u
z`s8<Lf{peus}0q%8&@B&z4PwGoL2vnJJlpU#lG{KZdpI+v;Kp5`wq@n@%HSKd)gcp
z&;OoNTh@9fzx8?CLeKrr_A1uhJ9thjZ)>Hu^vCavd<(w2*B&kr_;#CbYRlhk--Ov?
zTmHUQsy>?jFY@|rWf^yV-SVlmQdS$84}JV{@$1^xyvL`!R+jlLd`0j2nuoOk?k$hY
zo$B_kzqkM9L57DmJQpse{<}Eauf)w_zTS#Q$39<}$jmEv?zF&HV~*$#ugbz(KEH0u
zi?PWv-acROP^HwupRV1z`?AcGeo8Iodw=KFnS9B~dwehKm-#Yp->p=qPv(xnf4QC>
zz3l5LyKF_}*PCxvPrK`Bd-r1JWXF>ax8C)PP2(`S@bl{QGaj|J0llkkUwkQDJ;ADa
zf$gy_+2%cGJoi33Q5y1RR$iOfl05DZi~8&bo885h{F|5Y_?_qF;`;ZV6)#%X=tnQ=
z_O2GRt6F$wlKQ&ecXT-Qr@h$CuNmfjKU%1+O4<3jZEVf^?`k%Y3y)~V?<uuQas9E$
zDW1jJ{v!VdwLOuGw{?VHic*(8G@sXM_SDk%ev^40UKDd>_1Dqm*q~Z-Yf(b8_P>`e
zx3z>vKdD}oe(tGVSxdZiish4b!~O3!E8E;&m>zlR$%f};E&0_`?9y*7$Tfb;`Tm>6
z9>bS^x}?9g)K`Bw%Wr1(IY*|0k1u1%&2@_>ms<$f$^<;;KF?zt%I6~P9`epG`+0+A
zpNqVEiKYG9xv7hkFP;=^)hmj+ee+P~i>EDV`|jQUkk&1H@hs=gH~D-uK`&jtyl%dC
zi{(V>z1ohx+JL;IXJ6jjuyZ+Y_o96MzndrB&bfrAZ`t)lpeypZ&zDlS$Km@tt<=iZ
ztadAOe`}ooj!AsccJ8Vo6KgArQ>`!FHC_6}yY1hy;P^%L@gilfe@vXI{Yxpa-{<v@
z@6|5fm8*|TGhbu-S2XJ0dWjh8(vo#WL5xS4|0~*Ae0sJz_*<4t_=WCz@x#xbD!)s+
zv0}l#y<47loI6_Zdv*21Zl2Ht>pv&__wUthdKq4~X>ESkp<iX3;&-yT7DU~*PCA*a
zy5OMQ#p91EbnNdGDL8Wazi<8UUTH<g`e*Ak7Oq^M@=Ra3J0bMse7)j5Qv&`x$~$hC
z&!w38?ETBdyEPoG7GHmr-r4$~KygW^{(8A{?{@4-`gP4o;{E+^i!QIMG6?=U`$Bb`
zV#U{)CO1ocZQsnRIk&FPr2qaLoA>q$<#hfy31yw(URa*(^&qgl>PGp?ulKyytA5zz
znx$GF&;H)`y3Q??$p>6_|6XjP{VJj5`vNA{Z7gEHI3=$*UTAYW|Iu6}VWXbairN&}
zJKtYzS@32J%bD7EyMlWG>~EXiKkMV2-Z1m3g<Z4Nk0hU~#Xr+HO(ONy@D#6X-l6Bj
z!f9frx+6Y!HLGpozLP>b{_Fe??cO40!|SCKUg`Cqr^2Ck>S;gUEw@)4vzfU)xouTp
z=_;L%UXHwnj|ZLkUAt{JTh7rI)eTwy{Vc3EvAuF%o;Us6goelW&ScllUiI*Slw$$s
z9&U+04<5YCD(L6@tD_de^yp(lR>$AzA0|0Cb{?O&F@M#AKP@+FT!m)Dmpoec@Sj#h
z-PP)wq3@j}HdbEyF!@;MgXu*x>cyR|{W_8_?jCliAv*MLeyHf7<;%6BmhTF8ntsOd
z&+Pa8_dZTG*V0nBANpd3(h8aK%NM_G=i;!xnfq^hfA;~^;>@V_qoM}U{}0a#bnZ%g
zI%D2uDYh_&o`+GER`EQix3x~4n;GKpy<+|;-@EdTNeBP^xlzmY;OKpuuMd=enCTy{
zei6Z1^LM%Af4xSh>V=6S8~i3bPvPXq+N-bAF8I^?mOIaMA=}f56J{(B(M!><+g6-!
z`N!(w>xb78B>!wx%Wm-Zu<ZCGeL_<1s?zHM^{8{_{C_`QkX86U=!E#8crA8E-sVX`
zV)dJ6d`P}DpX<Yhc{esLbeGgo|H?C4`sDu83u2GW<K4G{Y1$qClf_$g+cnGolo)>t
zaX3<-_^?Rgg|JZlo^4EX!e4&MXT4&c@BjDaa-o*Y>OX6lwmqx6XZ|Wk^$6#eBiahD
zeAli#-tM(Q=#Z%PgP*%U3x=zQG+NB-|7l~VQz%#A(JRhZkv=DM<*{p@zF%7zca?|x
zrkQGpas45agKwlPB>q31dn%i2g|Vp3HKq?aGv5Rq_!j!@vro^f1&?=S&R;WoS;FVi
zt(y%NaxIFt%_$Y<&}=XG*d$aljgvQwZ;u84-7t;B1-Dzn<~}HzVRe1ukIR?6405}7
zRFtrs|LuQtw$*cugFYgAc&@$Ob?$EUvM0am{zdBT;i<b_>yzKUlFP-aeY)e{JrYab
z3Wy%~*0JEopW_!&|IKSm-5gR7BXi-nLCL{OSJll{H^|@Hn|)U9u-L|fp%2uj%nsq2
zsLWhfTj<Sts4QaFMv=Cqe<qr(bzps_-o5R`j_HRkue<-bYt@50h6fi))xV2+{XR-N
zJtF6Nh2@!_kuw}bw}`V&J$!+AT8;NAhnrho+&X)(XvRtZ>&r^DPI%AzJokUR(F)a{
z^5Xm_BKAJsZh8Eg+yut>H}l)KX)Esh#j{^AwPC)H*9?U%&HX%jxo@^_u8go<(pP_T
z$3a)Noy+@Vex$j}nylY%wEd0~+eF**XUxK{80A<zTe#|OmFd5$oA;;Yxtq?rq`Ncm
z@I$Ey)9U3!v$dHMrQe=kpY|_WPp`Z<V;z&D?t4e0j2<DgGS$O^*A5*Ef4ty*@x6P0
z7d0hX-M=D#Z~ar9d%2VK^N+lo);Ql_-P@D|%{AiPK_0^5(?qQ1{wgrt)82D-d+3dY
ziB*fe9BOQqcsb5^_T%wUp7kQPO1%URPkYp)k#ggP#|#h7!|KehDqKHF{!3-s{`B}S
z8wsCBwI|C@Kak^JxcfuJ$%%(TzN@mQ7t}ZGuu};VxKSDOwc(-5!;d>-dyh}m5?lBG
z-T%J-FVBnCf1f3LbCyA2(#5m;?LU5;{<U=RqZ@BbWsW|dY*?0adZp#fQt9@$3-3%Q
zJpS9<-{u&rTH>1J{~NZ?f5s*?|3O`c{ei+NiT&1llkz@nF}}0-#KGO~c{`pq`LI;8
zOfTbmEW0y&Sy*51vrC8mZj(B*_t;&rzwSkT&g*aV-I#FT&AzMe<!|YEK1u4dmw7K_
zDSs|fs9xl9g>Y8dlP^0e=V%Lkwmtvmmc=fw5Ar8pEu3-fc0jM;+WcgZZ~o^m|F~k;
z@?(Eq*T!vs#r1QRf6ug=QXXZ0bn2s@Ds9Zx9AArh9#?p(+&484{=d)bFZ0&qfBX~f
zYX?g#`y;$hVd4+@uAlAO6rJo|#{9V*{#cE-yH#Ogzt6kStp#>$1^Zs!>6Ge`e)7A<
zc-vjIe@P1p-A;U9S#6@%aA)~PD`B74ycZ6APO>b1%p9J;bui*T%YnbHU;SB5ZLdAn
z<b3bf^#jviT%FZt_||_;yivkGzvB3=ud(Xi4Bk!q$8z`Ag4qT!-y3hdFTM1wEqa?O
z<I_sfo&Ike9w+|oYn(l!UhBz&e+-_NcgAwA4_@H$M4$0R?U{I`3qM@q7kq8$yL@DE
zde@(WuMUR^E?qX=^PkM&miz@(f`1!dd46FxI?r=?*Ne)o&VM|@e_z(Vo9;PR_|M&<
z1GCPYXE&^~d+@LCf!mwO+m5VzR9~n1`If_*l7I0Xe{2%U?(j!CJfHoA`$Db5v$=^z
z^#MNLm}jbU2tGc3u~O;dqZfag^q+Ol-ER8xN2SsK|4+|ba30T>>HZq$`RU+a3AW$a
zANMWX6KJQg?CYh!p6@UJTl(*1-QqGU{aH1!k3V(PTg@+--@41%RX(zy{q6o$Kdz_l
zzV+OAciina>uu-!OZeUX;^e0#pI7hfza)9=S6acA`QN(!PCEXL|J?Eh>p2}CT@B|e
zmHk&d>25qvD!RMo&f@Po1^$$qznb5z^z%~HwvJz7ld2BwOjeQGdj3HD`sD9hAFF0x
zU4Qnt>ml`{S9u+cxBa}II^&)5spArd_@6SrZw_he-TG~};Z1ju<iFbu|M<uMzi%J@
zC*jq+gju&VeYvau`B&aKyJlss;r;`!4t|fypC>!*z<>RTzkix-J6_3UF?n%WGQY2N
z?{)?612<!?F18iCWV=)5BY*lbo4v-VlboY2E=={0<~G_fBk<5)r?mL*GhfJO*nOIK
z!~WZJCW&;Wo%||x8PD!so_sIfjOF&q`nS8b_y6^;?wU3~jQ^3oukn1JDE)wanSV{>
zf7PDif11jau>0BHd6`@6PKlp&mag8Jyiem;jp(Ld8u7X1rUsiY*Sx6hF^>FeSN*d<
zFldpx+0)t9h4QygR_0aet(*SJ_4Va7^S`;PU8u>}cV%7W{;SQ;MC;g(FMocgs9=v&
z%irYsS9KRw_1|6mwcoDkck405uN(fnjoKHq=Z_YDLA^TD+xV*Z+2=Pu+)*#OWZzu<
zkF~nC9`&n#&-|tS)3|y+PfzCmte58>UHmcY*UHaJ|2lq2{_JeH*ZtK`{rbI6EaZ+(
ziLa`QTTooEpjh_B<e#N4^;p&{`Rn{#az$JVzuEoi-Yox9MT#e<NCd}T@^!7>QonP*
zkHNB^6MiO^-8#RR?@^6LeZ~11n|{uo+jf3(?K0c?d0XO7*gZH=>%U8WgWb)7wIb)0
z-<F@!@AUsJo?h%+JU__JX~_?}yW%3*@49`S%|B;<Hr(5OhWC#}pS}ME)@8fiIq^gN
z@4;tt&#B+FpWX6U(Ch8LM<%}=_r`xHpTGGz<K5>XkL$FOzot$6_tW}+p^BYt|L0Kq
ze1rX+GIQJekEcJk<&ZC_^QaBlrq59yRUQ9#kILUuZ?5cLW;^{Y`<(Sv|Bp<pnDuqz
ZkC>&?=Kfkd<JaY%@%#Sqy8Fi51OR?F@5ulF
literal 0
HcmV?d00001
diff --git a/dmp/matlab/trajectories/s.mat b/dmp/matlab/trajectories/s.mat
new file mode 100644
index 0000000000000000000000000000000000000000..6102e05dd231ec6bbc5abd5191e89a5242db05b5
GIT binary patch
literal 3204
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NS<NN=+<DO;O0t
zvr-7jC{=JtO;#{8S1>fPGB&a@G*B=yFfvde5-`93qo*%F0|Ub!ZU%;mIcvkYBSNl<
z$uT+#3d$?^Z>>pPw5;r`P>i(U*$>9Pr^LHhW-V)*HDhZ31sCr8S^YJ!U)v%U%vVeN
z6kyZbV8h13I9F04K~BM7#g7#Z0eha$+57FF@w4*x*K>Cs`&swxR`&X@>$iTJ^LRyo
z#-zI<T+#n_3;*936!5ZR$Kl}l=l-bYT<rSrz;v~<-J~};Kk7^FcK%=U&|dqnec+nd
zEir;ZQmYD{e2jcu_J3Rbqk7dk?GHWrYegrm`TM2r>!rz@4*M@BraUtGdeOJCW4+^o
z&mF90=gdF6PkmsaocHVXGp`u)1B<kG$9b$WT6ouYhCo<B#<AO_8ToB>Vc+)V&E4Jl
z``v-b<$eCAZd8hYzu$b@{qHu5)VuEX{0`;2yJp}2B>n3*bBLX^{9KufEp@XMO>cev
z`fNk{i~X#RP6a97v$@js+@|TT^Rqvjc7HIOcb)B@`2Gy8?Lh)n>zbC#`t)sQMXAD<
zKYS@6;@`tB$8A<J;nNnf&&n)(vHM!fv1H$0XMN)`_aADzup{S0qsQ(%&f8W(QuB3R
ziv9ohGSYBc%dt%NZK88IGInRUXY85l5q&xT>&91pEywQ8bonxOhwX#9bf?GL)wip^
zl`Ofxkb7ZeiDII>v@t(VnSzNtd(gi86b+vjUKw$*C(mrrx4y$uq+(+4J}Er5BIo$>
zyBudPckU6m`gX=`5%n|IJ73(1+r9ZuoA9%{b7d^{9tfAbCHwikAJgAOWncDOm#uu^
zIA`5_z4$#F**{%uUNJ4E-g2wA|C6QaH^05Om>$dj&84nN=zlD8kvQM0R-3O%xqp}M
zn&<tdwdSkRO3(Q<8PA{h?6I7@m9hQ&`Ms}t{_LDEF?721yqV9NY<5k!z4~mU?5o%b
z=kKZ9+vcA0;MTnp+4(|cFTS$dNA6bIzi!F>4-+`o?v9z59=^!?{=d6h?D9@*m%jP`
zUgTf?<##5On;UJ8vwie^<GU04tzT|?c=Fixbvu$}Z8}b6ez<zr*mi@QoQhW2tnkJ6
zm|s8gRO^Y|U32L8ya;*!OUtZ^6605^Upv{ogeCpv_5AOLswyXTuy23%W5<_+yS^$a
zZ~xkLzj6=1;InU{yPuz5dv*SqwN2a2dH1HsRGnnV=fC1BxnebY8}mP<d*Tx|nJX-n
zJ%8R>a7y_hhSMR-mTCpt%B=qTCgW{7TL#Bgqt9H0(f1x)Ww>alUn~51l4MNNdrS8F
zmk)1B<hGOAc7P|CwY>VPz=S=|-#)QT)2MlHc(q*Hujl*T%+F@CUmp~=TETvkq5h_e
zJnuV$Soy+lzgfR+X5vZv{0~u__p)=^*KN#Y*n8F_PCIec?&OBu$;?l3_DtRTF^p9w
z+)6*#%G_bDz`cuHH#XkTuG$#RJY|0T!EEb$`>QzCzx`%XFMCJoPRcXUZ#*+LRUZ(M
zwUF`LZa*on?cD#MPf>iU_6b(jDxVR!S*)}E>4l?l0>(MwOL>Z(*PhI_t>Avw6BUtl
zeaDe?8~LnUeYSVcSl7CJp3d{Fd{XZ@7B#B=)0O|V{mLPACyU>v0_Hi-{STL2JG5a|
zdHeM}s~hKp&bztVCOa{^BRiyHHJcTy>J2v4J1s}{t>vtZzItI}YESOz8M7bgd|KP)
zdVk7?DCLK1!^J<{>SLc*^kA#ot^K7jf5IO>TCMuubd~w)%GUeN5Bmcy95|w|xPHRL
zmao?wp1oVqb2m`!hRTJxXV-H-t-i^!*!7~*n>lZ0Sl)Eoq0lAPXYSjZIV<bPKMm%^
zjv{N)B}*hk&aGy2*|9{XZo{VsR~hCw+z*f~xiNXkjmb;e4{EQiSlck+UnBQ#_E(A5
zlh2+zK8q_r?cP(N_hF8o&Fw!&ajK+>EjhEQsny9UFm%Bl6%W;G(fkuE3u>Mn@DeyJ
zzLNbuzu(8G#*eQQ-yX2u6Xq~u!aw<{8xIoCX0vu2=QlgRR(!3hcpBfe^s|qq9@o5m
zeR<CH#J{{Bc3k`&xOh9?w?}_|Oj+8m?fv|7k;(tt7tcH{oqT-l>=-_#-<-9*2IrPO
z;GN!NWbv<m=N_qt-G><Gi}PtP7ygc$=YH^SmF~Csf^xs6n*E-d^`Lm+-t1n3ZKWFP
zr39v1&t7mdIdJlEB~y!(J%?6v9y^vNCA|A3+qrJeN!O14-=+7@N=NJ{iy(V%HlLKY
zqffqSf9pr7C@!b%w_ImDxOy;gF?;(|kBX9vA1f|=NU|``lw7YCG1saz^wpJYg*bjk
zwTKz(c>S)6b-ihA;?&x;>vI&t(nk%m6XL%7+j;T#=Lg~&|MwUi|8*wkwXvnef43R2
zhIL^_f8^|VRyQkL?z-ik_~!+zcA`f)W_2g8i<g<)`uEsl#o1o%Ij4@)nKY=~US@Jz
zP5AkBhRwGhrENQ!x}e@R;|SwP2B`$=Wq-mvXFQAMob`UQ_rCMz4pfC6cyrpFNnift
zkxH%ho1U%jRYlVp%a&i5kh-9E#`?Y)^L4*g|9n_;_Te9yA6^ZGQ5;f_Ov^jM<#dEO
zpL~4#?sHyw_`-VK7g_$(uFnf;d~t8)@!%G23;82ORv8N)T`|17GWn-~?w0D!Y)w}W
zHL^}niOGK|b)o;?uA(p3!xJ}EG_dg$&szP=MEsE8ocq;t3=CW^)olK?@uz&~6q8LF
zFK3m++pMVkSa-Jj=!t$Qp}nE|D^_<on)C-9w#+M5+P;(JNI~<}p#L2uzgIr|erUyg
zuEk!Cd(O43e)z}I(aUkgxp$29^8Z%vpM74qzvkih=;xnny6we({Si5N@+1G9^V{yN
zj;ilH_WY*Hk>v^Nf9xrkDE;qht7E3I&<D$ilT&lIZvEDnQuFTYln;i5^=&P?WrXJ)
z*DE&Mv0whz;(z>yV<h|9@*aJDsLFZy`P$Dy$DY@5KJR#K@AdP--}D<+&-Lx{RX5jI
z{Q5KH)T77S9#ncC`Ekjz%r5r)L$hQ%{_SU1czxA-@=j*yPm}NRq8Cbi1&{yWI3xG}
z(w<P(JKH7RAMwmv*&bbFc})HD^_GttOQp_xX8vtvVU&OL>^A4eRTk6rGxi?ZEXVR<
zTg=2yDZeG{EIwwnreEht<3A(!*Hhv(@5he2=Q^J^`??>#_tkQyO!fLCiJz0dDcc9+
zdp|z>%kjL1lzp}Q?T(L?e&RnF#pItp?|Rbz>-@wIPDdW6K7W3oU`@a3yBFFv`RUFK
zNvF!$zKH+aec)B^&pD6%6pqzB+pxdk>+#P9x6d1#HJ^EU;?4L8KUQs@I{*Bq?MH6N
z2Q=O2`7__W=WkBnewGa{<TDQaTln#>#?|S!>eB=7d)!F8=>Idqf6MbXY#075`^R6%
zyK!^Sr?aX-`@@veK5yGqwdiV>;8CHpe+KgF+V||YsI;BDv@+%M@gK*kygz%t@2`pJ
zKU($7^RwUm`L~`w3jbKU%;8Y~k)OKL-px<>_;-WLuJtC5em^r6lRwh^K3FS{`6+kU
z#eYxZ7X0-1{?LC*+=sPq_U&SLQ<wT(eRuuMzo!cJ^3)$stY>^|@3yDj;-vg1PK*6Q
zX<rXkPIwbp$7B5ehQ7?V#are-J^tqW>1+HOe|dadxA-f=>HE|FhyQzb-?#js{iAo;
zjhFvz&zK@snQ37jSk(9^Yu%Cv0cDZy2>rF{_Em?x6_a#ty;iN)mgjmcV<4lgwxp%w
z-Xh!WQ*IsR{K%l}{&<_e^M9GI=Od3SXSdXw^r7J3pH8V8{}yap{y6#b;`E;dI`wUt
zHLi1P<jU`#TzBYSm6-nYXMTpKpTCy#|E-_&^I)B!oNz$(KU?0nx<4jAsy!XG&+o12
zg^m1AyFW#81h2bXb>i(-A%BtU_M6XrGq&D#?icIlb^-s&8nM5MM^76(?!ManwK>)H
zMiuih_n&K~J=$!;|NG$gS;oEXU)diACo0~^50Y1`msu;ndHv%Xbz$+A_g%&Q{$CUE
z$@lM5ufI;O=bzsAFa2XcwrY*aYkTjD2KJd^KYt|cjj&r&ulp+g$itr!`!6wNTZF}L
zJNGC(!cN}x{F&2fQTAc=J?oFU@4FW_KOs;m(!S{OhJQW!JNLTn%9ogS{qZ~F@4V6J
zf6rbkziD^YUvI<Dsh?|4ADTZY`QxR3Ynguj4QDUga)0U1WnWJ`jj%UQ*uS}S%ki&P
z6Fz5J`mcF^`)A5?!<uWMa{Ar<U)SI1-~B!^a=%EUf9Ib65%nkkUXT99zTxkuim4Nv
ae@@!4Xa7{O=bH1a!~gsKV_x>Fa3cWcF@zle
literal 0
HcmV?d00001
diff --git a/dmp/matlab/trajectories/z.mat b/dmp/matlab/trajectories/z.mat
new file mode 100644
index 0000000000000000000000000000000000000000..09243cdc8e4d974bfd3bc1abade837cd672076b5
GIT binary patch
literal 2094
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NS<NN=+<DO;O0t
zvr-7jC{=JtO;#{8S1>fPGB&g_F;OrwFfvde5-`93qo*%F0|Ub{b_RxuIcvkX147)z
zWdAL2vEw@IF4nW|$ct5-4P7n2w{QtFcQ~?`T-4!GH7wl7lMoUj5GbN6birtcVYd?Z
zEDfuMADvqiI=U6k@EkTj!NBNl*u*4!Lagw2>G$fzQ`Dm0#hovHb>{89?|05KAM;e1
zR4FFZY533NX#c*JS%QzN&fBE@;OxI4@wM>FvkknDES>-K|I+;^+&kYlaK3kvvfbpC
zwbAO5?LTvBV{0CZA9;RbdCOA)*M!{(|C(!ey*FJj|LTO4XZ!!$nb7qy?8V*m%i9$e
zG}qo|?bEE*+cjs;`9JoDr*y47x_kTAjm7>CPF0Jm@%qUnJZDdgT$*<N*Q@v&dlO%L
zE4^rP^xy2e@{K!Q3B=n^U#`>Fa(n*gJdfxda_)~`7CTOydoupK`GbrY=W~^})@jT4
z7(dFWQGR!@w!J;ss@HPX@fK~H;)^TSoS3rs=b0Tp{F#}{WMmFs+>>m1U$Xy^?V&T4
zv;Ld<{rX%Nx&F6B->VG|<gQ2O^BHY=vO>;VZ-<BNGpF^cms5}aE%*|1E&FVk&wNw!
zm)#cYc~h_XJg>-$fAM?O?B~af_X_W5|NQ2knPoE5<=n%px5}lDtKTyCYj$Vp!$0+Z
zc3hlxHSx5isrs4O@>;21J$4?^X8few{U#!|Y|gV8m)q2h-$d4ayL0KS?Jve4_ORv5
z)Bitwy1#z;^AoR1pS;=oZRbq8l9W>aCuM#yfwFP-6P2rze|*||vHs${2cH)fK2<Dy
zT4YfWHoGQlHDlxQJxc$JmKc^U2|S)@)_US>{+VMJKEBerCz6nMIV0=x3dY&}^2r*B
zC*K&qKWn$Rv2MK-kLaF_Moi8h%pFp`#PBG_3ui>Vsc5)qe%&zY!L;3?ZEVNRdmqRU
z{rmArx0%7+w@nKh7tdavxZ)vil2pdd9fHosw+jS!KbIF~%sSy~dia7v_yZrFGuK0&
zu35Q@{ib|q(t{s+C#vn8`29dvgtQ9(l{d00UbgxES|?Gic>BoH`D%K14ca5Lf4up}
zu$X(&p*P>!ZvN$Xdhqt?rtSY(d{@>qo;6T?`Qyow$8!s&MhHt-%)3x-bCHp)W`}R@
zgn$~3#dGJbZ*R5!vqQEn%fh}yA^$}K*Sx%qe{;Lv#zxG1x=-u$UcN_cm$&mT+wO42
zNSH@e=S8&b{`^wmE4n3z0uHEpR!vmd%jk20(ejT`X<yl^IaLvVG8x@;kAzqyuW;Di
z=dQcsd&{n`E!PhAJnDOSRMs@$Li?xR6NLWg$$#5hvxDtU%;D!d+O%)l7+n9vICafD
zDG3YRxAwZBhh87aa=j-|xx((#gucmNx}PeAOY0pC==dWwzphG_$AtZ>IlE@#`)Bty
zRCU;u{`(yKd}_dv{_RTzxtMLanQUJ_kgePMYj)xU`SbUz6y9FRXXUS7I(vW6hx@E=
zqpO<pU#?7GH8jn*Ud6xeYgnVb;03<c_jPRxn^<>t&fD4f_Q5*4hJ{Q@{&)Edjr$T;
zBzWEu>EWArxx(t)eqY5xE5B>jp~sCo{=T=F_A&hL&W}d3MWd(uo1R;=e){~1ja%jR
z2|wLx!a47`yG!2V$XP|dj~Gt$?)&i1X-28_p_tGaok9C2KX2o^-*El&K0oJa%P-zA
z@BdlCe#I&6(d5l5LyflV*|N^|*A%BO>-pm?uUqaGTQc=a-G^cuIp0fhLC4eX30l-l
zdSIEfVy)QE8#_;*-aY-Ez)vNic(%o_1s@sy?Ra)R;SbNEf1f?vzC2&tal(FgZt>nO
zt!Y!I-~GJ1r?2`gSH=F#Jg!?spRJK{dEvMq>0#93=nUSA)v`0Ee1DT!v75`#R-=59
z&(mkIz3-|6_7y%C557F@j-%z1@4=S0tV&iZ6gJBE-mkoUSoO8qEyrJr9r!)=)hoW|
znSW-EyVjKx6&jC^{q>f)^hev}-NQ|95>mq?Pwn5Jbi+D8^Hp))$9tEgLz*Y4Ek0oL
zHgw9dFPkbi)+{~IKj*yg>!p8Bxn1|%%i1IFcxVsX$-fdRXS(nCi~g2&{8b<HX8*K5
zwgq8N*34J>f6xAh`QLbNmv!gAwH{?>zU04jgZ_i)VhtTNZ`Kg|dBsNmzc(?xsPSX$
zRd4q@mU{n|FZbWA)9#*^*lIs>L+RhOg3SxLEUNl`+0K@d*~g`k_)lN4@ulcD?aC{4
z8~;jf&A+K%Tkp0%etp)Lzj_~k)&IX!QCzy^j(gUJa~gGT`@j6~_}T3G;kS+Zli-v8
zvu@bfnXKvxR^Ycg`)dAzt4+_Bz6d;ZMtjy|+X-eD4W7T<aQlVwqxHfTJ5G7J9EqI#
zS>eT>)yiM%^H0|YD*h^d>shDrq+G4)UFV%l!~d@0*80vWu_e1tMAj+FDLq_zb;Y%^
zv{RRFRZTJP;{N%2&TMaGx$T|*4%%O=im(q@qq_G0x-XG?ja1GkW-~qASa$xGXvTEY
zz4~9=Kb$XmP|3af{`*j$Um10|fx7bpzt4-DP#1DQJ!p}*PGr6Fins+|ryr;jKNY!m
zL;E#$pY;{@+dp!#->PqvUs82*kC(%Y<KbuY&v-vrQ#X;js=c#;ZDR2LK6}+`r{8Hm
zP+Mn}nzng{|Bd;H)AD6kE*CYdePq+JZ{x2(;o0B6KL6&+b~~fS<$HMBF1sZkb=GK<
m_tuuhr)boR&fR~YwX(M=-tD9Pr`JE~vz==HGfD)pyaND_9vQd*
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/__init__.py b/dmp/motion_planning/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/dmp/motion_planning/dmp/__init__.py b/dmp/motion_planning/dmp/__init__.py
new file mode 100644
index 0000000..40b5f42
--- /dev/null
+++ b/dmp/motion_planning/dmp/__init__.py
@@ -0,0 +1,2 @@
+from dmp import DMP
+from temporal_coupling import TCVelAccConstrained, NoTC
\ No newline at end of file
diff --git a/dmp/motion_planning/dmp/__init__.pyc b/dmp/motion_planning/dmp/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e776f2856a3618cc12f701c89327b4429c25b01a
GIT binary patch
literal 314
zcmZSn%*!=>t9E=c0~9bZFfceUFfbHrF)%QsFfgPrGUPBY<T5fuF)}cM_)H8rObofq
z3{lJsxhxD(EDVe(%nZRAEDQ_`ng9R)|F6Nwz`#(#%)r3l;v1j=GOk3Jfq@~!IV?5D
zF*(^eKd-o?C^0iHHKhck$1gv`*-wKNrY9x0APD4=5<!qiYHmS(QDRPfa(-z+PG(+u
z5XkHxkeM1FMF^rejDdkcKO;XkRX;H&DKk$$IX@*cFJ0d`$TK*;yjZ`uC|N%zF-bo+
zza%q1FTNlrF)t4!4-!WgOUW(JkB`sH%PfhH*DI(j;b353u*uC&Da}c>1KD2;vR{A^
E0E|0I8~^|S
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/dmp/dmp.py b/dmp/motion_planning/dmp/dmp.py
new file mode 100644
index 0000000..958441d
--- /dev/null
+++ b/dmp/motion_planning/dmp/dmp.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class DMP:
+ def __init__(self, k=100, d=20, a_s=1, n_bfs=100):
+ # parameters
+ self.k = k
+ self.d = d
+ self.a_s = a_s
+ self.n_bfs = n_bfs
+
+ # for obstacle avoidance
+ self.g_obs = 1000.
+ self.b_obs = 20. / np.pi
+ self.a_obs = 0.2
+
+ # trajectory parameters
+ self.n = 0
+ self.y0 = None
+ self.tau0 = None
+ self.g = None
+ self.tau = None
+
+ # initialize basis functions for LWR
+ self.w = None
+ self.centers = None
+ self.widths = None
+
+ # state
+ self.x = None
+ self.theta = None
+ self.pos = None
+ self.vel = None
+ self.acc = None
+
+ # desired path
+ self.path = None
+
+ def reset(self):
+ self.x = np.vstack((np.zeros((self.n, 1)), self.y0, 1.))
+ self.tau = self.tau0
+ self.theta = 0
+ self.pos = self.y0
+ self.vel = 0 * self.y0
+ self.acc = 0 * self.y0
+
+ def z(self, x=None):
+ if x is None:
+ x = self.x
+ return x[0:self.n]
+
+ def y(self, x=None):
+ if x is None:
+ x = self.x
+ return x[self.n:2 * self.n]
+
+ def s(self, x=None):
+ if x is None:
+ x = self.x
+ return x[2 * self.n]
+
+ def set_goal(self, g):
+ self.g = g
+
+ def set_tau(self, tau):
+ self.tau = tau
+
+ def psi(self, s, i=None):
+ if i is not None and len(s) == 1:
+ return np.exp(-1 / (2 * self.widths[i] ** 2) * (s - self.centers[i]) ** 2)
+ if i is None:
+ return np.exp(-1 / (2 * self.widths ** 2) * (s - self.centers) ** 2)
+
+ def h(self, x=None):
+ if x is None:
+ x = self.x
+ psi = self.psi(self.s(x)).reshape((self.n_bfs, 1))
+ v = (self.w.dot(psi)) / np.maximum(np.sum(psi), 1e-8) * (self.g - self.y0) * self.s(x)
+ h = self.k * (self.g - self.y(x)) - self.d * self.z(x) + v
+ return h
+
+ def f(self, x=None):
+ if x is None:
+ x = self.x
+ return np.vstack((self.h(x), self.z(x), -self.a_s * self.s(x)))
+
+ def obstacle_avoidance(self):
+ p = np.zeros(self.pos.shape)
+
+ for obstacle in self.obstacles:
+ # calculate vector to object relative to body
+ o = np.squeeze(obstacle)
+ x = np.squeeze(self.pos)
+ v = np.squeeze(self.vel)
+ # if we're moving
+ if np.linalg.norm(self.vel) > 1e-8 and np.linalg.norm(o - x) < self.a_obs:
+ # print("Dist: " + str(np.linalg.norm(o - x)) + "at position: " + str(x))
+ phi = np.arccos( np.dot(o - x, v) / (np.linalg.norm(o - x) * np.linalg.norm(v)) )
+ dphi = self.g_obs * phi * np.exp(-self.b_obs * abs(phi))
+
+ r = np.cross(o - x, v)
+ r = r / np.linalg.norm(r)
+ R = np.array([[0,-r[2],r[1]], [r[2],0,r[0]], [-r[1],r[0],0]]) + np.outer(r,r)
+ pval = np.nan_to_num(np.dot(R, v) * dphi)
+
+ # check to see if the distance to the obstacle is further than
+ # the distance to the target, if it is, ignore the obstacle
+ if np.linalg.norm(o - x) > np.linalg.norm(self.g - self.pos):
+ pval = p * 0.
+
+ p += pval.reshape(self.pos.shape)
+ return p
+
+ def step(self, dt):
+ # Update state
+ self.x = self.x + self.f() / self.tau * dt
+ self.theta = self.theta + 1 / self.tau * dt
+
+ # Extract trajectory state
+ vel_prev = self.vel
+ self.pos = self.y()
+ self.vel = self.z() / self.tau
+ self.acc = (self.vel - vel_prev) / dt
+
+ def fit(self, y_demo, t_demo):
+ # Set target trajectory parameters
+ t_demo = t_demo.reshape(1, len(t_demo))
+ self.n = y_demo.shape[0]
+ self.path = y_demo
+ self.y0 = y_demo[:, 0].reshape((self.n, 1))
+ self.g = y_demo[:, -1].reshape((self.n, 1))
+ self.tau0 = t_demo[0, -1]
+
+ # Set basis functions
+ t_centers = np.linspace(0, self.tau0, self.n_bfs, endpoint=True)
+ self.centers = np.exp(-self.a_s / self.tau0 * t_centers)
+ widths = np.abs((np.diff(self.centers)))
+ self.widths = np.concatenate((widths, [widths[-1]]))
+
+ # Calculate derivatives
+ yd_demo = (y_demo[:, 1:] - y_demo[:, :-1]) / (t_demo[0, 1:] - t_demo[0, :-1])
+ yd_demo = np.concatenate((yd_demo, np.zeros((self.n, 1))), axis=1)
+ ydd_demo = (yd_demo[:, 1:] - yd_demo[:, :-1]) / (t_demo[0, 1:] - t_demo[0, :-1])
+ ydd_demo = np.concatenate((ydd_demo, np.zeros((self.n, 1))), axis=1)
+
+ # Compute weights
+ s_seq = np.exp(-self.a_s / self.tau0 * t_demo)
+ self.w = np.zeros((self.n, self.n_bfs))
+ for i in range(self.n):
+ if abs(self.g[i] - self.y0[i]) < 1e-5:
+ continue
+ f_gain = s_seq * (self.g[i] - self.y0[i])
+ f_target = self.tau0 ** 2 * ydd_demo[i, :] - self.k * (
+ self.g[i] - y_demo[i, :]) + self.d * self.tau0 * yd_demo[i, :]
+ for j in range(self.n_bfs):
+ psi_j = self.psi(s_seq, j)
+ num = f_gain.dot((psi_j * f_target).T)
+ den = f_gain.dot((psi_j * f_gain).T)
+ if abs(den) < 1e-6:
+ continue
+ self.w[i, j] = num / den
+
+ # Reset state
+ self.reset()
diff --git a/dmp/motion_planning/dmp/dmp.pyc b/dmp/motion_planning/dmp/dmp.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1bedeaf35b653fa33fa47689814842914482a17d
GIT binary patch
literal 6785
zcmZSn%*&<D@+v-=0ScHI7#JKF7#NC`85kH+7#LC*8FCmHq8J%cm>5!67*d!SS{N9b
z85km?m_TyD8mtTq44MD`|Nrk-0@CZ^8vr*8B;^D$Yc>M|Ln_1+u!$hE7{F$+Fo4Wq
z0T~*_%#b3$kiyE)!pIQC3TCr`*=%4oJDAPRkir30%)yYt31)DDWx2qzTwpdgn9U7l
z^MKhr3@N-|#k>qDd|(D2Lkd5b!OsvBtRV>ULkfrx0TGN03=GL2$Ad5v0|SFI$P*_R
z7#M088EP09;u#ofn7|B1h8kutgNdPr1<YUug$hUw3n;`&SQ)a|7(_vQc94HT3=U8@
zf*72j@C7lrVGJG^gBQl&gE9DF3;`HJ5XKOKF@#|Z5g0=h#t?%s#6b*5tofyb<J_U&
z0m5J~aY&yv69i7$XM&_OBp4VNN<g8J4JK2-F`O7*TmlN1y!fOv2rE54KdHC`6i|5u
zV5%S!tRe|44pN&4W`Rt~D`8<^VDQV&O9iW}GywBU5=#xhR-}WiC`l{@^U6!uK~a&K
zSCU#(T*Ah{z)+r<Qj$>&maG8VQIe5bk_gsRkY5a@%TjZ|bYgNc*zAJDk_>Pv1@nqi
zbJBu94haIeHwctugFr6Q0DBlj6#FtTFz9FG=cei><|Jk2=_lu>Wag#oI|q3N$CnrD
z7Z)Y#=jNAW=I6y1<Rs?hf#k4>rQ{aqr{oss6;zgRFfcI0$7kkcmc++{d{nFjvWSV3
z5e%6*83h<QnZaz32!sV8!Te;9dqEiFP;go-1w{-4Llz@L2?IkGI4x#@Q(hVqLo*{N
zA!dQoUm7!%8_x<BWCM}$NJ+7VfguZ$a5<srGmC{m1e7Sb(D>j)8ms|wf?p=cG0;Tw
z0h(5L7#J9WKv4var?TRb#N=#n3{|BT<rfEWfuzA|gh4D`5Wx>3Kshc*6vPq(5#kIC
z3>qNggCs#LP&k33h6b?$N|;5d#i=ElpzIBD8v~;OBM&1dBM+khV=^dxfG{YDfCF6(
zQUIhdFl2Ku6iI-BGK-O+mXRS3Tri|S!kvjhF<1kn&kvG^K-z=2L0$uU925><X|Vr-
zKw@D3fe9LT5)_nGh9D1u;)a2dhmi;2HBe-Ny{3!bYoJsDF76==5x5UQ{?r%*KCCpu
z;X{zyz&=#R??ZUxiAcg?Z?O2S*cyl5Kn?`^4V0PSHB$-$Lohf4`GLtGK2SCSDTO3B
zFqf9O3RKn=r<TO0=O^ZX8xkP984zg>#3k%%P|<*tO~71Qx*A+6r<TN*B$k352XZ9?
zV=^fDfH0`20E_p4k`WWA4$bCdC<+ACjx~&+GB$;gA)5&-!o&b7ikT3_XAKjBWC}Bb
zu!txmlCoGq%?C*duvjxALvRhmKn{kY4u%p8?O<gHtq5)4m<DBGNO=rygk^#pt^vyG
z;4%kX5$2@kK?-J&Pf{xif`mY&5va`r$-1C|Hb?<fqksyFOmImACTLI`ft*lKoaqKi
z(x8;Wz$nLP3@egAp$zg5IHV7PLYfgA(wL<!Ql$q8{$@sKJ(k4^YJfqi!CEG88O_F!
z#SRMbW=4h*4u&jFFv$fKNnv7WW&*d+vbe#jc^D)>^<Ne<sIk|~$RJY73>N1FlMpq0
zP&tT7ez1%HR7^yohM7UUmIc%x3<d`<Bzn@VdJG=*Rl3@NTbtn60hj(kplSsY{Gdcw
zlv<pTSda>ClLQHXA_Wv=DfuOkC<H0VO{~bwEzJeTZE<NXIEM#;sxL^pMjjNqpkxpv
z2g>@OK!s#|VUPsKHgNN)3`}N#BN|LlKemH(7#J9QKv4~H9i;BjVsv9PMU)1hlnRdS
zKs?b6$rGTuhXoqQAT}!!n~i}RB*YFC5`naPVC5hvp&<o0$j%^XP+)-E2M#4jI}nry
zkP018c!5I(Oi({~z!8uZ0tz3HlOW*(ZtHO~Fff2(8XPhlObp;w9;|th1#Z*TGBQ-W
zVFY=ag~28EA!98QSP{5rX=Y@oWrlIVSp_7*f+WHQDza)>!6NJoSsdUL3>E~HV=N3s
z@<>w5prkATZB}wJWHZ2|xxk9J!F?bW28kM0P*wspNFheT^&x~<LH3G>f}Fz!wv&gU
zhK)f4qyUn?_!ziL_(5WzoCm_S>|jd-P%WwDV5s2$OF*pR0GTUV3lS0o^}K30Kn1WQ
zH;86okW6O=X-HvakW66$vzXvwEDR7;pi-KNA)Oh7Kt%-uLzWOoX$=R+TjI5xVEcr@
zBsW71Cj-R4HS7!`ApbxD1L7PkK4)Oa5&`>|i2)Rp*(?l2PZ(<$LH%8kS}tf4vWANx
zOBAXK63yndj0}bHAQWE1$Pf%J+5O-}3Alv`F7lCE<O~cN;HESvQ3c6>(mAMk4Q`8r
z%a#&O1_lODk1a7dC$$*d#VRf=O--#z4Fc%{x8HLz^AdB?!9AwD{G!|-kQT5+Vo`E(
zesK^eO9X+778Q^Mpd1D+j}nt0UBcv|{9*_zv8X7q5^Q;XX-R5P2^RwcLtbKDd`W(M
z9<1;JHO@3Zi5^@~f%8QHxYGo4C0HmQQeJAq3dMqqOt1q}KnzgTRRmTR1QskPOUwZ|
zk^xN6AoGGMQJC}M6U*{5Qxfx%QzJpS8B~riFp4q>F!3`AF$ytRF*!48Fo`jWF~%~=
zvlub*GATexKv2;R!r*+~1S$a;7_u0^34jqa$O6jVpvsAnK|~zXCxb|VJ9_9+%wSzC
z4A44{6{<ZRlFitm+$@l33=HusP}Ma|3=%br;OZ})160?-8o!|6)Br^~B+NnmgCI~N
zEC}3P10|Cna6JP`S&$MG+;IbW1`>y$3Mi!n9EoMAIq?NWsb%1p024Hb4p6(HxFofp
z1QY>0pr#^|BBL}jCnG<j7^EEsay}@Xf&=>iGk6Rq3p^ZE!U$~)G&6z2j}g?RPGMq@
z1cf=c9ROm0>mJaE7z0Bq6R3zuV`7km#uhkAAdL=?QgGFTP|D1JTPYjJ>J(0hUF_hd
z2`9*KQ2erkV~GniUR1)((9FyTD%Cj{vUtGays$b?w1yd61c7Yk1Dn7PCI!IC_&`M?
zsDNSt3kre?^BV9#o@6>B$YGFzJzfY@cOu*jZh5sbffPuBiYf45Ajsd~aXYAl1gPZ-
z?ii+k0|n$ws1S-WQBYOG!H^{gDl<WCFk!IMkU}ku2~-lMfSS^bwX7gzIGn-?N~I9n
zKq{C)aRIWHc&D(z!U4j-?-Ym{i~wf>2RNv1;$X;vnE;A1E>M6$LsW#J!k4iGQl5kB
zY?oLYMo=9EX)v;ZDi=@^1N8?u7$iZ}LJAwWdR@d+Si}SxRe+=b43#3aoZtXtXGmdZ
z5Uv5sgF*uy3E&p27+4Kh5)_4?nh+L?;1Cm$0J&HktcsmMq=psbAXH<-Yq=OIS{R_7
z-~uHjm)K&4T5hl=35FVWh8k{A^;pXTsu8m!8ESYKYPc9gvZO$2Yk84GIpLyw44^t5
zoG?>3z#g8=TxiY=@-Qz$4Ij8T0@aorAj8v`7;Fm7nZpZ<n8HEsl?D}MjNrZxs49Up
zt|1j6XcQV;;-u!K6y#^-m4Iu4#EQ(~^p6>jJGTCKVh64)(|g};UGV+b20L(L5205B
zTv>uDqae`ecaR>aY6DpkBmrW9N)<?B8`R;3w8udON={~8aY14-q|FYtGNh<9H3(cW
zf(t}JkZu!DwFN4rQZm!hO1MGen)!Lji6yCdi6yC!E*aQ*0gyUS&03V0m!1k~(t`?9
zNIwBoe1dDakRVx5>z*HEG^hgyuAD35Q&MyD!ORi}6I6tk#3w^WP{Fo<>-)+Sh&0Ig
zl_^jT$kO8Y;?%+*RgfD%&4;x3^u)|OaG0jWmn0UYr<Q<i&jMHI1;v^1S>S3v5853`
zNzDVfkY<%GC^*wHOX@*|ycM{RmjI2OGHNqwGx9PkGAc4kGs-i^G3qkLG3qkQG0HKD
zF{&`KG0HGyGKw(@F@ow~Ax1Vvd1gNia6p3sAU-}XF*h|n9^5pEkI&6dDa}cZj}MXp
zg)JzQA$<oq5LX^Vfa>=kMG#8~M1Z<f;I2*(xSDhXiMfCXP-6fQ9AFmpYrY^*Fo5E!
z8020yMo}eB4jxtz<YeY#=3(RE;9=#I=Hv!j5AKhG<1Vi>x1bW7hQS6vWI;xOS=4tX
TI1SlA!qyHHjm4lu&BF`;N@MEc
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/dmp/temporal_coupling.py b/dmp/motion_planning/dmp/temporal_coupling.py
new file mode 100644
index 0000000..5d9107a
--- /dev/null
+++ b/dmp/motion_planning/dmp/temporal_coupling.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class NoTC:
+ def update(self, dmp, dt):
+ return 0
+
+
+class TCVelAccConstrained:
+
+ def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
+ self.gamma_nominal = gamma_nominal
+ self.gamma_a = gamma_a
+ self.eps = eps
+ self.v_max = v_max.reshape((len(v_max), 1))
+ self.a_max = a_max.reshape((len(a_max), 1))
+
+ def generate_matrices(self, dmp, dt):
+ A = np.vstack((-dmp.z(), dmp.z()))
+ B = np.vstack((-self.a_max, -self.a_max))
+ C = np.vstack((dmp.h(), -dmp.h()))
+ D = np.vstack((-self.v_max, -self.v_max))
+ x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
+ A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
+ C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
+ return A, B, C, D, A_next, C_next
+
+ def update(self, dmp, dt):
+
+ A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
+
+ # Acceleration bounds
+ i = np.squeeze(A < 0)
+ if i.any():
+ taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_min_a = -np.inf
+ i = np.squeeze(A > 0)
+ if i.any():
+ taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_max_a = np.inf
+ # Velocity bounds
+ i = range(len(A_next))
+ tau_min_v = np.max(-A_next[i] / D[i])
+ taud_min_v = (tau_min_v - dmp.tau) / dt
+ # Feasibility bounds
+ ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
+ jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
+ tau_min_f = -np.inf
+ for i in ii:
+ for j in jj:
+ num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
+ if num > 0:
+ den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
+ tmp = np.sqrt(num / den)
+ if tmp > tau_min_f:
+ tau_min_f = tmp
+ taud_min_f = (tau_min_f - dmp.tau) / dt
+ # Nominal bound
+ taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
+
+ taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
+
+ # Base update law
+ ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
+ if self.gamma_a > 0:
+ pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ else:
+ pot_a = 0
+ #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
+
+ # Saturate
+ taud = np.min((taud, taud_max_a))
+ taud = np.max((taud, taud_min))
+
+ return taud
diff --git a/dmp/motion_planning/dmp/temporal_coupling.pyc b/dmp/motion_planning/dmp/temporal_coupling.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ad105c2dfe090b880329d78c3dd0cb0fe173a9ed
GIT binary patch
literal 3407
zcmZSn%*)lJRve$q00qnp3=9qo3=G9)3=9k@3=AoZ3^@!8QH%^JObjV(3@OYEEes6J
z3=EM`ObjV33@PjkDXefIW{~n=4GsndhRpx}|Nr+ZVPRlk@XHTzhMUjGz`)=HGGCB^
zfgu%QG6TqfC?<xWU=5ITGRO@e4B|M0)PhV&VPpu_U;-JJ3393im;k8)+fkgFlU4%a
zrQ{ZrfK;WFKvaN~6-P5LFz9FG=cei><|Jk2=_lu>Wag#oI|q3N$CnrD7Z)Y#=jNAW
z=I6y1<Rs?hf#k4>rQ{aqm!#$v<QFC8#3$#M7UX2+rRx<`mas7}Fq9UgB$lMIgPaSp
zpMg;W<eU;vSj5NYCFZ8a$Cq$1Ffhc&=jNxB=A_2Q2Z6kWa3z>UYo`W*!UN>oVvr-*
z7)yj17#KpF!%}k`larnE^NLG~5;OBsQ{eFhQU;DMHRSk8VPt4wWB?@rCUD#_LlZ_6
z3n&sn{!0I|^5Z4nqyYP5kZm9g3J!2=f?}hFk)eixA)bMuh6&7IWT;^UGng1^m>IH|
z8A@0{jAlkqib`W*Xojd~WvF3+$+Cb1Qy3W#vTUFP3ri%R#G?U<juKu528Q&++}y<Y
zy!_nEyu_RmkT66f5u9jJ3yQ&FMXALZi3O=(v7FSr5>^HVhO+qF#0oGg5ya8}c_j#B
zcn~ORgE$!&7=l1H1o44bTnr2h;D`hhw2n<^>dMT^EQyZ?IieV3KLaBt7|Jn%a}r1h
z<Y;i%b1*V6lrS)4F*4LJGGsA<axyoZ1qsPoW|#^FP*8K%Kq#mr3rv!cA&Uj19i$t~
z;D)MTg{fd*$YNyxYiDJENwR_Mg^=t}HYjcx8L~JSL~EEBM8s>^VFrL*Si=sAXQ(3~
z9H>PcNa|P^AW{(5K-e%lIYB9@h6S9$*cfU!7-~2f(%2Xvg^iyECj$cmxX8#W0B7W~
z;*!MV>=IBESAioH%*+5sGMHHbCLzU5Nn)u6D1ad;0+d;TK<ON;!Vye5fk|gD=>oQ(
zB0ev*q6ExzL@=GfOmMOR6EsgdpaM8OH7~U&u_QG<H?gEBGdZ<b1XO+rfC>plB}Q2=
zR$<g&)PN)<X$A%cP;h~hk{dGvD92_ofTFR6iJ_T^A%=~imYJcJg`t*}p_YxImYt!N
zgP{Zxjo`W=n*kJ=wVa?R$YKU1pQ3WG0v1rluHj^m1Vtb#sIn0j0hI`x43gs9HOwIO
zlA_Iw47FSgd0b!}Yz*AB5Ei(|NMU5i2A2pZhF<|IWhd5fZiYNgunsncT5g6C4u%p=
zXu1Mf4Uqz;G>}WVYuFIZ;$f)a0fj>rD}zK06N6|iFW4L|6ceB!!44|ZAR!@H%ST8Z
z*i(|V{9tt;PYW<qY+|V4V{nOG#Zb%1P_dAqhM&PDb|youAT)7HmT-fdT_eaK+04iw
z0!s0M3@|}Xh@g0_5JQa+*d;6sMO};_Ei7RF)UY#vbc!@HGC&0d86-i1oD7m6LGfB)
zuv>UQ`fG$4z|kVYP$R-nBLIqnB4wr;5r$d;hC*eg!Y;<bF2?Y}TE_4i0hI6&1@m~p
zBrMFtz~-_r)NnD>@G{hhGSrAMq=EfkD-Kr32TC!B6ap%l_!&e*YbC%5Q2^$f4@@8#
z0kCO;3^fwq<OB+QAy731<x4=+fJk8m5s<71II<)_DKv!{TCIpQGl4xQQY*=j#|E;X
zRuXI(n)^i{uBwp)6<|_eo7ov^q!?<rp$64L1X;j>;t0VSDTZJTP=)CSZq{UiijYiD
z*`px_s!!xV1Smg(>!;$v($v%{Nd1(UR|&4CauX|pKuW=!%sfc3l$!~uyowU@(o=(Y
zKqVZg3P=R=!73AziXjcB!lDu|qa?A^APCeN0GD~-dJU|wxHK1RcWz=uW^QS25U8{R
zYsk+_Ee6+t8lYkUtpE%X2RU2<L`Z@NDG(t8B4k010eLVJ+#D)NEKP~e&CH8WMB*k^
z#3z=33YwC{Qm|whl4NFHd>OdB&ddZ;Sy?Db)4(Nd7TE21rMX}_B{dIBm*f_p8kAN7
z4qKQTVC@4?%?@)I*z=VsDe*~(Mc`&YL4HYmA~*y<N;E)aC77Uffemghg3?2=3&>6;
z6=qRJ9!4W3E=CB=$t1)m!zjoEDy_K~c^P>b^%!{=^%#X2c^E|)1(`&cq?zRy`57aa
z*qD@<MHxkzG(hc}ATR++9YOq{_yO4fwhHVi1VMA}ff{Appy~(Y7B(gxc1|vY?cgi|
t4*tB-+=9v=P_%*DvOyqiNa{dALu;pkn_V`LX1pD!x-SN0U>;^+Rsh6yumk`A
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/online_traj_scaling/__init__.py b/dmp/motion_planning/online_traj_scaling/__init__.py
new file mode 100644
index 0000000..1d15623
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/__init__.py
@@ -0,0 +1,2 @@
+from gmr_oa import GMRwOA
+from trajectory_generator import TrajectoryGenerator
\ No newline at end of file
diff --git a/dmp/motion_planning/online_traj_scaling/__init__.pyc b/dmp/motion_planning/online_traj_scaling/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..12e3377806349a16333dbb0e3992de92b9d20efc
GIT binary patch
literal 315
zcmZSn%*$nCbSXZW0ScHH7#JKF7#NCG7#J8*7#LC*8FCmHav2$-7#SErd?tn*CWc&Q
zhA3tR#uR3TU=0=q28PW4|NsBjU}RumC}Cq@U~u;hD))DU@P!!|7($8?vr>~w@{20n
zQ}a@b5=-)n{J{Fas?&3e;`0-OKn^Go0T}^T9iI+U9t1L51Ed5&6elq-Fz9FG=cei>
z<|Jk2=_lu>Wag#oI|q3N$CnrD7Z)Y#=jNAW=I6y1<Rs?hf#k4><>%#O=B37$6eVWG
q7bhp?fYimuXXa&=#K-FuRF-fsFfiEU=BJeAq}qX;Q49(r0Y(50Mo>`z
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc b/dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1833879cf8567e520fb4534cbd3be33e2a81c347
GIT binary patch
literal 11530
zcmXr!<>k^-y&V74iGkrU0}^0nU|?`yU|=YgVqjoMVMt-jVTfXMXGmd6VQyhaVQyxM
zVoG5QX3%7L3DV@33?g9|t~Y~$fgzP4iZO)&q&kJEg&~SLg*k<#g&~S1g*An(g&~SH
zg*}C%g&~S9g)@b#g&~SPg*%0(g&~R~l{1Amg|CI7nK6nhg+E20g&~SNMKDFEg&~S3
zML0#Ig&~SJMKndMg&~SBMLd{6Q{onzyKhjrzhg2J)FB{ua4;}1ID`C=#K^!<!jQ#S
z!<fP-$<WML!w}C@!<fP($xy=(&s@Wh#gxTT!w}C3Rl^Kb!&bwX!U9&q4wYer%5XsW
zY*0QYl+O<3bJZ|pae&R_E@8;xsbR?Cu3^Yx1>rPiu>HKq;%Q7^F}{i&CHx?@&5SM#
zv8!sCY8bM(YCt4w3PUSX3Ij-gD^nV?Btw=!2}71(mJmb*Oa?}SR5UY!%*^7gVM<|u
z=mwE#%r+oXI1#3%fkOdP9mtLvhAd8yeyF`nAm2%V^z(s4!R9g9FcfYGXGmcPX3*sD
zyTw#taErA#J~c1p7IS`lGKeU;#a2+1nO{_Viz&DC7He^4dTyeo@GYjif?KSKMMa5~
zMWDDUVqsulxW!VOS(SQ=F|UY~fq|ij4Mebm2vAHGae`EE<awtS<)!8n-(sywEy^#x
z#prm8G3gd-QDR<t>MiD!{E}O2IhlEhIqA2UGxN$cnQyTar{<*HVk<~2O3W?3#aO<Q
zp-7T}f#Fwzenx(7s(xZlQf8iha(+r?Ub?<>kY{jwd9i+RQL=t+eo1D2UVK4LVqP9d
z4x3nhUQT9SYJ5piVpe={a$*ihU3zX&e14){LFFxu`1s7c%#!$ckROWWKq-?^fRT%l
z3rq?yf*=>81fvL}0HYG431byMBoXVu^ubdzC~!a+6jH@*3=9k%3|R~vOwEj)j3ta&
zOdSka%*~9oOeHMs4DF2VOliz1j44bl93`wB3|VZT)C5Wmwagt1S?nNPSsXRY&7d62
z?3c_4aux#v11RHw+-VHTFd!>x7-H>e89Nwj7$w2h)i9zaBuR!WMv!403|XMe1Wsh2
z#3arj!Vt#5$WX`xP85uOn#@H4phy%35rPa144O>0nDh*aBtT5&A~8_3vX`f3re~BC
z-(su)$4(Kb==sHIlb)KFT2WABCk=8H$cZcrRnpK<O4GB+$xlwqDYnyttJCBxk_B1A
z1tR!C1TV;V=Hk-aA|Vi$55yJB%}*)KNd(7lZemGMW<?Rmahfb(#}t8L>K0>K6e}ny
z)4=Z0gt!x8I7lAsz#=6E1_qEfia`mKfsu=mhf#sCiUa0Ac>IA}4hm>+X#|SM8c^sn
z`a#TQLRbqHfDoV%xW$+bvaA@Sp8;WTI*P#{NwC2@4A5eZA&Utdz2LA11;s7q)XLN%
zP*7;%v5YCbLJ=0u42)HrP;aDHz^zkdWMJTgm8!c085kfXYAq-MFiA4hGS@JtFpD$P
zveYo8u!u9%vevMqunIHOvemGHY4#fC6gCNlT8<i~6m}7YTFx5g6b=!FTCN(-8ZL2$
z6ix|-TJ9RA6fSXwTAmuF6mAiQTHYFFuv)$v-Won}h7=xghFbm_jvA1hKn+g~zX(IE
zV2uDbLye#WL#+_FBo=CBtQD?dPT`ecs1>PUO5qb>s1>bYP64SEs}ZdclVC^@5ND_r
zuMw#c7h$NCs9{PG6lbWFtYJzK5@D#7s$mA}k*<-dk(OXc5f*2tm8p@ekr832m93Ge
zk(FSml>^%(*UVTeU&E9lBF<2&P{WiWD#B2!Si_tmCc;pwRHIm<B+if`F3wP^T*I6q
zA<j^%Qo{t6Q>|f6krZL5RjX00Q4?oKkrHR9Rj*N~Q5IpS)u>UaQ5RvT)vVFrW~kAW
zV5rps+ojdaSgT#5ogyv5P^(kJlp-U+P^(+RoFXg2P^(wNlp-g>P^({~SEDb%kRmV6
zP-{@5TVo)?P-|GjoT4DkP-|4foT4bgP-|Sn1lD6xV_ah*!H}XP&QNPwV^m`*!cc2g
zV_0J*!BA@sw$Z$qvDTu-A_e3s%NnK>6$yq~s~V;hRS||->l)@1H4%ndn;PpH8*zpd
zbqR)A+ZyH+4GD%?yBa32oP7;*ilzudtwW7{je|Htik1XJtz(T<jjaenty7I%jiU%d
zt#geNH$#oH1VgP0*iM&b##+}J*A#6LhFZ58<`f-qhFbR;rW9QfhFXsr<`g{<hFZ@W
zj~Y*Lh7^5qhFY&0rW6BlhFb3$rW8XFhFYH*X0Te{8lM_pafTElafVvI8uuD65r$g-
z8t)oE5r*1;8h>tvng9ug+CZ?Kfz6DyK{Y`s#v%;0!8J@NCK3#_AvH`Xpb!bIVNNj<
zVW<tO39SheXGk%ZV5kkRVNS6SXQ+*+VFJrV)-b17iZIkh)kM}ri8G{FNift#*M!uB
zi!jv2)I`)oi!jv2*2Hi#)Wk|K)W(7BjB94BjjxGMu@+&dO{fu0u@PaYO{|em0fkLc
zjZTfFI75w_I73aaI73Z>I73aMI73a6I74l6id{{zG(&Al4O5D}2t#dZ4ReZv2t#dJ
zO=?Y=I75n~1Ve3l4O5Df2t#c~4ReaK2t#dVO-4<oI74k#4O5DX2t#dl4ReaC2t#d7
zO?FL=I75n?1Ve3Z4ReaS2t#dN4O5DT2t#dtO<qmD1Vf6aI74kg4O0pz{0eKBQ@lkO
zYKv+LYl<WoQhdZ2YKv=_QhY@iYD;REQ$S%`T2oR}D$bDNFTqe-RwG|iEW!ZxQ+Z8%
zO}YpJ*uNDuOep~(47HUt%qf8)47F7?l{Ho33@Jh447JrY!Zp<*4B+snsbNY97GbEZ
ztzk|H5n-sUtEsK26K6;X6=$feuhFil7hwSBhK3r~np_bEaGGhXVM+-TVW@4YVNL;s
ze{)S!O|v*dN`yE=ZA(p1O^XNvIG$T;ENTiw7{F<}t){%DLY$$dMx3FhL7btcQJkTs
zRh*%wO@g7e9TeW6(o345wxfnAB~pZ;wzGygB}#;$wyUPIrb~jMw!4NY1r*0UHOwh7
zA`G>?H9a-G;taKYHB2e7A`G?tHOwh-A`G<?YWiy?h%?kqtYJ!t7h$NKRKuK-Ai_{P
zxn@$$WO0U+L<xr4DK$(fNg@ojQ)`%0l0_J5r`1fYnI_IqJH3V}B}Igxc1F#NlvHts
z+L<+MHSFRHwX;&}YGy&*H@k+lW~K;3?Hq_)%^V4a+PO7MDQO}MwexD2Q_@8kYUkI?
ztC=ssP`jXpDJ4UMp>|;nb4sQNL+zrPg*A&L7*evt8EO~TFr{RRFw`!oVNS^rVW?eN
zv!rIJI798S8m5$75r*33HOwh_A`G=FYL?fmkYK1?S;L%?FTzl}s)i}0K!l-ob<L`p
z)e;OTiQ){kYigKM3Pl)d*VZtn6p1j@uB%yFvrdAcc6|+VO0ft-?S>krloAn!+Kn|E
zYBowRq?C#?)NTU%sZ5wbnxS@cjc`qh2t&;RXqwnk;|k_4hQ{~S8hJ2(88n@3s|f=0
zS3=`?dyNH{zaARbJ8I&={58<DxU)tZ%-;k}OS@{e)NB=JsM#jYP_tc}p=O6TL(NVJ
zhMHXx47Iy!X4lLWVMr+#VW{0xvnRz)oS}Aa4HGz>?W<u<sSshP-Cwh>X1_Q?N~JhM
z?SUF5a6UU&!<<qj!ccpt=3vbsafXy?35MFkH9<88pl&=;!vxO5M{Af<YD5@nkJTKl
zIVR4KQY*nwd%PyT<~Y=?Cu*3$`R`;6b4r~EL+z=WlQpL#7*guR8EQ}0FsC$#Fw~x@
zVFKslvo&XG&Pp((G>S9So~yB_IS2Ls`5Gp0Ub;}joYEx1P<yfFLd`{ShLmP;hT2Ot
z+BKJ;;c&UewdOE1jIY!%f%ECr8s?N15r*1pHCJn{i8G|MN-)%3uMw`f4vm`|HS#s5
zq499D=19#6afX`n;tVyH#Tja@h%?mO5ND{lDZx;CtA-hzmT%YGPH7WosJ#P?(=4cc
zccF1q2KDznXdL!I{cs-|XA_}reE^N~DbVnI2u(B7q3(JFO(We<w>_@8TXRpGq2|6g
zL(KzmhMI@s3^k9$8EPI&Fw{P&nOQR%8pls-SZeNwFw{PS=BH;847JZ8W~a1^Fx0+)
zn48ie!chAX;--{N5r*1V5cj5Zi7?c@hWIU|TZEzZ4aA)(Jt7RXZy|n6=@nt9eODt~
z(=NhL(}9%E-`BXp#b9auLybIK43^$M)&#-DU}^pn#C}luRs&1-pCRr8m6<iLwEm^W
z0<IUP?kmJypt8CKmPfwTyr_98&QSA8oT28mI77`FafX_=;tVzK#2ISdi!;=G5ND|Q
zD9%vxNt~hPvp7S|7jcG~ui^|f-^3Xpp#>_@VL4cW0TODUnxO{j3ke2DD1l1x8mMn2
z7$BhqDzR&zK9gXm(T2Mi>I(^mns~Sv)Ws4EwcjB&r}T+1)c%0@J*8iSq4p;vY*HqO
zFx38n*qSm?grW8~#I}@4A`G>EAYqU)S%jhXZ;fkBmIwpP25=qo4`K#5d|_$wKa#JY
z>1+aHEy!(UNV=^>7$z{*G9raLRR09VS|%jFK~wew##&~i_=TqL35>NYNGS%I$|o?^
zvex{l`6<p&^Glqe=C?RQ%^z`wn!n-<HUGpJYW|BeOkk{G0F#Vhk_k*QgGm-J$tn(w
zo0KUc4DdJs`5s!kN+8E4)LwA-gG%Uh5r!IQjs?dPNUUCjp$6&~aJ<7(IW&Y&;uz{X
z2~Y^J)y%GW4#|ZR7;D)lFxIexO4|vHwH(lRoeB;~PH4<d1M|2b@sctf%;Sc}?hG)G
z2O7UK!8~4Q49^1d_#i4lDGyfWO<=6$uh9nQ$W0;)HMbxo%>>3;0f;%E6bUQ8Cot9u
zLR<w(<FL|j0%NTZ#66%q2rGXlfO;nJU~|_%&4r{1kqL}7oD	{h$eqHQeC%-~p4o
zV3H3^@`FhMFewNog}|gRm=uA>-)wOF!D~iXsSJ-7ke{F>JvdIGB@fu2HekK*S`t?K
zfa4FOZW+|wwqSJ@V85(~iYbG|T)|?Cp<>!#u^_P6N~oAINNfUQt!T|`PzejFQ716g
zia|m#Wezx2#34B<WiFT}0gdT-V4fr-WK-sYc~X%0OIZNsNkd{NWg(a+1C8lLV4iGE
zJSdHV-2#eJNZ8BOKzz{$b)7O;tvodTCqhM~!J-N^5T8teiW-AO6>EgSW=@BS+JHrs
zpk)Ur)IhdrgGH4mFxH4qV62e<$C)IUlme5|U{VH5%7RHbFewiv6~Lq-m{bCj%Hptc
z6O>1xwKX`NKzXlQgaIkuA+0Eo`=GToJS~9yhe!*M))FWrpe+V?{sxs}(3S{1--1E`
z+KK_&3-T$*UXX9W>0<(8tx5@Kh-fc2sQh6QVW@dh^R#9rs2x?aS%e{Fu?Pcb$Vi-_
zh6Pmb*6fCeNr3n>MHs+h-ytOdL=4o{6=4ABg{c*1sQEsDv4%s0p=LMO?9Cz!;L>9;
zD3yWChorKaClGVQ8ERO;=H3CtM-3a2D9A>T?mHlN!d(J!y9!8b0%MI>&0MhUcVO}$
zTOh3lu#3vUd=^kk9AwJ`#u`z$4G>X~4ZB4cz$qW>e~|0J;Ufz21K5tuAX%6%Kp`Xn
zb2Z2pU|lG_P=VXQ3UM7YoS>>D7}7!G?yTudDNCA}gBdiJ`W1nOq+WuEbnqZxL!bkM
z!4T>25;UrmuKp~<C8I0CA-(uT+=Z1qAMDZ<%lS2<mTB9kOQx6p4>6o#pWgdCeBW&4
z0EhID&&KtC*zN4o(@a(~emyY9KHdA>w-PZCZ-?|P!p{HN+F#qHCm&|jJ!NHRpFaPS
z^&ta^3HIs5#cR4AXva9Dt4h1PF!S`bPycE=M_tflhkd%6ndzJS7Bh$RE5~17v7BvZ
zpB|8ra7Inm#y&mHE1!9#-v;}1j;yRJegEPd(!c$+P%7sMv`>E`?ZS6;|3UlouMLOu
zT%3N|rOVAOEb81J<dEJMAAF?zv#fo3$<*raf6jQ>r{A32rPw%QlYP1XYaGj(-4PDy
zxsOH9EuZLQpZ<JW@&DD`8|>5X?2yzt=kd=j{r|FF>HSO09MXgI3JSkwCfTPS{~NMD
zFE-Ua{kzfB9}fI$?9*3APgtMv?Yn)thTQoL;+9kG)2Dqq=};=L-9BBqF`u<|y`g=2
zi(_Z;q(*;-bdUd06Iw0&?9-JhlEV4xJnhqOS8qEVaps79`otn3mGcMV9MXS&crLVB
zGQmE5=c$<J?S_}_(`(*HJz|^l&o13`7k8=b@?eMbv#EQha`R=|r?YVC$v;(2vrqT@
zE0%gX@U(sUMAsXqUW*wzq+b!`|9I3n$39*E@4uS;OAp(pi<(B}@rcUUr;D|1pSAM*
zHM{ih0e|k)da~F<Q^5!O^mAa7t6?UW!A!nwpYG7uURLsz&msNB*6sgOj8$O4QUnW@
zQv39;;9%Kf4-I);`}8t!$Vb_y-vEdFA^UV?aJ)y`r*8)*lHYdepTP;>gnhb2rbBY<
z-v9RLrX}+huyk@dr2qR50{ph=q2R>kXP;gIPMHk$=_%k;x5gftTKr(C#m7E99-LYZ
z*r)#j2WEkNx*Is;<LuLSfJ61MeL9b1?y<g)4EE_ePqR(uxA<kBzO7Dh<(_Rn?9(Tw
z7788mV0K9VqkXc5>%uep^qnG&yK|kH9MavqSNxk?d)Yqy(wQ)oH!+N`45VP2ey-3q
zI5_92eY!z&+nc@_3@|A;Bhw+B>-fh<?xCOT)19{o-0#kKZ=Wu*`Lyqwt_Sw%!5el5
zD!vql={jJOKEe3Ou87`W_UV4p!ehUDd1Rk%^o&nrsx^~C`ex>Lz2?6r+o!+$FiWB1
z5wk;j?H9ZH6%XFpr}u9ElUiTG?2w+Y^7gTsgF*J`3+<x=c|T>@r%Ui#K2fTLyV=C{
zC1`#D68Q=}4(ZC^R2gZXeg~W?ufP)ZQdpusZVydXHTLPc;AC~#9-74+VOe~EeR>W!
zO9k1dzXWHg1NP}(z?n7CKK&UuvtG1^mMhk<a%BlL4K^e?r0@Ey)L^1~%Pw7G<BqVP
zs{;1vSMKFBex1#2pMHz~{L`vTA^UV$RfBWfo=W!VmN)uU4;2X7r%wnmthBO_vQJlk
zd{O>?*Ezd%{V;8TV~n5d($_KHx9?77w@<f<{-~}i6KJ2#=~kt9c*!HX^k?s<2TZ!9
zZJ)k*&%!%Lt3~b7#nMz>ZA!jom#!_a!s)q7sa<;6l3Y!}1BrI&S=u`#zT`&Pr*Fvp
zs{coNv0eHa^`v_5Vnh4%&294fE}TE?();6dwX{|~w@W{AnemXrd;$A(McW12{$Js-
zPZw0#@>tYC$UZ&q|3MG!kTm=B-nFr^$7T!Ir`!Kze!M=z3>pXua~y6lx@j^OL5K1y
z3U09%r50x-7Nlx`M&uFG2e+6i3~w=27~Ntj0b%1PrV^7VrV`UArV_I#rV{fgrV@)N
zrV`62<`P2#5Mc-+j6j5O6my9=NWuavVFD5}1rcT-!aRz(!~(3&0>lNWGXkkI0vTWd
z(rW~gwE$@|0&~GEkO4*@1B^fh7=iQ}f%F=K^csWo8iTcgBrL!ZAic&Qy~ZHD#vr}M
zAib7g9UyhaAay1nt_jEhOAuiKGQ|=^n1Bp00U2NdGQb36fC<O|6OdjLkX}=eUQ>`Z
zQ;-f*kPb^Q0n%m)CO~>kL3&L=dQCxkO+k9iKzhwUdd)z3%|P1BKsqczIxImt%s?zN
zFbkyD45Zf#q}Lpz*BqqR9HiGAq}Lpz*Bqq75~SD?q}Uv!*Brz(2XQSxIxIj+EI<+#
zAPGy$D3%gK0|PK=2quldq%oK@0h6X+(hN+RgGmbzX$WD1O)vzTU<g)e2sXzMY>pw=
z97C`<hF}v6A$q~OjKI2#!0L^_>W#qajlk-Sz@{02bs2%}G6L&00_!yf>oo@JH3q9O
z2Ag0EHo+Kdf+?6Z2Ag9HHo+Kdf-%?x6R-&;U=vKhdQHH3O~ATLz`9JpCYylunt=71
zfc2V!^_qh9nu7IOfc2U}NU&a0ur5=u38rAZW)L>m1T(M+W?&P{z$Tc1^_qe8nt|P5
z1~$PQA_6f1tk)bYYYsNS9BhI)Sg$$Q9CNTO<`BJLUFKk27GU)jAoWJz5H^B@umxC`
z1=vgruw52l6D+_cSb$Bi0GnV5Ho+2Pf}tf?mnGO<OR!!`uwF~BUQ3W(BS;#s1e*X(
zX-43bU<6KYMg|~rj0`~L7#V=fF){#|V`Km_$H)L=j*$V_90QOEMg|}^7=cro5jb5L
zfm50hIMo?}Q<o7qO&WpIn2{k^JvfaSfzy}~IE@*B<JSnB(u}~V%Lp77M&LAN1Wsc{
z;523gPGd&kG-d=&T}I&4Wdu%LM&NX11WtQK;M8RVPF+Ue)MW%tT}I&4Wdu%LM&Q(C
z1Ws2*;B;jKPFF_YbY%oiS4QA;Wdu%FM&NX11Wr*#;FN4+3Jw`?iZTMHE+cTdGBO4G
z6r7ffz-h?{oR*BhsmKVNqKv?4&j_5NjLg8{X$IB>PDMuGRAdBBKStoRWCTuQM&Pt(
z1Wr*#;1p$K4%Q1!QAXetWduo4;8bJ;&ILx`v}6QMOGe<dWCTu2M&Pt$1WrpvmIjd7
zY0wf1@ErIpwv>wa^oleU(9CfVsMo;AfdP4-A`lw10s^82!UwS-EG`sv9E>ngm^e%X
zQwKr@w;tkUWYFA!un%GejO2m35Sd1nlYz>E^rK>kDwr6E2g1mF7#l*P%JP8Mj)2yW
zKrq4{h&p8Vfb?UEB1Cu?A-;u}1S26Pg3Lk2B8;eT3nRL$7$X9nV&r4wVdO)C2stsv
zZ%E=0HOR6b7+DzOnBtg{n9>-lc%ds1ASn&L8UeIO2-GA3FMcRNS^QATR1sDJUfBg&
z{Sxa@3u3d>FiSFk)-u&F)-YtT)i8tCOR={yftIa+R%{_e;4+}KV9X*6HB2?kHVlOl
z;S4oQS)9$_6%I8l(x6pOjDDJ2;Fehtc)bZ|feB=(31~IVE%w~9y!e8Yv?9<75%3BO
z@H(a<1<>LbR-~32b6#mKcuCAH=9JVtv`hwGcM`<Nz)%eGF9Rbe{yi8u7_0c8Ap>1L
zlMHIWgZlEIH6|eY!9f$jzyR_#LoIU(c)3stQwd`h$WzS13?+<NELp5IOwEiU49$$v
zAU<0POASK}qXa`1dox282UrCsNJSP0L>#o32&7sB<b77ZA~n#$8g&K+21wU|;Q_SW
z%J5*nCRY(?RSGzKZm|>>7M0v$D#$Dn0j<JePDw2((gLwKONtWniVO0KQ^D(SibO$y
z!vbE1Qv?n-(3%rS;DO5>FacU0Q)I}%zz_)v7tmTU1|~K}HAI*|Lmr-7Kw*O#CZJ@%
zAi+=z3J<0l@FJ=jhAfsErYzQGPy%5Bg-8mcFeuXDBA^9V5Vax<;P7OxVX9$jW&#cR
zfQFia88n&via^1k$pKl%28y#HQ27Y)0?4UF=AbYGEjqfznqOLyTBHeGU<6w3bBn1W
z1w8>MGB7a2!#&E##c07;B>|0mjMYqVFN2C>P*MPS9lQXogaNcdER6}eK8z8xcng$6
zKv5dZpoz8~43xo)LFKv;I0eOO)iQ#XE@UymSA{WyR)<M2G=mIck!DC?5QeS&0xfW2
z0tbViCM!7pA+gSqmtT|%2^Ua6XfhSafRX@XCg#c{a16_X){%ld4)IHsJk-<aiMhFn
z*j9isgYy<73u0aIgkllMj$5o?!%|?jFfdl}AlrfwoZwsqT27Y2m;z2iHH@H?A_2+|
zOpyGb$yj6qO2v4VTXEzh<|d^i#uud{SqxhO2$Lh$ZgKMLPR+t?cdDNzKWGhOUSe))
zeEco0_;_$a1F?DH;|og@b09KBpv8IMrEo>cpj532A`CzTXer(;p5l_klFa1X)RK(+
z6mW(u0wrc}@D$mCl-Yp@aGpjGpm-?)ML8&ufEOZjFmecTuyHVRFmu#1)H4`xu=B8S
ru!8ttb(+k#So2DA3o0RL4Wtz*4}lE^nR$!D2I31laGn4u<6s5=gQAK;
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/online_traj_scaling/example.yaml b/dmp/motion_planning/online_traj_scaling/example.yaml
new file mode 100644
index 0000000..94f5fb3
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/example.yaml
@@ -0,0 +1,38 @@
+update_rate: 125
+x0: [-29.1, 114.9]
+s_end: 3.5
+priors: [0.2599, 0.1352, 0.1344, 0.1377, 0.0550, 0.1664, 0.1113]
+mu: [[-112.6856, 54.1569, -88.7499, -154.9200], [74.9928, -98.9324, 122.6443, 182.4869],
+ [22.6435, 1.4977, -102.7541, 2.7533], [-22.5173, -123.9775, 299.0837, -48.8174],
+ [-64.5564, 98.1626, -160.4187, -170.3178], [78.6112, -17.0895, -101.3953, 115.8752],
+ [-123.0283, -63.0123, 174.2271, -279.1635]]
+sigma: [[[0.6009, 0.0300, -0.0491, 1.0150],
+ [0.0300, 1.9033, -3.1190, 1.1773],
+ [-0.0491, -3.1190, 5.9745, -2.1668],
+ [1.0150, 1.1773, -2.1668, 3.1809]],
+ [[0.6290, 0.1268, -1.2774, 1.5296],
+ [0.1268, 0.9079, -2.3844, 0.3074],
+ [-1.2774, -2.3844, 8.2566, -3.0188],
+ [1.5296, 0.3074, -3.0188, 3.7767]],
+ [[0.2605, -0.0328, -0.6769, 0.3306],
+ [-0.0328, 0.0282, -0.1585, -0.1858],
+ [-0.6769, -0.1585, 7.9287, 1.6320],
+ [0.3306, -0.1858, 1.6320, 1.7752]],
+ [[1.7232, -0.7104, 0.3933, 3.7694],
+ [-0.7104, 0.7734, -1.2591, -1.5555],
+ [0.3933, -1.2591, 3.6391, 0.7968],
+ [3.7694, -1.5555, 0.7968, 8.5589]],
+ [[1.094, -0.079, -0.568, 1.331],
+ [-0.079, 2.376, -4.876, -2.423],
+ [-0.568, -4.876, 15.627, 9.517],
+ [1.331, -2.423, 9.517, 44.624]],
+ [[0.4217, -0.3304, 0.2178, 1.2149],
+ [-0.3304, 0.4037, -0.6332, -1.3119],
+ [0.2178, -0.6332, 2.2057, 2.3334],
+ [1.2149, -1.3119, 2.3334, 4.9441]],
+ [[0.5734, -0.6797, 1.3006, 1.320],
+ [-0.6797, 1.4485, -2.9368, -1.5774],
+ [1.3006, -2.9368, 6.0104, 2.9347],
+ [1.3200, -1.5774, 2.9347, 4.7359]]]
+o_c: [-100, -70]
+o_r: 30
\ No newline at end of file
diff --git a/dmp/motion_planning/online_traj_scaling/gmr_oa.py b/dmp/motion_planning/online_traj_scaling/gmr_oa.py
new file mode 100644
index 0000000..cc49b5a
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/gmr_oa.py
@@ -0,0 +1,363 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class GMRwOA:
+ def __init__(self, params):
+ self.x0 = np.array(params['x0'])
+ self.s_end = params['s_end']
+ self.n = self.x0.size
+ self.o_c = np.array(params['o_c'])
+ self.o_r = params['o_r']
+ self.priors = np.array(params['priors'])
+ self.mu = np.array(params['mu'])
+ self.sigma = np.array(params['sigma'])
+ self.nKernels = self.priors.size
+ self.A = np.zeros((self.nKernels, self.n, self.n))
+ self.b = np.zeros((self.nKernels, self.n))
+ for w in range(self.nKernels):
+ self.A[w, :, :] = self.sigma[w, self.n:, :self.n].dot(np.linalg.inv(
+ self.sigma[w, :self.n, :self.n]))
+ self.b[w, :] = self.mu[w, self.n:] - self.A[w, :, :].dot(self.mu[w, :self.n])
+
+ def f(self, x):
+ weights = self.weights(x)
+ f = np.zeros(self.n)
+ gmr_f = sum(weights[w] * (self.A[w, :, :].dot(x) + self.b[w, :]) for w in range(self.nKernels))
+ return self.modulation_matrix(x).dot(gmr_f)
+
+ def g(self, x):
+ return x
+
+ def gx(self, x):
+ return np.eye(self.n)
+
+ def dx_gxf(self, x):
+ x1 = x[0]
+ x2 = x[1]
+ t2 = x2 + 7.0e1
+ t3 = x1 + 1.0e2
+ t4 = t2 ** 2
+ t5 = t3 ** 2
+ t6 = x2 - 9.816261394778176e1
+ t7 = x1 * 1.530596366426007e-5
+ t8 = x2 * 2.109513339111852e-4
+ t9 = t7 + t8 - 1.971943673320373e-2
+ t10 = x1 + 6.455638083061622e1
+ t11 = x1 * 4.582396934010209e-4
+ t12 = x2 * 1.530596366426007e-5
+ t13 = t11 + t12 + 2.807982275662759e-2
+ t79 = t6 * t9
+ t80 = t10 * t13
+ t14 = -t79 - t80
+ t15 = np.exp(t14)
+ t16 = x2 - 5.415693532998247e1
+ t17 = x1 * 1.312704257151209e-5
+ t82 = x2 * 2.629035561972274e-4
+ t18 = t17 - t82 + 1.571727929602906e-2
+ t19 = t16 * t18
+ t20 = x1 + 1.126855799373714e2
+ t21 = x1 * 8.327024557111857e-4
+ t83 = x2 * 1.312704257151209e-5
+ t22 = t21 - t83 + 9.454447953270757e-2
+ t84 = t20 * t22
+ t23 = t19 - t84
+ t24 = np.exp(t23)
+ t25 = x1 + 2.251733280467149e1
+ t26 = x1 * 4.670158568124115e-4
+ t27 = x2 * 4.289836618923725e-4
+ t28 = t26 + t27 + 6.370025953155209e-2
+ t29 = x2 + 1.239774676360322e2
+ t30 = x1 * 4.289836618923725e-4
+ t31 = x2 * 1.040569631086783e-3
+ t32 = t30 + t31 + 1.386667556436968e-1
+ t88 = t25 * t28
+ t89 = t29 * t32
+ t33 = -t88 - t89
+ t34 = np.exp(t33)
+ t35 = t34 * 2.408872289627689e-5
+ t36 = x1 - 7.499276410192803e1
+ t37 = x2 * 1.142541742241694e-4
+ t90 = x1 * 8.178935231312593e-4
+ t38 = t37 - t90 + 7.263953274615121e-2
+ t39 = t36 * t38
+ t40 = x2 + 9.893237408811206e1
+ t41 = x2 * 5.666798935371871e-4
+ t91 = x1 * 1.142541742241694e-4
+ t42 = t41 - t91 + 6.463122355088623e-2
+ t92 = t40 * t42
+ t43 = t39 - t92
+ t44 = np.exp(t43)
+ t45 = t44 * 2.888145048134885e-5
+ t46 = x1 - 2.264354107101735e1
+ t47 = x1 * 2.247790700044474e-3
+ t48 = x2 * 2.608028701302809e-3
+ t49 = t47 + t48 - 5.480387720544926e-2
+ t50 = x2 - 1.497658429905395
+ t51 = x1 * 2.608028701302809e-3
+ t52 = x2 * 2.072560326248954e-2
+ t53 = t51 + t52 - 9.00948794532844e-2
+ t93 = t46 * t49
+ t94 = t50 * t53
+ t54 = -t93 - t94
+ t55 = np.exp(t54)
+ t56 = t55 * 2.698593356722665e-4
+ t57 = x2 + 6.301228183938932e1
+ t58 = x1 * 9.222652594971037e-4
+ t59 = x2 * 7.779669674794524e-4
+ t60 = t58 + t59 + 1.624861712161579e-1
+ t61 = x1 + 1.230282678775932e2
+ t62 = x1 * 1.965364578835002e-3
+ t63 = x2 * 9.222652594971037e-4
+ t64 = t62 + t63 + 2.999094383441548e-1
+ t95 = t57 * t60
+ t96 = t61 * t64
+ t65 = -t95 - t96
+ t66 = np.exp(t65)
+ t67 = t66 * 2.919147494282987e-5
+ t68 = x1 - 7.861117292542697e1
+ t69 = x1 * 3.302595736312175e-3
+ t70 = x2 * 2.702662948139369e-3
+ t71 = t69 + t70 - 2.134336703875113e-1
+ t72 = x2 + 1.708953540592262e1
+ t73 = x1 * 2.702662948139369e-3
+ t74 = x2 * 3.450285534628542e-3
+ t75 = t73 + t74 - 1.534957275707511e-1
+ t97 = t68 * t71
+ t98 = t72 * t75
+ t76 = -t97 - t98
+ t77 = np.exp(t76)
+ t78 = t77 * 1.071251714048381e-4
+ t81 = t15 * 5.43860953025742e-6
+ t85 = t24 * 3.869452814178776e-5
+ t86 = t35 + t45 + t56 + t67 + t78 + t81 + t85
+ t87 = 1.0 / t86
+ t99 = x1 * 6.605191472624351e-3
+ t100 = x2 * 5.405325896278738e-3
+ t101 = t99 + t100 - 4.268673407750226e-1
+ t102 = x1 * 3.869851041803628
+ t103 = x2 * 1.010037993543203e1
+ t104 = t102 + t103
+ t105 = x1 * 4.495581400088948e-3
+ t106 = x2 * 5.216057402605618e-3
+ t107 = t105 + t106 - 1.096077544108985e-1
+ t108 = x2 * 2.285083484483388e-4
+ t117 = x1 * 1.635787046262519e-3
+ t109 = t108 - t117 + 1.452790654923024e-1
+ t110 = x1 * 1.665404911422371e-3
+ t115 = x2 * 2.625408514302419e-5
+ t111 = t110 - t115 + 1.890889590654151e-1
+ t112 = x1 * 1.983731639577469
+ t113 = x2 * 3.191903357349043
+ t114 = t112 + t113 - 1.4210854715202e-14
+ t116 = t24 * t111 * 3.869452814178776e-5
+ t118 = t77 * t101 * 1.071251714048381e-4
+ t119 = x1 * 9.164793868020419e-4
+ t120 = x2 * 3.061192732852015e-5
+ t121 = t119 + t120 + 5.615964551325517e-2
+ t122 = t15 * t121 * 5.43860953025742e-6
+ t123 = x1 * 9.34031713624823e-4
+ t124 = x2 * 8.57967323784745e-4
+ t125 = t123 + t124 + 1.274005190631042e-1
+ t126 = t34 * t125 * 2.408872289627689e-5
+ t127 = t55 * t107 * 2.698593356722665e-4
+ t128 = x1 * 3.930729157670005e-3
+ t129 = x2 * 1.844530518994207e-3
+ t130 = t128 + t129 + 5.998188766883095e-1
+ t131 = t66 * t130 * 2.919147494282987e-5
+ t136 = t44 * t109 * 2.888145048134885e-5
+ t132 = t116 + t118 + t122 + t126 + t127 + t131 - t136
+ t133 = 1.0 / t86 ** 2
+ t134 = x1 * 3.231732335122654e-5
+ t154 = x2 * 1.638686742732473
+ t135 = t134 - t154
+ t137 = x1 * 1.54461351435013
+ t138 = x2 * 2.410526413045497
+ t139 = t137 + t138
+ t140 = x1 * 7.128956258032981e-1
+ t141 = x2 * 2.282924141766298
+ t142 = t140 + t141
+ t143 = x1 * 3.04539679230562e-1
+ t144 = x2 * 2.17037228002446
+ t145 = t143 + t144 - 2.842170943040401e-14
+ t146 = x1 * 6.694733262807462e-1
+ t147 = x2 * 2.074491355757708
+ t148 = t146 + t147
+ t149 = x1 * 2.0
+ t150 = t149 + 2.0e2
+ t151 = t4 + t5
+ t152 = 1.0 / t151 ** 2
+ t183 = t3 * t150
+ t153 = t4 + t5 - t183
+ t155 = x1 * 1.145820509435398
+ t157 = x2 * 9.81512008356454e-1
+ t156 = t155 - t157
+ t158 = x1 * 5.169583816001215e-1
+ t166 = x2 * 5.977666165428746
+ t159 = t158 - t166 + 4.440892098500626e-16
+ t160 = x1 * 1.659508447689607
+ t161 = x2 * 5.924016783798807e-1
+ t162 = t160 + t161
+ t163 = x1 * 2.279092938141525
+ t165 = x2 * 1.950918964167032e-2
+ t164 = t163 - t165
+ t167 = x2 * 2.48561120484202
+ t175 = x1 * 9.336740956565702e-1
+ t168 = t167 - t175 + 2.842170943040401e-14
+ t169 = x1 * 2.43188361814004
+ t174 = x2 * 1.144763805707237e-3
+ t170 = t169 - t174
+ t171 = x2 * 3.289247523618103e-3
+ t173 = x1 * 2.186101706496526
+ t172 = t171 - t173 + 7.105427357601002e-14
+ t176 = 1.0 / t151 ** 3
+ t177 = t15 * t87 * t156 * 5.43860953025742e-6
+ t178 = t55 * t87 * t159 * 2.698593356722665e-4
+ t179 = t24 * t87 * t162 * 3.869452814178776e-5
+ t180 = t66 * t87 * t164 * 2.919147494282987e-5
+ t181 = t44 * t87 * t170 * 2.888145048134885e-5
+ t224 = t77 * t87 * t168 * 1.071251714048381e-4
+ t225 = t34 * t87 * t172 * 2.408872289627689e-5
+ t182 = t177 + t178 + t179 + t180 + t181 - t224 - t225
+ t184 = t152 * t153 * 9.0e2
+ t185 = t184 + 1.0
+ t186 = x1 * 1.844530518994207e-3
+ t187 = x2 * 1.555933934958905e-3
+ t188 = t186 + t187 + 3.249723424323159e-1
+ t189 = x1 * 5.216057402605618e-3
+ t190 = x2 * 4.145120652497908e-2
+ t191 = t189 + t190 - 1.801897589065688e-1
+ t192 = t66 * t188 * 2.919147494282987e-5
+ t193 = x1 * 5.405325896278738e-3
+ t194 = x2 * 6.900571069257084e-3
+ t195 = t193 + t194 - 3.069914551415021e-1
+ t196 = t77 * t195 * 1.071251714048381e-4
+ t197 = x1 * 2.625408514302419e-5
+ t212 = x2 * 5.258071123944548e-4
+ t198 = t197 - t212 + 3.143455859205812e-2
+ t199 = x2 * 1.133359787074374e-3
+ t214 = x1 * 2.285083484483388e-4
+ t200 = t199 - t214 + 1.292624471017725e-1
+ t201 = t44 * t200 * 2.888145048134885e-5
+ t202 = x1 * 8.57967323784745e-4
+ t203 = x2 * 2.081139262173565e-3
+ t204 = t202 + t203 + 2.773335112873937e-1
+ t205 = t34 * t204 * 2.408872289627689e-5
+ t206 = t55 * t191 * 2.698593356722665e-4
+ t207 = x1 * 3.061192732852015e-5
+ t208 = x2 * 4.219026678223704e-4
+ t209 = t207 + t208 - 3.943887346640746e-2
+ t210 = t15 * t209 * 5.43860953025742e-6
+ t213 = t24 * t198 * 3.869452814178776e-5
+ t211 = t192 + t196 + t201 + t205 + t206 + t210 - t213
+ t215 = x2 * 2.0
+ t216 = t215 + 1.4e2
+ t217 = t55 * t87 * t104 * 2.698593356722665e-4
+ t218 = t77 * t87 * t114 * 1.071251714048381e-4
+ t219 = t44 * t87 * t139 * 2.888145048134885e-5
+ t220 = t34 * t87 * t142 * 2.408872289627689e-5
+ t221 = t66 * t87 * t145 * 2.919147494282987e-5
+ t222 = t15 * t87 * t148 * 5.43860953025742e-6
+ t244 = t24 * t87 * t135 * 3.869452814178776e-5
+ t223 = t217 + t218 + t219 + t220 + t221 + t222 - t244
+ t226 = t150 * t152 * 9.0e2
+ t260 = t2 * t216
+ t227 = t4 + t5 - t260
+ t228 = t15 * t87 * 6.231670342579768e-6
+ t229 = t24 * t87 * 6.421389633066001e-5
+ t230 = t34 * t87 * 5.266039823087284e-5
+ t231 = t44 * t87 * 7.023632629371505e-5
+ t232 = t55 * t87 * 1.395060454288188e-4
+ t233 = t66 * t87 * 6.653008439613883e-5
+ t234 = t77 * t87 * 1.000199975334673e-4
+ t235 = t15 * t132 * t133 * t156 * 5.43860953025742e-6
+ t236 = t55 * t132 * t133 * t159 * 2.698593356722665e-4
+ t237 = t24 * t132 * t133 * t162 * 3.869452814178776e-5
+ t238 = t66 * t132 * t133 * t164 * 2.919147494282987e-5
+ t239 = t44 * t87 * t109 * t170 * 2.888145048134885e-5
+ t240 = t34 * t87 * t125 * t172 * 2.408872289627689e-5
+ t241 = t44 * t132 * t133 * t170 * 2.888145048134885e-5
+ t242 = t77 * t87 * t101 * t168 * 1.071251714048381e-4
+ t243 = t228 + t229 + t230 + t231 + t232 + t233 + t234 + t235 + t236 + t237 + t238 + t239 + t240 + t241 + t242 - t15 * t87 * t121 * t156 * 5.43860953025742e-6 - t24 * t87 * t111 * t162 * 3.869452814178776e-5 - t55 * t87 * t107 * t159 * 2.698593356722665e-4 - t66 * t87 * t130 * t164 * 2.919147494282987e-5 - t34 * t132 * t133 * t172 * 2.408872289627689e-5 - t77 * t132 * t133 * t168 * 1.071251714048381e-4
+ t245 = t15 * t87 * 3.641004012563601e-6
+ t246 = t34 * t87 * 1.717274518394355e-5
+ t247 = t44 * t87 * 4.46106787275255e-5
+ t248 = t55 * t87 * 1.044315431291755e-3
+ t249 = t66 * t87 * 8.889962415356397e-6
+ t250 = t77 * t87 * 2.125075919109369e-4
+ t251 = t55 * t104 * t132 * t133 * 2.698593356722665e-4
+ t252 = t44 * t87 * t109 * t139 * 2.888145048134885e-5
+ t253 = t24 * t87 * t111 * t135 * 3.869452814178776e-5
+ t254 = t77 * t114 * t132 * t133 * 1.071251714048381e-4
+ t255 = t44 * t132 * t133 * t139 * 2.888145048134885e-5
+ t256 = t34 * t132 * t133 * t142 * 2.408872289627689e-5
+ t257 = t66 * t132 * t133 * t145 * 2.919147494282987e-5
+ t258 = t15 * t132 * t133 * t148 * 5.43860953025742e-6
+ t259 = t245 + t246 + t247 + t248 + t249 + t250 + t251 + t252 + t253 + t254 + t255 + t256 + t257 + t258 - t24 * t87 * 1.25050357788129e-9 - t55 * t87 * t104 * t107 * 2.698593356722665e-4 - t15 * t87 * t121 * t148 * 5.43860953025742e-6 - t77 * t87 * t101 * t114 * 1.071251714048381e-4 - t34 * t87 * t125 * t142 * 2.408872289627689e-5 - t24 * t132 * t133 * t135 * 3.869452814178776e-5 - t66 * t87 * t130 * t145 * 2.919147494282987e-5
+ t261 = t152 * t227 * 9.0e2
+ t262 = t261 + 1.0
+ t263 = t15 * t87 * 5.338060562709511e-6
+ t264 = t34 * t87 * 7.923377213370146e-8
+ t265 = t44 * t87 * 3.306243916737403e-8
+ t266 = t55 * t87 * 1.613129020273186e-3
+ t267 = t66 * t87 * 5.695020205797351e-7
+ t268 = t77 * t87 * 2.662715263644876e-4
+ t269 = t55 * t87 * t159 * t191 * 2.698593356722665e-4
+ t270 = t34 * t133 * t172 * t211 * 2.408872289627689e-5
+ t271 = t66 * t87 * t164 * t188 * 2.919147494282987e-5
+ t272 = t15 * t87 * t156 * t209 * 5.43860953025742e-6
+ t273 = t44 * t87 * t170 * t200 * 2.888145048134885e-5
+ t274 = t77 * t133 * t168 * t211 * 1.071251714048381e-4
+ t275 = t263 + t264 + t265 + t266 + t267 + t268 + t269 + t270 + t271 + t272 + t273 + t274 - t24 * t87 * 2.292270341531259e-5 - t24 * t87 * t162 * t198 * 3.869452814178776e-5 - t34 * t87 * t172 * t204 * 2.408872289627689e-5 - t15 * t133 * t156 * t211 * 5.43860953025742e-6 - t77 * t87 * t168 * t195 * 1.071251714048381e-4 - t24 * t133 * t162 * t211 * 3.869452814178776e-5 - t44 * t133 * t170 * t211 * 2.888145048134885e-5 - t55 * t133 * t159 * t211 * 2.698593356722665e-4 - t66 * t133 * t164 * t211 * 2.919147494282987e-5
+ t276 = t152 * t216 * 9.0e2
+ t277 = t15 * t87 * 1.128234845786051e-5
+ t278 = t24 * t87 * 6.340821028223617e-5
+ t279 = t34 * t87 * 5.49927270442291e-5
+ t280 = t44 * t87 * 6.9619499232357e-5
+ t281 = t55 * t87 * 2.725681819413176e-3
+ t282 = t66 * t87 * 6.335636802894656e-5
+ t283 = t77 * t87 * 3.419331942636945e-4
+ t284 = t77 * t114 * t133 * t211 * 1.071251714048381e-4
+ t285 = t44 * t133 * t139 * t211 * 2.888145048134885e-5
+ t286 = t34 * t133 * t142 * t211 * 2.408872289627689e-5
+ t287 = t66 * t133 * t145 * t211 * 2.919147494282987e-5
+ t288 = t15 * t133 * t148 * t211 * 5.43860953025742e-6
+ t289 = t55 * t104 * t133 * t211 * 2.698593356722665e-4
+ t290 = t277 + t278 + t279 + t280 + t281 + t282 + t283 + t284 + t285 + t286 + t287 + t288 + t289 - t55 * t87 * t104 * t191 * 2.698593356722665e-4 - t24 * t87 * t135 * t198 * 3.869452814178776e-5 - t15 * t87 * t148 * t209 * 5.43860953025742e-6 - t34 * t87 * t142 * t204 * 2.408872289627689e-5 - t44 * t87 * t139 * t200 * 2.888145048134885e-5 - t77 * t87 * t114 * t195 * 1.071251714048381e-4 - t66 * t87 * t145 * t188 * 2.919147494282987e-5 - t24 * t133 * t135 * t211 * 3.869452814178776e-5
+ return np.reshape([-t185 * t259 + t223 * (
+ t226 + t150 * t153 * t176 * 1.8e3) - t2 * t152 * t182 * 1.8e3 - t2 * t150 * t152 * t243 * 9.0e2 + t2 * t150 ** 2 * t176 * t182 * 1.8e3,
+ t243 * t262 + t182 * (
+ t226 - t150 * t176 * t227 * 1.8e3) + t152 * t216 * t223 * 9.0e2 + t3 * t152 * t216 * t259 * 9.0e2 - t3 * t150 * t176 * t216 * t223 * 1.8e3,
+ -t185 * t290 - t223 * (
+ t276 - t153 * t176 * t216 * 1.8e3) - t150 * t152 * t182 * 9.0e2 + t2 * t150 * t152 * t275 * 9.0e2 + t2 * t150 * t176 * t182 * t216 * 1.8e3,
+ -t262 * t275 - t182 * (
+ t276 + t176 * t216 * t227 * 1.8e3) + t3 * t152 * t223 * 1.8e3 + t3 * t152 * t216 * t290 * 9.0e2 - t3 * t176 * t216 ** 2 * t223 * 1.8e3],
+ [2, 2], 'F')
+
+ def weights(self, x):
+ num = np.zeros(self.nKernels)
+ for w in range(self.nKernels):
+ num[w] = self.priors[w] * self.mvn_pdf(x, self.mu[w, :self.n], self.sigma[w, :self.n, :self.n])
+ den = num.sum()
+ return num / den
+
+ @staticmethod
+ def mvn_pdf(x, mu, sigma):
+ n = x.size
+ return 1 / np.sqrt((2 * np.pi) ** n * np.linalg.det(sigma)) * np.exp(
+ -0.5 * (x - mu).transpose().dot(np.linalg.inv(sigma).dot(x - mu)))
+
+ def modulation_matrix(self, x):
+ xd = x - self.o_c
+ return np.eye(self.n) + (self.o_r / xd.transpose().dot(xd))**2 * (xd.transpose().dot(xd) * np.eye(self.n) -
+ 2 * np.outer(xd, xd))
+
+ def gamma(self, x):
+ return sum((np.linalg.norm(x[i] - self.o_c[i]) / self.o_r) ** 2 for i in range(self.n))
+
+ def lambda_r(self, x):
+ return 1 - 1 / self.gamma(x)
+
+ def lambda_e(self, x):
+ return 1 + 1 / self.gamma(x)
diff --git a/dmp/motion_planning/online_traj_scaling/gmr_oa.pyc b/dmp/motion_planning/online_traj_scaling/gmr_oa.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..248b7e889920da02a0f108bf4358457600deb468
GIT binary patch
literal 14523
zcmZSn%*&;xdO1Fs0ScHI7#JKF7#NC`85kH+7#LC*8FCmHq8J%cm>5!67*d!SS{N9b
z85km?m_TyD8mtTq44MD`|Nrk-!p6YB;O-k#?(YaUj){SR!3ktsAp-+LD#RcLkP#5=
zEes4%%pjvd44COrtRR~}B5VvPtYF3L45=IpDQqBiGb2M3CqoK5h#kenkir3Ga5JQE
zf*CvvDO_L%FGEnU1~&r(Lo&z>Aa8LnFfcfS-1450fuV$fA&Zfrh7sfm$!11|8U}`V
zCNQ6gK@!Aa1_@;`F=VlT81bw~I+z(?I@rKESRgvsk(98)m2e=5vcW|;kwn?yqFf-W
zvN&LNa)TA|fCah1BrB9mV`hN5nHP(iG$xoVA43HLV+lW47dJ@ICH6f-Efbi}#ZUub
zu!7>bl?g=FFfe4XGPE)=q%ku{W(k1J7G%g0LTE@~0JFi0kl7#&&5R%`KxXiQ%m$?%
z29RNJMjA7N4M>_3Bn@J~tWJXjdKPYdAlHC&fVHAn%mfZT36LZo$XtYNObj-KKN!MO
z7#M;zK#}iP0!j)M1|^_0TO6O7mr?>s7y0qYP`U`5gbRu?^NWhXYI92=YBSSw6E%bx
z7#P5!c?Dq6#G<0a${<Dt28JMzxg{*1w3S(vS^^Tv3j!G(1QH7ZSr-J#96{_LBS22c
z^G+?wOU)?;8&s8AlwS<i<p?H|z@kNodFiQOho$6~fE|{TnU|Q84i?JHE7Jg30I@VR
zCk?EiAh9Sh7h+Pm1}H;=iQ)tX1_u3%{M=Oi#GIteJpJVSl+3(zedi$0;P~=l{o<lz
z{oMSL%>2Cgf}F&>JdhkVvHZN8%)He2lA^?{_~PWm9FV&7+@kpWM7@H_5|FRr<1_Oz
zOXA}}-Yk{_Ie<}&k(ZGlOo}mrAU~rDqa33cqamXgBRFS*LI{LGz9~*(U|{HAV8~)%
z=m4kTPDX|jMusdVupl_i*D^7burRbUFtjr=w1d-W3L`@s149WbSPdH}1-d{IVl6XR
zh#hKH76(HO*oa^aP;~hvgQ5w<0=d>06nBvf;KUv4&rr+A&;c$TBq6Q=l@s9N5Ry$&
zz{whv<g*x|ri0W$$`6=2P?{DO31ci|2IZ_^4Ulzy;7AMN1Em>35W&yDzyNj*IAQ4-
z1c`wJKw%Li0!k|&x0R=6re~BuqP2p4F<Q*Yz`$UWo|>0hQBY(j4GJJoXt6P9fFyz>
zLCyz7dJr#&#SL;T$lb-Ixj_OT5gw2T$dkGGDWy4y;PjQ7SW=W(5d;#|07W|3lR;7-
zm7w5B11Ee?no0xv6B1gWfPh#BQb?E793;!Yz#swgGRTJvjQos3jM|X&3JL>|TfsF6
z$PYD)46w)rrFd{`BK!sM2S|c04hNSX>7cAv400#~!p$Hq*v+8Gf!2Nupsdx5a6c&i
zf<Qh7mocf8sX?G<9bg}U9G+gG2#O346Y3>ZMg|5@Is|*EN|*s$5i>GKf+{y~V@0x-
z8I<2sm>I-tS-|xI3xjwqD?<$ns8JzY%f?W{3SqH>)v+;1)N+8;u``I&a)QM;7({Bh
z7-~2fYPcB0Q#ctUYPrEGxERE1dB7}g29a7`uv(BVK86}zh8jKw@f02g@mhX{8V-gU
zkO~2Y8Xkrkeg=_RL53Ou2JRX`28miBaP7*-P$L8?vucIGrtmUI)QW&j=3@}46$Oid
z^ocRlh%(fOF-W8cFo@TRGt`JM)QB^P)JlNW3Nnb-N`hHJ3?j8sU|k^n(hN0H3^mdW
z5-GwA;<YjiHIfW9G7KWMvJ5p63^lS0618$LN63L4ArCf0gh9Mk0n8F*5UEuJ8z;sf
zQme#JqsUOB#2}s`&LCc^3>KGQ5U*7Ms|Bf01&c{Ch}5bv)TlDls4<AANHK`lsx#Cm
zFw`hBh}3E@)Tl7js56MvYBJPlFmTssGDy^F!CavQc7--WjW$DyG=oU34%lQF28mi-
zu*tFvBDH#8F*yd2T78BZJ%$>628k4T2Ju=0h8kUl8UqHAT0^i}1qSh2BQQ&mL8R6g
ztP7;ygrUZmp~i$kB1MToyw;SV#)zTDltHA{jG@Mmp~j3sqShSd9CNU9EEsAm7*arv
zw*(ua!XQy=1!k!-h}2qxtx#hSskLFKv1X{TVGvJIXOO711&eDiNYvVa)q+&mgT*u%
zL~0!vYU~+m92mq?v=}669T{q@7;0=8L~5NFYU~(l92rDvof&GJ7`SVk86;|5VD51N
zyT_HG#+4yOn?a=34Q!qcgLthw*bZF=ky;P1m>z>jttUf`2Sbe~gLsNQgLthMScL(D
zc&#^>Wyl~>>jPE`(&fug<HJzn%OIX{Yl$57+WP~*iQQtQu9<IPax#~@N0z)<7Q
zz+Dr-AW<6#b59`HJwXgLK@2Iz3?j9`V3SQ4Bx*yzEKsn9g6%M45UCAgs0n4L31bjX
zF=vpd4F`)`Fo@SifYpLjM1sXE8ANKM7-}LJYN8m#Q>+*yYNHuyLKteo8ANJh7-}LI
zYN8oLYGWB{Vi>q<Vi_cA<6!QI1G^`lp(dUo#hO8+Hi4l=m?6c6L8La3p+=q|1r)+b
z3^h6oHI@wGHEs;zHNgzxH3<yjHHi%3HAxKOwaE-Ab__Mi45GCuU{~2Qh}5QnUFE<a
zQk%w5lgdz&#vq>J$RJUh4p!mBAX1wFR^iMbQk%(8lfh7v$sk^v1(tPT5UI@u%epd%
z)aEeMWHZ#{Fo>tPF-X+rg2mk#L~8TEYCRZ4YV#Rt@)&CJ86;9X8N_P~z~Z2IC<LqZ
zW)P_@VyG!(s3~HQNbzA1uPp|v@MRFGEdi?l#Z)OnO$kFyDT8>5KZ8VV8OY}~#S9`~
z-<N~JrY4<11ROpUVBG->BDIxZ-GK}uwN(r?l?*ji4B{z44C1xbpb)I7W)J~~Vhvb*
zFoQ^KEm(aBgGg;1LrpD1O&x=HN+^SPZ9ORK*3>hIfU|D{D5ciqGKhdvYa>{97=uV{
z6IeGWKARb8niy)D8N^c}7{qH^Kq;xFg+T<Inp#2muBL!N1f2KU7;4HHYAP7SYibz8
zYZ@5DYZ@8EYg!q^YuXqjYTKdlmcq&)ELz(EwkeW9q_z`mQxt<pZ5KmLCqqpagG6mN
zSQeCqdcd+V3?j9?3^hFrHN6btwS8dOSO$^Wez0sDgGlWJhMIncnh6ZzwG+Xz@eCrh
zlfbeG3?j9Y8EPgm)J$d&Pf28usGR~<k;EWUI~A-VnL(s>8bi%ghMH*%;<eMkvMCHA
zwKEuMW-z3rGKkmCWT;_ds9|Riubl<<#Vl~OBw9NgR72IwWDu#H15;5mhe4utF4&AT
z29er%U>njIL~7?V)XZb3na?0my8tYk!5~t*5G<R?AX2-Cp=KdN%_0Vglq?4E+QncM
z*$g7JOTa2}7({B9GSn<#s9DM&Ub_q|o68_lyBsW=#~@O>f}v(PL(K{XiQ1K5v3v%R
z+Erk61q>p!s~KumG1RPPkVr{n5U*VW7B6HFsa*?JTf`tzyN;n|Ekn&Z28r7BV6kEb
zk=hMlbtMcUwHp~~HZas|WROTHWe~631Pg;QP)u$Hr@s^ik(ve2G`Izvra&T#q3LQX
zIE8~mmO)ebHgGxuiL8XCrS0If1QJ;fO;0<(IRPZH2AUgog3~TYWD_*~?qaCf!ceo7
zLA+)ggLut$2JxC54B|CA86;|UF-X+zW~iCXP&1c7B&D1|q;?NO%^r}`#cTJ1eFjS1
z`@p`fU=XR@&rq|Ep=LjWcuFOMc<lkO3Q&n~5Uiq#L8SH&L(M^knnMiYDb)-TwTHnz
zH~@A15wLnt33L>!zJ@`h_83FWQHGjh4B{!Z3=*}+!M-^T^~njadQfR`60E+CL8SH+
zL(NHsno|rCDfJBEwWq=24Gbc+XTWMfrO;W1nllVFXBi|?8X3fE&w)ei95l4fgH?g@
z`vtJhCI*q(iwrdv7-}vuh^I6&h}T{Mhu$S<$Xy1<(qU)}T><L`l`dDox?31TYOgWW
zTxF=a#vq>3${<mD9URZsp>ccz9HXb9v3ZlB<_JU02?p_+^9<rOml?!st}uw#++Yx|
zxyc|=dkbtLC`a99sJYFM(#9ZOdk1VgC?>O@F1QO0ACO2HG>q<n{Ra~1gZlA4*asky
ziBMlW0LKDIWC}DU9)jHo5}6Kl>m#sRK_cBy_daH*xyw*<k3qcVK7)A80|xP$hYaF1
zj~K*j9y3VPK4GYt2}(GSRPz+n9<8~<AX57bp2wdtNYp-u*^|=FAX57RW=Bc~gGlX5
zm^)KC8ANJd!TgcZ#UN7q8s_tqZU&LsH!%07^e~9jzJ-NBN-u**?K`jy?F=F{9Z0$B
zJy<_X6qdt2fbEBg!gAS1u<0;SSWf!{a}TKGu7TyY&oDQG>V_Iv{`vw|3o`+x_AAVt
zpn9qXmOH;O)VyG*dC4GN^NK;d<~4(O%^L>snzsz%HSZY2Yu+=6*L+|QuldLzUh|1T
zyyi25c+D3E@tUs;;x*qG#A}2ZYCus`jqs{OjXXrGm_eil>LrO9SBO|HgGde3^Aa^d
z5U~~pks7EcC2F)8YGBTWdPbrq9w7#Gv_$Q9a44rRh@|u}h}8aog+fX{gGlX9u<t=~
z6BtBle}O|D#GA+<Qu`Ymwjkal29er7un<X^%pg+x7pyXiK?LRmaP9gJW*Ru$VQKn5
zIOafJ2YC~gLKzrAjx9qn0n}PzWUR43cmZYt6C<)`VL5}D5fm)2Ab{l#7DiA&!$e^@
zgq5-82Sd$I2JxC-4B|Dv8N_S;Fo@UuWe~6V#~@zwpFzBafl<7Mkx{&ciBY_UnNhrk
zg;BhQl~EiTLn%`jMBp(4@;0<VApx=s<TX&NLM;Qw0!S>KL8Jzn)xog^602trseyU`
z9Q!bbLW2b*rlDSvsAXdWmGaM_r93-h4Lf6s9fNo+2iUKm7@7*|FM>k}#G3}{GlD}B
z#GB3_Qp?Q<O;#y07({A$z##{c1J&%j;FtjMW-*A=^1)Ps5?>9h^yCMd4a%jP7({Ar
zLCYoqm^q+CSp%yn1YxcM)t5D}@>B@s9#DR)ftBgPV7o!)u7R2hN(CZ}HJpq!T#Vv1
z+>GKiJdENsyo};Ce2n5X{EXr?0*vA{f{fxdLX6@y!i?fIB8XU;4T>dbo?6BrQUfi%
z;h_(5JhXZM#~jGw1yGA^KzidL`3&S3XhjB&C6GFhc_5G5g49_sfQzj4P`%0^F;|G#
zVyKulNGu2<wh}64%pf9BD+-RyJJ8q^gN0Vg90rkEaYk5zn+xh$gF_t@GxHckY9(PI
zm@*&K!-mCE$^r(FT4`8Jq%33*sg(hTDoEcVP){4o1LZW3H$Ywhg|QsiG>}Li)N!ER
zr99Y1kmy9HD5wjp05%;YIt3~U>S-y0T@DhR4iyFUa+JXC0%Z}949GT6FHxDXMx3!m
zf>FFil2N=yic!2qno+z)hEcpmmQlP$4itlo;x!74;x&ql;x$T);x)>Q;?N{p12PPn
z5W%ql%17PEu?sC(LC%0CH+YPLyaA7K3rOsOJPb{M@Vp8N9(asHOIwg<p@|x7FUUh6
zdqJK8rGi=&M$jM@6GP2j2JRX*Mv<B)3^h-|b?aOPk($j6A}Na*M8rkJYgoXwZOv|o
zhy;i~lR>0rHiJmbcSt&bih=qLV4X0v;x*qHYB(50YIehn-pn9U11bs@Gl)oth=U4-
zI}9M3!Iimq4J*VdkQf^yLJVXbNGC}A3CtNFXRAP*Eyh?gmjUEtP(uLZYzYwwXd?ma
zlyXpNW&!yP;%rfv*-$Z%*}EA;V7kR?I2gHWL?P}LW31WC0Fr^ZTU<l}=1!<vP~5Eo
zGn*CWB1o`6)k>s;htE=28Pb^;QkF0@GcyEhEM;I|@XG{^ab|)>M$<tohK4`~2!kQg
zArmxkm#+RS#3iFE!Xdr*McjpzJRj`R70dZGqn2sgr%R@n{tq#nVxQjoJbd45<p78D
zkk7{Tf7tEp)6+~=Gk!fV$3ETr-M11k5pRd|EyB+K+S*^+r6(U|)jefpXrDg+ll373
zi3#@U#l>s79%#onq^nB1yfE|hwom_RJ4ap6WQTpao0;jG{1!8Z^ee|-U$LBRXP+LB
zk#I&$*2X?P&MTjJrQZhobdId7D}Dds9MZr2wNNVO3A9gtBJIL=b^k&8^sfzv^IV*M
z+NI0QE-dQYALNkU7ax42{Ijfmddbx4?|;sC*{9!}-KE$#W0QTl0Ban}n%xl&>A8<Z
z&n=(mWS{<gTJitY-5c!F@9dD&I_L4vF8%+qUg`Zy%pB5#^a={UW+vIEAO9P&KQA`b
zKK;AV)E^G~YwXikM^9Lv@$I{Px`y2O4dRwl?bD}yJLym=u-!ggx-p-%cD<o}dW&Oc
z@uWt7hjfqsQ4?A%{Or?}Dw4wa>pbn#Z&z<S9dYJ}efq>AA(it7;~dg|et0glS~9^t
zedno|=<SA=?bB=CNIhbk^Up5bbQgE2?DAlT^s}jZr*iXU+o!W|>d8M<PP0$<{417v
zI`Fi8`b5_or(TO0I;39_<^OopImbR-|L?z={Ywwqr;D0K=JANi*r$uNZJ)LB{58Au
z?*V`A)OxbmLsP*A`}A{QldEASm%&WFZJ+MY*j`rhmCqsl#@6lsQ;bz$!BPYZmQwrl
zui#+WV-F2^UHkMhaL7m5r{4gF{2}{vWpKPl+ox{_Cz9WG>7T&~;Dmj;MW#b??B4(O
z>82&~7O-@3I;8&tO<Vuxw@nWPCpJI(^b&B&WUx<90jIh(_R!Sg2TLtJ_UZB9)N;T+
z{TDbe3+&U~z#$)JpS}Yes*ml{c_ed>^?hWpPv3c(Z92cjFZ=Xub%HDRZ2MuKJ~_2e
z=#U4qL;4@>lQmoyp4q4G6lvU@>&)bk?&iJX-{jiM_UV_-gsHrVVT5HM1>5v<g|@-L
zIZy4=4Vv5D^vz&^Nx>PJ4(VLSKR$8~{bZl+yiMSKcgB1Bbdk-cecyCFuul)(uscxk
zr8rF20h{y*#!q%d^!~C>_nQ_T`{m0c`*fpcd?Hh=nH<tLGr#LK|25e@{pE*Q3LTG_
z9nx#R*wwFi@YX)Pcl)2z`VwY`^n{hSkJTIuvQJ-VA05d1Da$@xg5UCqQZ3xgCbpTN
z*#t=BEA%*|D}z&Iq<#7waH_llOVmqYiTb!bG+EWyr|W`~)n$8V7I%bY@dftjIp8c6
zWS{;LoTU!fr+)!w)<paCXW-0w(H>f^Si{PdCD1h3km!)U>$6gWiSjMGbd8NW!h)^}
z*r#8)m(%!lHn)BHE&lUQt1^Y`(`8i+&T)Gx*{56H=vO^dAZVXHA;hrK%0kLMUH$Pz
z`Tt$#?9%nav;~eaezHqn$9&(uJDJ@+-75N{x~@#1eLAOGmEz$gkL=Q)y`LU1>6W&A
z`sO_g?;NcbwM!REQ+c&1`I=q2w!jLf=Pspo>19iDH3bhO+NEb{@09qG8)cuqA@{5P
zAL+$*>1))J>b;8%?bA26$?Lmt{;*5$kJHuCTKU{A{m5m;Lk{x=?9&x(7i{}~g~vW!
zP-V+wQ3oOW^t}HEJ+wp8?9+SK#>yU>EnuH+|C9Og`V2E@ASldn08bLRK_=G_vpp3B
zC7{{HqSWGy#DY`}&;$!&8Vfu<Sz!pFjKEY0lro0!OdymggffFr<`BvPLRo^Rl}Zc^
zptK>BHiFW|5ZWB7&H|>+1R`!|3Z>1Uv^j*ffa$k@@C~8*jiCCCp!Qfm%{PLow}6^!
z1QUnxq4pR-?J<JdV+1wd2x`7D)O=&8`NlAFq3SGP>Y(NuL(Mmanr{p>-xzAXCCogi
zeq*S96R5Ze)E-MHZ34B=5=xsu?J<GcV*<6u1Zs~7)E*P4`6f{FO`+zSLd`XWnr8|%
z&k{yM%{7J5Q1eZp=9@yzH-(yS3N_yhYQ7oNd^4!|W>9m@pypXZ&9j7>X9ne)!T3<~
z&7kI+LCrUZnr{v@-yCYbIn;b}sQKnl^DLp}SVGM)hnjB=6*q^9TR_dTfa<e=s<VKq
zv$O=Sz9=y?Fn}=(VGJV}!x+XefiX;B3^N$R9LBJKG7RA&Fe?mURv5yx8^Y`|gxO^X
zv&#@>mm$mwL%7K>1C3w?8o|slf|+9kGsg&KjuFgKBbb3kFc%uZOg4g<Yz#Bm7-q6D
zOqVgt3S*cR#xN^PVGLuKUB)mgjA2$7!>llYSz!XR!USfr3Cv^@n1LoR15IGgFoBtD
z0yEhJX0j>FWK)>QrZAH&U?!Ww88DMgVFsGQtT2U{Yz7yBSz!jV!VG4G8O#bZn8{`^
zlg(hhFoRiP4wr&k0W;Ygrq~>2g*nU$bC}8IFuTlQ4l{?F3^ULiW}pSk91Ey9MzADd
z1WytcFas@Mwp+klXaTdr0%nB;%nA#b6_zk7ETL8yTEYypgt^%gX0j#BWJ{RImQa(8
z;JLsOW(6#J8o{!I5iFk?89?naGJx7;WB|3x$N*}Wkpa{$BLk>iMg}mu44_sR89;qu
z1k0XAusmo4%brHC3~L0-j7G5BZ3N4eMusqRV7bx=mMe{5xzY%ho{eDH(+HLsjbJIt
z2$n02V7bx=mMe{5xzY%hD~({8(Fm3qjbNG42$lzpU^&+amKlv;nb8QA8I53>(Fm3q
zjbNG42$mU*V0q98mIsYsdC&-!2aRBP&<K_XjbM4u2$lzpU|G)yR$LgF!lDV5^^9Pd
z(Fm3YjZ9&|3(IjvupDOu%W+1qjAjJOdPcCEYXr-BMrN>hH-i}n%V<WhjAjJOUq-MT
zX9UZYMzEY~1j~9xu&iff4l@~+^^9Oy&j_CNU>VH_RuCD%a-0z?#~HzLoDnR?8NqU#
z5iG|USsKuLO%iD7V@gGQdPSNFXcZ55wIm}K2IPZ^Kxl48eiS}PJwygw93qNP&&3E4
z!PJ3}fvCWyhj<w|4EMpzhnN8)`JnDWrjh03pz<L1qGE_Dm>7r$!pM9W8$zSX@-Ygb
zK!iOIb;#}k>BkgBi10B&e2dLQkU7X0w9FG3Z)3zKyASMpTq^FN=mn`lHu)MO8)FPp
zG*b*yJY=C8Xr(O|Y?=2Q2KX{>*h05jCWeY)h7xA*8fNecQ<vBzhFS=p1vHZgTKk>F
z%22}yR>B4ng041Z2Q6g=E$RfVH0D5;Kv4u*AkNGn0$N|q%wSWf#1LM?#E``aS~m|_
zi&n$JAPQSF2y!oY(H&$VC1}|sWC10}GvF>+ZdqP@K}s5A2{AWlZ6&Cq76e+730{7W
z)J+5Rx${bM!OKX&y`hxUJUVB0$Wl#428Lpg&lnhm7{wS97<m|zL2d&rm;^Z<ylk%s
zvfeg}0Tcr%u%*5!Obnnkzo77AW)KE3SwM@@YrxSZ(#*&p3KC-jZAGYI07aBU7CR_p
zvpB%|SU>>{Q4Ct~3sC`C5Dd~R5)58=>IWXFgp5mqhbtK#?AHLfHwa9Cd&MBP78e$k
zfIF`RnL$FJ^aS!mN@__EXnRBnCrG3yF|W8Fzc>}Vsx=6-NE~bfcvUMT%0M25tfT~4
z1&$87ECvnoWMp871jPiX1ZH63WHf|i6OboBjzYu&BRIJ;Fi3#bSA$nvgHkzoxip9Y
zUO<_}3JzvA&}2R+8d4Y;ghA;VRTvaGOkmT%x<#Pz$_~;6iWVmD&^dGsHaG&nYx5z?
z<w4;dqyS3c;A9SpfgpcI1_p50f)@wpmzJa!L542C>w={~Q2-iVu1Em~ESR8A=*loK
zFvP<{mywsz4-&c{*MM9O3gu!okoy>*3)a)XduBkZ)fvG{)4@p^x>_9?BJfq|pxh5$
zm2M5mC9#H}Rq3EP4{-D#El+0#WdccwW{8<A45BFv48o9g=b)v%Am@O?*$+}!fWsJ^
zmO+lp%P-1>gguA{S?UW~_zWH+%7pOY0ZqTaMO}QJ3JPVAKiC)`Q4cEmAcZ6-onUh%
zniySD1UP}FC+6lRrobEwU9AjqJ1Bg?8BP?mvkJWN2$Z$J1r#VdNkCHoxMTwF76=Mq
zgzgSNl<?pv02B1_5-7FgB<3ciB*qt|li@4z!S1WnEQGH#KpQilJ6ZA)b5rBvOSnLp
zJRZD-AvHceNE}o)fi?#O$%9y+Rp~(<paKfC9ijwOcNdo=mSiU9rj}&nr+^FKAW%60
zj>@1=kjgMnq{1r#`bA|B$WBm2Pz=i7Y>b?OoNS!ToXnj68T2{X`Pn#GK|HW~z?lr3
hn)6C?3o0S46OeX@ESyEZBxnPP0XuM+3d&79%mB@S{E7eo
literal 0
HcmV?d00001
diff --git a/dmp/motion_planning/online_traj_scaling/trajectory_generator.py b/dmp/motion_planning/online_traj_scaling/trajectory_generator.py
new file mode 100755
index 0000000..899b8af
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/trajectory_generator.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+import matplotlib.pyplot as plt
+import yaml
+from gmr_oa import GMRwOA
+import numpy as np
+
+class TrajectoryGenerator:
+ def __init__(self, dyn_sys):
+ # parameters
+ self.dyn_sys = dyn_sys
+ # state
+ self.x = None
+ self.s = None
+ self.ds = None
+ self.sat_flag = None
+ # Output state
+ self.pos = None
+ self.vel = None
+ self.acc = None
+ # desired path
+ self.path = None
+
+ self.reset()
+
+ def reset(self):
+ self.x = self.dyn_sys.x0
+ self.s = 0
+ self.ds = 1
+ self.sat_flag = 0
+ self.pos = self.dyn_sys.g(self.x)
+ self.vel = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x))
+ self.acc = self.dyn_sys.dx_gxf(self.x).dot(self.dyn_sys.f(self.x))
+
+ def step(self, dt):
+ # Derivative
+ dx = self.dyn_sys.f(self.x) * self.ds
+ dds, self.sat_flag = self.traj_scaling()
+
+ # Integrate
+ self.x += dx * dt
+ self.s += self.ds * dt
+ self.ds += dds * dt
+
+ # Update reference
+ self.pos = self.dyn_sys.g(self.x)
+ self.vel = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x))
+ self.acc = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x)) * dds + self.dyn_sys.dx_gxf(
+ self.x).dot(self.dyn_sys.f(self.x)) * self.ds ** 2
+
+ def roll_out(self, dt, T = None):
+ self.reset()
+ pos = np.array([self.pos])
+ vel = np.array([self.vel])
+ acc = np.array([self.acc])
+ t = np.array([0])
+ s = np.array([0])
+ # for k in range(1, round(T/dt)+1):
+ k = 0
+ while True:
+ k += 1
+ self.step(dt)
+ t = np.append(t, k*dt)
+ s = np.append(s, self.s)
+ pos = np.append(pos, np.array([self.pos]), axis=0)
+ vel = np.append(vel, np.array([self.vel]), axis=0)
+ acc = np.append(acc, np.array([self.acc]), axis=0)
+ if T and k*dt > T:
+ break
+ if not T and self.s > self.dyn_sys.s_end:
+ break
+ return t, s, pos, vel, acc
+
+ def traj_scaling(self):
+ dds = 0
+ sat_flag = 0
+
+ return dds, sat_flag
+
+if __name__ == "__main__":
+ with open("example.yaml", 'r') as stream:
+ try:
+ params = yaml.safe_load(stream)
+ params['sigma'] = [[[el * 1e3 for el in row] for row in col] for col in params['sigma']]
+ gmr = GMRwOA(params)
+ tg = TrajectoryGenerator(gmr)
+ t, s, pos, vel, acc = tg.roll_out(0.01)
+ plt.plot(0.0032*pos[:, 0], 0.0026*pos[:, 1] + 0.7)
+ # plt.plot(s, pos[:, 0])
+ # plt.plot(s, pos[:, 1])
+ plt.show()
+ except yaml.YAMLError as exc:
+ print(exc)
diff --git a/dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc b/dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c1df99f5686f4e652cb2f5d7683dfb0f5585c66d
GIT binary patch
literal 3744
zcmZSn%*(aD?s9xG0~GKuFfceUFfbJ7GBPlvFfgPrGUPBYWHB;CF~PXZ3{lKrHWNb*
z3qvj|Lli3mBUC9HLliqh3Nu5B07D83Lkj~#GXq0p6bC~pCqoJ=LpBpbQ6^(57efjg
zLkc@XGZRDqe}*V-hRRzEsmu&nJPfJa49$!TQM?T43=FBf3@IE8k}k1k3{iYw0X_zo
zSY?JNelUlh!6jCJAxeNDRe&LdlR+eoiJ@>l1BjdiCR?Ef*rc+8SRi8s8B#eIQU$?Q
z2r;AzF=Podq;P?_F{}(xA`DTY3{heXQQ{0y5)7$K3|W#4DclUHVxTZcVPt4!Vo2d(
zNMmA<6iMMl73PBq^D~GyGchoN^h$wz&KRC2&A<ZknlwW;7ei4zBV&{d17oTTgS$r_
zBSS=Z3L`^cL>@SlgEa&h7#K4D|NsBrPlJ(xfuV$rfq}u@H>lj-u|$}Gfgz+QF)KB>
zB)_QAJvA@2D6u5J2p(%p3=9lTpjfkDU|>jPV2EM_MNkU^IMx_J@c@c4CNP5q6cZ`T
z3@wZdQEbpyiehI73f2Hwm<%!shMhr1#xpQ5)G#vCFfhb}Qv`?sP75FgI3a)-EFcR(
z3|1I}4aQ)HF*slhPLOF?T;L!|VPFUb``1r{8{}(t1_p+d%DnjE%Hk3h1_lPd{Jhi>
zP<T{;$>I`_<5G%CI6!VoEQwFcNlY(cW?*0_$S(%dWvMw}Ix#sJY(PO`Nk$1P0|P@*
zYH@0b21q+tyf`%{EeOQZ0404eQJlxXz@VRzpPQ<mn3I&5r=Ogkl9`vT?;PYA9A93n
zUtE-|pPOHjnV%P5kdv5~2a>}kmY<iCnU@-0Qk0k#U!0tn15yX#K%y@`9TtCj1(jfb
z#K&jmWtPOpbAtRN0&)%$Cle<l7(!?!PNrl~Ab>E)v*4In28tO5hAeRY1cf~lD5-)N
z%nT_^pfHYSfr+q!xFD5mU<MN?)q_;BgSiY0SsY*nSUd|7KMV|6TnNEtMi4um8$^Q4
z;Q=!Us^<m8d9Vh^&3>66pJjr)puxw$zyJ<>aCBA}1c9PD2oy^}pi~$HN(JEfP7eY_
zG+0%71vp+)@=L%nX+fNz^a4_zQW2kCkru?oz`y`bN<q9J7Ra`Niq;@dIVuH;PLNL-
z82KTQ$tb~SzzEJ_Ape3eI3kr98Ni_li^wc+u+}gzWHB>{)G~qPSU|ZYhKZq;8N`ie
z1(7USObj(lpxh~9&J+(;$CAYclF4F*iGsN-AU<3OYyv0_A(0QWEFO{s7#N`O4;F_e
z0aTYYGlJ|uqy<QN!LOc~K?LL>22e2%V&GQ|_Ie5<gRqD=D3wA}5h%C%Wr9dV0tyla
zB_&W$f>TZjDBwYP4VLl1NePm&1VAc5$tg$>#1aA#oFGC3L~w!94A>lAP`U!yl2QUr
zcq!0?mr@K)PGDjniVBe9ic3-pltJkV<a!1s8AefNVMZ}VVP;UuGG|N%Wi*hVKv@Z#
zwp!q6tC@j;v4oKUlxD!eo6ZO-1ZtTX5P~cWP(c<XK~|U`D^xIr5hlWhA;JzSaB4Xi
zDt?11u%ZIS8V*odWM0bwN-|mOpk&$12yqApLk$~44F^a_q?w7K7GfF)Lk%Qda)KHN
zwd`OkI2dY>-N_7f0H~$G$k5Eh2=xUALk+UaVctO~gUE6))WG}#N{TQKB9yU$vQ?2U
z$niCd3?em54B0FUMZSyyc?=*Ko}x0aZ#cmu7ef{|SiX!Qps<Xgu#6!b<S%xRlWSN&
zR;95r1Z#j|52*+Q*FlLDnZ+8Q6bmjdz=<ud09;um78NB{g3CT|F$XT&z*<6zN>hU*
zK`9he@+B4&q~@hSk~b*Dg7p^1r{<+-fKmatAPf=%sRk9hA?OBz1WUlW!9{8|IBA24
zfk@w=tXY(wlM|m`T4D@J<6xICaxsd6p%9}G6B{EZBNrntqZp$oqdFLhF|shqFmf=0
zt4C1k2L%H-<%7JP!pKm|2u_mVVzPz_)KUuuHv%AW0k#5MJwd`s0#p=$Y9ELMs16x`
zpa>FWU|{e91pz3M85r3Z*_iw^K;Z*zFXScWrpCvYfXb}+_}u)I(wx-z_#i%z<H5-Z
zlt6+&p%DZsEWpMgJOpM9gs(u!4djVpkh|F!IR!a6IR$vcp?=Cu%*>0AF9x|kwIVUM
zASYF?GBGy?oHC0b2|F`AH!&R)Ckzh#4(Zh*>)an!wb-ZYI?KF%WR_(QZeXNO`NkGM
z!za}q+|o!-1A}MwU{7nvg0g}j0|P^DVo5<xeo0Pdl3qb2hy`wLLz$p}E66DU=Qpqu
zK(UaXTNIz47zDB%?3ldL+=9v=X;6@Z{1v1EG8&Z0^9xe*z@`<K6s0ESmVh#1abj9(
zd`^C13Rt2bu_!SY(ppW<hqP9U^2@>5AKG|L&n*ITOVS|`s0gwKRHmcnV337i=Ytr<
z8TsX4>mnU}eO!x*@{7PNrqqgLi0?se2eSqs^1+d814*iOpa3pbU|?Y2XXIfNVB}#E
c_TXU@Vq|9IXY^*|VdP<yW3*xBV-#Wp0H!GO>i_@%
literal 0
HcmV?d00001
diff --git a/dmp/msg/DMPState.msg b/dmp/msg/DMPState.msg
new file mode 100644
index 0000000..aec6613
--- /dev/null
+++ b/dmp/msg/DMPState.msg
@@ -0,0 +1,8 @@
+Header header
+float64[] position
+float64[] velocity
+float64[] acceleration
+float64 theta
+float64 s
+float64 tau
+
diff --git a/dmp/msg/TrajGenState.msg b/dmp/msg/TrajGenState.msg
new file mode 100644
index 0000000..936c4e9
--- /dev/null
+++ b/dmp/msg/TrajGenState.msg
@@ -0,0 +1,6 @@
+Header header
+float64[] position
+float64[] velocity
+float64[] acceleration
+float64 s
+float64 s_dot
diff --git a/dmp/todos.md b/dmp/todos.md
index ec74f5a..6ac3bb8 100644
--- a/dmp/todos.md
+++ b/dmp/todos.md
@@ -4,7 +4,7 @@ and it's matlab
# calculating the dmp from the trajectory
is in
-TC_DMP_constrainedVelAcc/something/DMP.m
+TC_DMP_constrainedVelAcc/matlab/DMP.m
# running the dmp node
TC_DMP_constrainedVelAcc/catkin_ws/src/motion_planning/nodes/dmp_node
diff --git a/dmp/traj_scaling_node b/dmp/traj_scaling_node
new file mode 100755
index 0000000..e4e6aca
--- /dev/null
+++ b/dmp/traj_scaling_node
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+from motion_planning.online_traj_scaling import GMRwOA, TrajectoryGenerator
+import numpy as np
+import rospy
+from robot_ctrl.msg import CtrlTarget
+from std_msgs.msg import Float64, String
+from motion_planning.msg import TrajGenState
+
+
+class TrajGenNode:
+ def __init__(self):
+ self.active = False
+ self.rate = None
+ self.dt = None
+ self.traj_gen = None
+
+ # Load parameters from server
+ if not self.load_params():
+ rospy.logwarn("Trajectory Generator not initialized")
+
+ # Setup ros topics
+ self.target_pub = rospy.Publisher("robot_ctrl/command", CtrlTarget, queue_size=1)
+ self.state_pub = rospy.Publisher("motion_planning/state", TrajGenState, queue_size=1)
+ rospy.Subscriber("motion_planning/command", String, self.command_parser)
+
+ # Go into spin
+ self.spin()
+
+ def load_params(self):
+ parameters = ['x0', 's_end', 'priors', 'mu', 'sigma', 'o_c', 'o_r']
+ if not rospy.has_param('motion_planning/update_rate'):
+ rospy.logerr("Parameter " + rospy.get_namespace() + "motion_planning/update_rate not on parameter server.")
+ return False
+ update_rate = rospy.get_param('motion_planning/update_rate')
+ self.rate = rospy.Rate(update_rate)
+ params = {}
+ for i in range(len(parameters)):
+ if not rospy.has_param('motion_planning/' + parameters[i]):
+ rospy.logerr("Parameter " + rospy.get_namespace() + "motion_planning/" + parameters[i] + " not on parameter server.")
+ return False
+ params[parameters[i]] = rospy.get_param('motion_planning/' + parameters[i])
+ gmr = GMRwOA(params)
+ self.traj_gen = TrajectoryGenerator(gmr)
+ return True
+
+ def command_parser(self, msg):
+ if msg.data == 'start':
+ self.active = True
+ elif msg.data == 'stop':
+ self.active = False
+ elif msg.data == 'reset':
+ self.traj_gen.reset()
+ self.active = False
+ rospy.loginfo("Reset motion_planner.")
+ elif msg.data == 'step':
+ self.traj_gen.step(self.dt)
+ elif msg.data == 'load_params':
+ self.load_params()
+ rospy.loginfo("Reloaded motion_planner parameters.")
+ else:
+ rospy.loginfo("Invalid command string for motion planner.")
+
+ # spin -----------------------------------------------------
+ # -----------------------------------------------------------
+ def spin(self):
+ print_flag = True
+ while not rospy.is_shutdown():
+
+ if self.active:
+ # Step DMP
+ self.traj_gen.step(self.dt)
+ # Send target trajectory state command
+ pub_msg = CtrlTarget()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.traj_gen.acc
+ pub_msg.velocity = self.traj_gen.vel
+ pub_msg.position = self.traj_gen.pos
+ self.target_pub.publish(pub_msg)
+ else:
+ print_flag = True
+
+ # Publish DMP state command
+ pub_msg = TrajGenState()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = [0.0032*self.traj_gen.acc[0], 0, 0.0026*self.traj_gen.acc[1]]
+ pub_msg.velocity = [0.0032*self.traj_gen.vel[0], 0, 0.0026*self.traj_gen.vel[1]]
+ pub_msg.position = [0.0032*self.traj_gen.pos[0], 0, 0.0026*self.traj_gen.pos[1]+0.7]
+ pub_msg.s = self.traj_gen.s
+ pub_msg.s_dot = self.traj_gen.ds
+ self.state_pub.publish(pub_msg)
+
+ # Notify when DMP trajectory is finished
+ if print_flag and self.traj_gen.s > self.traj_gen.dyn_sys.s_end:
+ rospy.loginfo("Trajectory finished.")
+ print_flag = False
+
+ # Sleep
+ self.rate.sleep()
+
+
+# --------------------------------------------------------------------
+# Here is the main entry point
+if __name__ == '__main__':
+ try:
+ # Init the node:
+ rospy.init_node('TrajGenNode')
+
+ # Initializing and continue running the main class:
+ TrajGenNode()
+
+ except rospy.ROSInterruptException:
+ pass
--
GitLab