From 3b12738bdbf6a4db870ca012b920a7ebf96440ab Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 19 Jul 2019 12:00:28 +0200
Subject: [PATCH 001/264] [hand_over_action] Add initial action implementation
 and parameter files

---
 .../mdr_hand_over_action/CMakeLists.txt       |   45 +
 .../mdr_hand_over_action/README.md            |    3 +
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl |  Bin 0 -> 4176 bytes
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl |  Bin 0 -> 4176 bytes
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl |  Bin 0 -> 4176 bytes
 .../config/trajectory_weights/grasp.yaml      | 1667 +++++++++++++++++
 .../learned_obstacle_avoiding_weights.yaml    |  828 ++++++++
 ...d_obstacle_avoiding_weights_1600iters.yaml |  828 ++++++++
 ...d_obstacle_avoiding_weights_2100iters.yaml |  828 ++++++++
 ...d_obstacle_avoiding_weights_2900iters.yaml |  828 ++++++++
 ...ed_obstacle_avoiding_weights_800iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_1600iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_2100iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_2900iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_3700iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_4500iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_5300iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_6100iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_6900iters.yaml |  828 ++++++++
 ...corrected_underhand_weights_7700iters.yaml |  828 ++++++++
 ..._corrected_underhand_weights_800iters.yaml |  828 ++++++++
 ...d_obstacle_avoiding_weights_1600iters.yaml |  828 ++++++++
 ...d_obstacle_avoiding_weights_2100iters.yaml |  828 ++++++++
 ...d_obstacle_avoiding_weights_2900iters.yaml |  828 ++++++++
 ...ed_obstacle_avoiding_weights_800iters.yaml |  828 ++++++++
 .../mdr_hand_over_action/package.xml          |   28 +
 .../ros/action/HandOver.action                |   10 +
 .../ros/launch/hand_over.launch               |   10 +
 .../ros/scripts/hand_over_action              |   52 +
 .../ros/scripts/hand_over_action_client_test  |   47 +
 .../ros/src/mdr_hand_over_action/__init__.py  |    0
 .../src/mdr_hand_over_action/action_states.py |  189 ++
 .../mdr_hand_over_action/setup.py             |   11 +
 33 files changed, 17794 insertions(+)
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/grasp.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_obstacle_avoiding_weights.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/package.xml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
 create mode 100755 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
 create mode 100755 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/__init__.py
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/setup.py

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/CMakeLists.txt
new file mode 100644
index 000000000..2cef07199
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/CMakeLists.txt
@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_hand_over_action)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roslint
+  actionlib
+  actionlib_msgs
+  genmsg
+  message_generation
+  geometry_msgs
+)
+
+roslint_python()
+catkin_python_setup()
+
+add_action_files(DIRECTORY ros/action
+  FILES
+  HandOver.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+   rospy
+   actionlib
+   actionlib_msgs
+   message_runtime
+   geometry_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+install(PROGRAMS
+  ros/scripts/hand_over_action
+  ros/scripts/hand_over_action_client_test
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
+)
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
new file mode 100644
index 000000000..daca0dc62
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
@@ -0,0 +1,3 @@
+# mdr_handle_open_action
+
+An action for handing a grasped object over to a person.
\ No newline at end of file
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
new file mode 100644
index 0000000000000000000000000000000000000000..a6955f53292b26d734bcf8789324d89cbf5d5207
GIT binary patch
literal 4176
zcmX|^c{J5q8^>`($dGxS=iyXBhU#qd>~JLG;W+1zsrYe>8Q+Q|5h^Mfnh4D@+&GU)
zMC67{U8IaRY0%9OUfuh?cki|KAA3LF^;yqe-{)EDNyX8QL?4cb$J4?i1Mr8V!x+K-
zk&*uK+<uV(wD2QQjL2vjgBxS{CyM(B{hwEi)t==9#%9h+#c&xII|<vOZqK^8ap@Sx
z|3c}E_=o^*jMIaSg@q;15Zh|cV$Yg^agi`?b$gr#>*noVjK|5z$!=qF{e9T8FtBaZ
zOmlI&_e`Cw3U3KG<DtQW$#)6#+uYLX_&EZ`%fR@kK`cRk<N1R&*#abN`!6=`7h5oh
zJJZ~zsAq!x)A(<MH%s=iG0$@d&Xb=rzZ`60hA=~}jkF9BShGu>W<D<_gm7J*uXb){
zzH0Y7Z14q`)04TJMz=$lgPNZ#b>EE>Fd+se{4Y)97fqCeiTz6x|C`oz!br2mKa9|G
zKl$^j+!ca3gxpwH$|pFJ0(ZMVuOkR|n2@fu*AdQ3-j7qbSw|R@=}yvVt|KJjoTfI2
zC4`!+5e}<@A_6ACz$B>|nAGO)*q{G@j?%xVG9*m)AL{vZOm35ECcwwS!riG^+q&!r
zA!nN=&jwLorZne$o~S?Aw!4en8uEvyR?RO~qp6U@O<H)-MFoMzPm$|uR0vvlBj-h+
zfpq$=J?htKP)|HWsl(C1$Wl@AoirWplKOC@cQkmdmUH^de`s*#V?}PJH4VnoBLbvI
zH250Kuj80S11E}8IirOJHz@Nt-TgF(IPR8?@1cR9zQY}-E*ji*ep{3>N`r>lY`5df
zG*~FwN+xV_jxRP0$4=9rc;Afnt4bQEcUStP2GU?Fb@4mXf(C7&v4$`FX>jlQT4Owu
z2AsSK*{tSt_)}@%m_{2Nv?+S_d-VdKY|UWp<jDXS5O-6#)foUf83(`h{S1H&6Ehy*
z4}|D%uPQQ^1K@3SSTB2X0O<K1y7?qL07MG!?*66}09yGkxo%F<!KN;_+TuPPBAeK}
z^-}4ef5m{X&z251UDQWT3eaI`Kz!)#_-0<NPuB~c(csM}BeV`^P{m!A{cSVfDv>6q
zw{tXLvy~JQ*vzjdRZn?VkPdBeCj1IgbU0%)-Q6xv2Op|V*F6<FNc=FyH!0A;z%jaQ
z|7LvnOH%={dKy%_?A;n_OoJFcGhfd_DtrspmaPBm4`*xh5-S7z;kHA_>sB6rkSS2l
zj~Jx@Be8IxkV%1AY1L%haSE`pbw<f>QeYjb501tALZSZPx2Q-T$loZjaCPy59{pEm
z&!u@lAS-KW3fUc0FLaeR3c16jeIdRN&bh-#asQjeTo0(6aA0jr^aPV#SCS>4dO~M%
z$N2RX4=5#jz2FdZhnAIhEgwh+pvYR_snb3e7_bXz=f3LziGz<`mMhypMbGO>e5@r%
zWN!?xtyzE+`$xZRWme!MX2YHR*cPJE@zFIIM_^BmrLj|pu)TeyoiBz6xndE!|4?!S
zsFFRsB54cX7i89}YAoP~&qB%=!4&YJfk~QC1jvb!8Er5?!1(>xvVo8p7>NBj?qz2I
z;_hA^2bL{?^I_fGA+UlBYW2YbJyu}c)L&9_%?hG<Q*=^GtUxKOD^22s6%gnA85akv
zpdz;-nmybKs#?PAE(BSElBZMw?W;L_K6J}hGR6$pb#AV{_B4e&6UueM69jusi5eFq
zAW+nC3{vAqutT(ADqna%w5BAGGyIG}{PItHvX(wn>Ph7C67Z0I(UnhPK>==21XP<u
zxS&?$3bj-ehbRrXZ8Z<%P@m<aVdLvsDAY1mW>Q-Z4F<JDnzb7vMWk`$#C3#v;w}-{
z>daAsvam?~X)6@39n({(YK#6*nB^?P>`{zhj!w6hBf3BSq3X{Mj;Lwib}cA4qQ^DA
zx9Aqwp)mu6vT{2sWN|*twec}Rvp0Ac!eo6U>BqKu_MH+EiDfCSws}wZ&f3%}<|76)
zj-&YfM-@SHB=UnRk1~Yilpo^RPyj`{Kd;_tmIiU7tG)f%q7WXe)@`*@1gwXx?@b?<
zg0|71on=>4;CQik)XeT(@K<$l!pyW0w122{BiWk4!)K44G(C3*;j>O|BG)~E$MR}X
z`&S>hlt<#yDy9HU$9bfpfeN)W>0Ml#bs`e2OepcBgL~y~ZiAb3fo(<Yy&C>t%1?fB
zvw;k_pJEzX5?(NE89ZHY<^i?ub*~J_c|c?pQJQDj18R7kiOalRa87+XHjUF8cojbA
z(U-kp;)qzdv>+K$>ksnV6MdojX3LTFBYrS<CMKnehXUKDwp@|opn!nPqvlZ?KM>6t
zDJWs_h2GBhr?PW=;QJLk(<RIs@D5sIx3_siZ(YnAm;2t32NUc=CS;(r=P*08{h+Mh
z<|p$g1v2&~_&ieghoo0*DJ3`jq0Pas!cm+GM?`*T4qT$bDPbw<{=GC%&5LYY|4xIV
z&@YpIPd59cT-(dhD*^EMY{;9e(}A#L^h9+_#vw4y@mD_>8w3YNR)xKFg28}Le^oXl
z7&bh=cZ9420Xx2keeLxj5ZC>t6CoW4vJ(9Knrd_i;ECw2bfQ8=k*^`&TM7vF(YC*w
z^@As)&0EZN`$3H2Q7c`t9~?eT9qQRifdRwxlGy|bgwgoMgC;1@m*@~ZqvsFmy>ml$
zC;dV0wMp{}UMg5I>xmbCQXz`#a?I&yI`FYAMsP$1LS-KPSU^V*Ot6a87dnQ54qvYO
z{F}pIzWa$_y+#Byi;ocBdt(q$l*}&{9|^-kD_v$-6dc=oEOthl0UhTB5;<}hP^muW
z@FR`^!pt+=%fClKu`$V=nimQFJgn-uT@jGb&0%#U;4nB#-CJX@2E(PZqP0Ws>9Aiz
z+D6ySA5<m|4K%*=f$5_@g)2YZ;k4R?<%^$OVbG<nTn=-Fu99_8Z5ewAb&`^q5U>X2
zIM({~P)l%{tPu#_>=)&h&Hmmgb}+QA`z49p89oi)vM+2VfsR8YyK>+G5E%*VHkUjA
zpVPOToey#WE3u%|xVJXISt!k!{lfqrudf$2<;$Q-`?<I}adQ;6*sCqr;EcA~gqo|&
z9z<IcgI8nPy;1V1cRyW}{7`(jsO0lF3KA;Y;yH7hf@V}5w>|KppyaUu**=0FGU?<L
zCy9_z3cGWl&~`8MAz>i2{7*Nu8mV8C6z+^Rp5_d-956>K=Y~2AIOWmav>{zW-X6f&
z2pl8i*+EX=7H5@QSKu1DAN7{v0bwccZkyM5gG>F@i;|^ekk&S5IhE=QcX0VN<3+yU
zIxi*tT-z6tId`Ar2_ysCXoSEkHy^-zen0(n+6&_2mW4@^9*{F;k-LFA2vV#?a_?{?
z$TGfC(Qv^Q95SEDiOd>->hxs5z;zsIba^al&4y4v4UIS3JE6IoF50n)Zm5*>HP|-E
z3!NqGJePT&j7+V<?_h1d=+1?c6VEP@Q8usMf{&XQLOPQbp@D8_?EKBlH>^ZN(fRUK
z>!TSuaQDp5Rpp&Xdg-JYHapK;^*_UZSyltOMpIkw?%fTai9;=c(FPEH+fYA8)essf
zt0lwA#_)dP;iSateZby$PB>%D6e<<$@@FHgfM$Rv4kel(CHMZUs%kvSmK!u(5|V`3
zRb2LdQ3QS4T9bO??ZD<+QD<126UZv<Yg<^dhgUNr!#CJ0fU_FQo6FV*Lw-?_JY_{_
z7^rQ&a8DlAC>44{-`(J?U+;EU+zeQ{)=Jz$Y(c`x@JE!9BQUw2M;ktKgn8LnMcD-h
zcs75v>g!Pl==rKZ>f~{R<+TUrwwXD>jfnz5J3k@_mhYqftRX^(5LuF=%n5Rpx)W)`
z_OQ#MTiL<Z8a`HenFM&7ZuSSVPmjC-j5X=zznoHqyn)kuABqTpx`kG>c03Mn_$-3m
z2@XgV@6*ak<^k!yG`rJewt!HyY->pR7;|o=!j)LOLO9X%Wjd%2haP1V`o=mf6Ru^;
zcDUB}GPy3j-LqX~gK78IZ)<zrvx0jlk6Llp1hcZ07$J0PE0W&3KfO>(0aX_{o_uq3
z2O{=c^kkhkL`LCd+7BvB(YLn_TCcxbqvJO_61EIGA#F1)yz2`Rx<n$9r;6Or3m;GB
zk6d?j?4fbTsfQj&RpUj~63G)`ms(WwJ3LY9tG1X2oBmkUfKqJ33+a45rgOj68$D$@
zZ3}So+4R%r+^p+9NMQWg7e{v=#M^j`m?-a!^g6?jYEOG0@2UB4!*Mrc@4tP>Ig5l!
z-$`E{kR~F<dv}i;u-l`tmLd+JOj{%wXdj9?>`;MB^HdIb)4y5jQ~IqgXvDq8aTgpw
zY3T~(3H9zM<<9WQ2nR2ebj*03rs;!*^)E>UE0Gay%Y{G|BQknfI-~dXoDa%aNK<?k
z>5Yu-635(^JW<d~3732N9*DkJkQR(Th%_Y%Z%%%5MYa!Zq~;#DAi>zA+bI+xdi8MU
z(t?cxvZ-vDoN}{69+x{C?5MWLY{tSXf87SXoVC|%xL|`mSqD^=lvtxhM$S}Zl_fF@
zqxmnMG($(!ZoQSixEJk+^@~fW)<wJQf=|7jlt(7N^UtUsn`O#-Jl-EKC=UZUGG^hS
z8sIC?%9j+U18eJ|3VyZPFwL?n|G|nHM7Bm|y*expp%$YymA-snbLU6YRrWrDjiT6K
zurL=g3Hrdhk-?Af^E0-ki#RmpEUc^I^N}gEb!^dMN(69140HEEW$?4(r4;evVLsr%
zo0u_OFj`ElxYMf-_tPGja?I}mvGh}Gi^vE*QXiAWhK;~_PsS<k+eQ#R>%MTb*bt&v
z4AqMg4Z!O^!8<-G?S@)chj;mWI#B2&M!s=Q0jTlsuZ?=MK{=PVZB^xV<h3Ngu}D`z
zp)L;XIsH4(+1>)YTjp-`Ny1#RSW6#?zHk|zxTc3>`8ib&^Xj6<=Tt89UDQHJfojd>
zn?Cl@3qJ0?lN(jdv<L^KEi#`>n_7PTV>|Q&mvYV<i^KjuI1`juB_LkwN#7r4q9CYz
zU2pqkK~U@|SSt+`0Mm-9^kT>D;8$WD)W0MGX#z$4uQ6HRmR|YIOIHP0erm=FHfn<!
zU*-v`L0$N$WrT*=459ObzPk3nJ}7@hU(!%DgTJ1d9q_BP1WcZRDf~OGD*hTAl}MQK
z--F``Oy$?GDt`3ZzSD|5ll#k>^c*s5I``vFF69`h%M&nF2B!9}R{fV&gM?}RPpkE}
pR{po@59gZ(38#v^6#r7cOCT@n5nW<G6EJNCrbA_5_;hRs{=WsRq1yle

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
new file mode 100644
index 0000000000000000000000000000000000000000..3a77140ab1594395319a20cd99c2ef735c6a0612
GIT binary patch
literal 4176
zcmb`K`8U<u|HpAek$E07&rT&|s<R#QcsR&-IL<jlD&8DpgpVRq#)?XYCZaio8|S6r
zA{lN-)J4j8b9JSgA$-*ReD7cI-FvP5!``pgdaUPOuh(m>X9|vfH2O$HJf0pN8H7I)
zeV7><7#SHD&l3<CL=Qh2#f*%mGkGw!f1-GfGX8nR*d5qTVjLFiGz^!4agwnu8V>B6
z8<&o8{V$ZkjE@N7!MMFR*x1;Djj*i_Y!2)h*fui8qv3$_V&A+q!+4#Yo$WU^*U!U&
zjfriiWm-sPvNv>BXN?d@74G{k=F|`-cBwbLPn#lOd`yg=7Qz<t6E6_5$rdDILO<BJ
zA8g?eo=gkdoI4f26sWWiw8W?NSRUnsg!yQ_=-2J6)ne_N%k(jV?zM$2pL?zlY!+-6
zqnIyQ58rkhrHFU1I_iT(S3@td%Ce^0Y$C=9m<SUS{g)>8gC<VKB>tsI{-kZ{_<vcL
z6cdxCWneO!zvFQB|1`?}pvsXk`G2Tq)3F_!RH7h18yiojR{itk060|8JW&urh3SgC
z4~61^VAt*`abqYDo?5qbtVYuyiHAJj(M1EnravRs*Juzj|9XcHfey0iyZ35br9%Vh
zAoUTB0mfEJ((h#%aEIKFBfqD^EA_mb)BmBv=}*=9nKpD7OOFVWA=6<YR6y4$n-0!Y
z=PG6^9j;O5^127;5OKmI9p6g_VFSn8&Rul4<MOUFd4vv)^|>A=mgz8Gx|KrM<eXS+
z9FCo$L)pG*ot_#xXmr>3r3BMqD{b*B%aRUlVX;OXfpoZgb*(9$MF(y^#awm^2K=Es
zn5Nmr03E8ngPDF1RIVAWojMf+gOVPqH#&nrH{-yU{_jDsVM^o$fnbRK(o>ze90c#`
z4)<}k1cAQ)!Rt@LgFvk0&YmyIL7-jqa@+L@2G~9dt+TwxfXHSJU;PvY7+f+W?6YIQ
zbytnIrvw?WG$=WAXKYiK+tc;pXLNWy!VG%^bg1R2%>A;dw^pp#`Q0oXIP9cF1UL2d
zrs%882s5B9&Qw5Ah5@IIr@Gq}7~n_K?YgVV0I6>#_+~{07&=9_?ca<Ke`zi#(Ljef
zSF^2QCUl75C;EGr(BMm`j&#H4Kq#m$OsokCgj<e>UOndx1i503qKFYHFcV7#OITEx
zkySg6J3$2wj?O4KZYr!p-GR|~e<(3{^Cc?M4~jO*E!|vwpx2<M;7qC)1hcbOBvU*=
z?Oaz?lZYo=*mubP{uxhrTQ=}|G2aVn#vR$461~B6_od^~Prae@_=~ZtD_&4R@#)|a
z_Jr1z_pKkv?oetY_|$oyD-7BnYUjD*2#IeVzN}KQh3ei{HTYO7kjmW{<XE!=8O~1u
z+bgZXS;Cel_pu#BqZ1=*a!$Z`JeJN$C4o@;N;`iH3GyW(_WZ8w1W+rVvm$K=U+3l4
zYj0Y@H^2GhQGz+(!-A8vq6m-|B{$M&h=BQ9+OnYt5ey~%81u2W1W8XHFZX3D;C}FE
z_8?e82CeRZd#^Q^G!K;DykZT}e95{g<<_8lxGPnv!x~7lfz0!R)=-^a9nBeT4YjR@
z?azf+fwH$uF@3=TJ|Dc{FC9Y!PTlLPue{Bn(3E<W@C3o$EOC?K1O!UDP9f?72zH7$
zP8Nyohv&%&l#Boqki7UEe_Y!DYV@V@`3QJOKkvpbHLnOasDf(EV%wlz^%AW@9EYfl
z`E57v??C-l4~I>zYNIf#Sh)!uee@=zHImqFf|QWv(UVsZ>W#ZV;&^0%5>!OR8gi^r
zyiQDSjhY?$U2%rH5_3Q?!g;#g+D_=+*vHyGK02Z1!CUp9=!72M{H;~5*dC1<Dppq6
zTO-S}scub=5t_Ni%M_&;An5>()q?lRNGz7Etj_iW;VXOda|u5QpmQC=??0vlT5lsi
zy78*O;k>GYyc>$3WdFzI+byynX?(eFAXgm1L)E*jcZq?`u+8166Ee^?60)oEk}8}i
zlZ=|)vm5@dD@&N3GKTh#H6CO;B0PBZ@JaKpo*-J_>>+m58+ffQm$onX!G%KdHtjMh
z&~;tjRyWe1o-Vt4+h(7LMXM0Xy&2$H^NYuu&A!04qV_(`Krk1eJh|RT0o->9O>HS3
zn6e6;Y9M++{Rh2EgFCz+vX&&vyX*xw`CLfLd_HhSV=6Y4+ZXs0Kk74<ePR5lM7XRl
z1yULg2sn`Zq3(L?(e<MNFnc;Cxr>(yLX%rA$#79YQ0`&Nh;0CfXTL2jXY+@?&JS6+
zd4BNq5}xIH*cb4Q+M~C&`$FHNnAfiNe4!A=IfqOsKyA-sz0e7O$^qN&tYcKjFiY@z
zs1XQBJsip9*8-u<F`(K>k_Jb`zG)3!ph1?X3~j#|9n=aVo7TV5p)~BTiGU}Yb8<)9
z%aKb#@VMa6>+GChSTcU1wk6{rnB)a&9Ec48_qVH}zPh1cNNBh$e<&0-yuZFUv=RcG
z_)^ZbR|i2-?~87PY%s`62?%JZGa!gJqPxbK1{tOPM*Q!nAly$EdN~sSPexj{5cdQ?
zjM6b{JxTx^IYArh-AaW)qxABb1S%Y+^N)p$Q=vc6F?w1*5H$K`hwM)U!j4y_EggI`
zux2%o&VQ#t)V7N;InfN@=U9y3iVTLDLPlE9ix3!RmuM()3IkpKe9yVpN5Epw6X6EU
z2xyUfOZwo8K}6|s0g3oX7#3OSB4SaHW|kH^t;2*DX9W|v@|aMgG3)p(jtQcy(>%++
zMM0Sf*^^cn34y%q8u?ujkkHL#eKhC@xX9dHW3q?Bg#z*Vp$`n$uPJM*=Mf01;|B+u
zUi!h*F~5?P@1BsOes1~vpKkER^-<Lh%mup2*Tr?@93aeDMs8ft22|qM8`8t9z<J`P
zVCd$&sIqDa^i8&hp>@5#k~m%9&*2*mB`svob&TXx33dmux53>O((dp%eM`Yyh$~o2
zgrvm1vjy%FS?=6#hVXcOy`;HF4%Il!#yyg>KyizGI>L=EXsd0Qh3d=!v^6nwHKyGc
z9nX6I-BmdN#fOVa{~AX{B9&Xbr*Bcww3^fQ`#w~3d~{I0pAdjdJGmvvVic6j=@Kj=
z<bysY42D(x;el2o4Q?idyP%Dyc|)!47HH+n&<jIu1!R^wq(>;+3piWBG(w?0<OOeW
zQO$RQZA15>-ch~aaPs?G7B_vtwc+x4=?V(S>R7O4rTD{bT+z+3Qh#uplac*Z#~+S!
z?>WUAOaYFO2*Dl?Kfrr`%~_c8f%v#(QSyWr<c(V9Z{QAq414L0_c$_Sn_Q}HJZA@v
zna_5J%@~8))I`wWRUB$^eJpOnfzSXQjkP#9quJ}OI<bi!sDiu@YM11L3JAN-WZt77
zbL;TiSerk(eeTrwvkMfI%cno@=i!5p?nHH1um>7Fdp+|tI|)&B|60)gL`3d)PJdrj
z*@a}6P7$%0Io4|6X@QIKn$R_p^8Ak39{5ZeY7LGygz#HN26<{m&_rD=A67Ae591Fe
zq+aa<&ZaY>8EfWHqiA0=6JZT>Lp*6H(G)3r4rJHX;Zg36H|9$s(lE1%%iS-Ipnv=G
zq`r82u>DfndAQ9P<dyfe%`Z7X&-B~jYaEuqU56FU<{E&JfVfzpiV`#q*0-Fys{m`%
zYJHOb9`H42@Hiq#1h%fVa*soHAZ2azEy~ylSUkT*8$EM^Ir$kS`FTfpHg~yp;g}=z
zE+~>ad7WT+?f#kVL}$1*UMy@MKmy^aeYEd4NpMJnBF$Cl4Ef64iS%Iy*lpRZ;%H|B
zpK5(fgM7_5=L5yBSHTcQoArubPO3rSV2;@XF%i(P)Q;AP#{mwXO|U=71u2sK+S$i>
zLH2L0?o_!gAQCPA{80KRYj&mDja0TmINAKyR7gJ#J<KTak9A%qT*;Mx;nvW{+IHdH
zULn;Dmi^zqtnK~44xVAW>SbNyteWSf2$36Gk*wMN^b&1FR9EVB>h-amh%{i?n|;;@
z8HZQu+^;rAU*0)tzxrx}PF#PHuw~d8=@7N?ZXINFflQ)ImU^HLKX2Bzd{312z~n{N
z123ee*-^Vh_D0x+R<)uR-YBJ~E$04aeXMFi1-9XXbU&x*-mCXTPg%~}gFO5;>*;fT
z_EkS5IQHx>Cr>}b*OW#|RPaUmo#DrHro52v<XpJXm<MtQ6dH2LCZme?vKI$sNl5AL
zofC$f4rsKsluIPj4oL?)grOJqs93ILGLN!Zzu6j-2G3p5ThCsn-QbQ=(-o@{8az?*
z?cq}qjy@<U&18<Q<%fn1F35x`QxI;;xnMS93VK;Ft-o-_4`s}!Dm{zzMJD!%qn=CN
zDCDJ->s<pc#8@m&4aFZoT2dv~C%(8Ly9c&1v-e$*aBR}8WGV^uJlM4~Z|jI`Yg#8J
zJ?xR!#m+{1njIoeTly5O+oG2<4qA=pY|)=KLAB-OHfWKVHyK%Lg@}jgfs3bz=&1UQ
zcM9jt(9YO^xP&@AwA(&3>)nI`GW|_pS|e?SrQr2=f4r~)4CcuZ!^1SeU+_79Qk*WV
zt&1xL)a$?$+wP+KE9wyWJTklIhysLJj@Z`t^Mmc}Z&8;y`w6y65^q99w;|Jzk9->$
z0ti1hZC9~~Lz6C|da8b(SRz|T7cD2n04Kt<@O+~J0rq^<Qa(J)1-ZYD8Px;h#gyvX
zeFktZ^}adR++L7K&stkV#_);um?AN33^sc+vUqM8L->s6{IN14h+;F+C`~j3pZ|pJ
z{G_}G>fIdQ7xC*tiL(Ud+8ITl#ecXm;>!V5+kEY6YlM)`k|5V2LluR&I=1Hx>_i29
z#dwd*J?Kv<3+Xa#10>$zIyQbqAIS@Fs~zFfLyym>UgSTojgo@ZTP!x~*iS$7gy$|E
zR6E@&8j`xmdNO5hweY(T^oCY&&zVTV{@=M1l-Z>qUi(S^??iDBR=KJ#bWs?TdW+X8
z!UVy*x;DMcNeBYUZ9)c?#2{6$bl?>x4?MCfzxe2>0^4`3Sm7oeQ0LD)Y5hhIK4}}H
zVGbkc{Ai$|Gq?|`dKgQZYDD<^DbYQk#tJY6CZ_nWT~+#F9F@tK%1`5X5>x$QRwa*J
z*_Wf#JF&mAS>G|kwsSx3)KZ?Yh5`XoV`A$6=4$-N)g)tD|I5|>nae*z)lF-eB|M5)
ea1#@FKuG4&H<QlXw8=V5Oqa&Q@afo2{C@!^_I?il

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
new file mode 100644
index 0000000000000000000000000000000000000000..da6da9554e2b676b03e6c9e9afb507aa89e1fdd9
GIT binary patch
literal 4176
zcmX|^c{J5q8^>`($UKjkXQvV}<!r}19u6`dj&lx~iXX=q;Z;-;8H!4VZXq-)+&B;A
znj*K!P?u81o2x6`5PEg*``*3R+JEf*JnOT*dp*yy);9x3KOBE3CK*qUjt#*dijQK3
z2gk++C-VlyhR~xA$1!8$=}caX?Vl*#!;F7kF?J`m;~0k(I}O9-V4P%Zo2C={*2ZOH
zT>lGYFq30Kcrk7t4mLKnP-ASn6Ppuz4#q>qcr~4HKI~h!<`|!=tE=Ppt@Zce#Ky#S
z&`w%O={Db7OvJq*3_goIddsMpFk+Frt14xVfblai0a_Sa*xz`;ur0O_8591&#{FQ6
zgz=uVvcF99REzrBNsxvS5xi+NVb_^c4|x+hS%up2jz#>#1ktgLPP}L{!6}S;7M<&3
z9YFh}pUFOAb=>qcu4=i)5(tmrDY@~EfQd3Ov43geKWGwUO!8lv)Zes}jDva8#*Kuw
zx(jz_zg7_Pb}CDbuvZhjRZqn(8C@iJZS-;+&#EUhy5UJ{%T0uIIm*LFd+G=S+3S?O
z3=hJ5z#ic%%GU{)G!v7d<zTW~zvFc7{~G0fQ02*(!avk=+1So4Dp5#)jg7ZoyQyn6
z2qMn5O`Q#+!fa*!`$CCeaOm-tygnKXk8Im}zs1uajhDRGJ3s@W)<0r5H)s&H_-dyg
zfev!nd-iEwr9(66AoT%`0VX!eGVkOVaEtr`M}9|#XBzpZ&-{lDXFk>zoV26E+w7PS
zSu!1#!UgqQa_Qhob**N0(BT?&A%E~C9b%4oW#fnFAY$lz({+Fjx7^;8rH|90r76$r
z*eV?s%eGSpTbyIdEn|r@bSU3HtNXN$4w{2?0U4om*iKvi!m_4AcVwb*Z!jHhU)^X;
zX3>G0Un!5>iUGf?3}<O|GeDPW;ACzP0#zGE8z)YLz_65;+V%br(91dS`Net&d^aQV
zfnX@ae|}nfay10rG(<h;Y!3m0z=JiN(IFsSdTZ}zl@QP=dcspP#Q^&U;SJVz7!cdW
z;ct+^0K>~hg#8W-sBzbPeL{!<E5lNwx882)@_4ja{Fn}}#+i{1fDZM%Re7Jc^wx{F
zxxSgF1BZi*sL+<ap$r4nIS~eQCz%N<$ui)K$;@DnA_D?wdIPuB7$E)C6yK)A03(<9
zZi}t>=qHv!lFf8za5vu`X-bC#0b-zUDGfe{>&i5L3Wl>yg{gHR!EnPl;#n78Fvu5c
z7R8KHftgx5T*{)toSgbm+%YO}aP-H?b5mgx8V*b(2STag$mh7&04VxiVeR4W2SbKW
z&z5BRKqxzVWje(h)Xxu8w~Bhh#r+Y1cT2qCb@|Iz%LP7AH|fmYn(7N?doCZ9dE^WI
zNBiDhUGsrTieE36h&Obsz3ccu_JlG!p+~O!-C@`<qKEgEGo+5(e^RY#54A(j>hOs+
zAf5Mpm}A2lWH~<u?WnQ^S4n%`yoU}DkB*IR$h!dN(L_2Yl?1{)Ydr!9Bq)%K+54M{
z3qZZX=`|S#__8R!S>I?4Ujr7?CkU2+j|@%Ijw3*Rocwr;5d!9~S*u2(L@<*4{jHy)
zHAs2;`FO6{0QbEI^9R8ea%c?)Jcn$-wC!a@<4?8_&!4WBQDF-zQ3ILMy|zG_4`yB%
zwuRb)+IY@rTd41day%bq11i3<#q=dB_;m1kpiBZ0IQ442J@d7MLNn@BLMMWKrzA{^
zQxGWYxrAv5BG@I-GF>EQ0bS`Sl$;<_kh-*vKdNI0bq3M}`~*B?U+@r+UQ~kXR3Y^?
zaUN(=yG*N;z#(c&L3iWbo#=(l{V~(4Iw;a6QGQC-0F8uo#1eZ<kuuUceEcdxLrE7&
z91pBeimI4+^J!a@teY@Yr|y7$Q<~$h!kkcoNWR{njtjc;_Cx*eA6!t|@Qo%=azPIp
zf9=pOc0?0KN>$a4w#fQirbp{TgyydCF~ul`NG6Em+u3(2NIa3PyutoG;R}0Pmt=q>
z(7BG_EsiLI_UqUW9(<}0m0x|3@4FHxJN|y<X1g3nnOu4PGEV}c!!-tNcZ-AFnBDD}
zW3tda9=5ycvKkyKmx`O+y9fSiC{LN4F@c^BbzWo#BHVj?zq9QZZxB1{>Lq^F7x-+h
zl=UnHz{Nr`k4`xi=z4CiYg=g0M3>vcv(+c!@v4LhUj}&B{p>Ze)fd=y)bm^`7%T-T
zoi!~Kz^zMa=}7y*j7|7VGtmc{-s@i;-suCe^&~mIRUc^NcO$Lx`$383OkyUtKkzGk
zFkr0u!{lMfXgLuIWHcWTbRq>pLrurw&BH-3e<mS)fR75o)7vh~a#2A@{(k$oeGo|G
zzAmm{3xwzW?@#6B2f&xhc$RyVKj58pCT{HThvyFxUb)}#heDX-95tf=wI`p|ryB%S
zFYVV^N2rivo)U0hGZ@mIa->&W3x;mzpjsCx8XOk?sy%#>2B*YiX%^;mP%n&a-TXp_
zvdBNDf;zY6<j(FV<CjC=;n|2+xu-*6#iUbxTh2i+%@5W*kQfG@ufK`;>xF|6q4|nJ
zL^yo+{n8h)76zR7GR}=>2SG~zvtEo`C@4q^3TkUGAcQYwu+EhRIc0&y0&l1w@`5h>
zWG)Cg$J@6N_Xa_N@)28oN)Q}6MjIX4PK9CP?25S*Dn!u*-iA$5;YF%*{H#GRXg;4G
zbvzLaJD-`g_wv)gmeowUuug+Go=XX*;~5~pu^ht{8wz!WjI5BpFqmYQY%X<)1U-QQ
z?}b-~z-n)&NV8T9v`f7vz4yl;rtGMoWO6KwiLMP0u{g*w&q|!tWkTOMp;WGXCe&%p
zJAX}Lf*9)z@9M8{P;N^0rWM9QFdw^S!9Wb84072X4mkvFvbQ&w?BQ_ntVGl3dj?o&
z$=U0B1%uk;!Qs{?0WfnUpmc5B8%}GSU%l{$2aLEssNRXW!9c~Pgs!|3M7qk#PYT(A
zY7%>McBBosPBjXJZ_SHpoAzM;bVnH7)c-S$(+&O@yY5ulP6j>aSWeYYPY{0{I%p;1
z37@jJom~iX2V2RojHEaAz+EcGo%huU9&T=ywiU^vI;Z)h2U1ojY5BRXNQ)cVZXao-
zHg^DRPYwT;(BqGeo_e?Lt`dZjqa|d1Nunars%^frH>hY<-DSsJKPoypF|6={5QNP7
zxuwYB6qL^C7Ah+2hd!hXM^^vtg}%iaHl{_pq3@6KM>{;N&|1l8pAokrGS3{<Clu}j
zoV`#Mq0kZXL$|r96?g#8=$*JXR3C^+e|N*G(I4EKuUwF+q=1~R72BzdK)8u3YJ6K3
z2p$Wva=+*X!cp$MC-_1sz%d>p^wcW=@V;M8FU|Nta?+|8dCCX!C#(y;;|_o<d)dx+
zI5Om#UaoC9?*PsxAMX^OGXeFPsgU8TIMnL?P{NJ_p_g>@w%y4U&DXf=CZ>9!O7c>;
zLz*8tOW0j<@(u-A+D6~Rx&zV8^Cu=BU!<TseuKpTFF%CzrfMTYz0kzDnv<{CNr<ZV
z=aSAxBJ#X-X8oJ$ZX~yIf{4v6u)YPK5xk_J1q0(5UAN5l!Y9&bM`*keMBgwr%vU#t
zR_eEkF;!D|KY4FT`q_TqY%LMX*|3B<CC8$<7+atl;Yp*ZW=O^RWo~@~9_8&Ev0M?A
zfw^zEJPQc~FLrdLJx_K7`_E<lQQfYfpt8Svam5Lq&b}VI#$gTI4OroPo*@_uN{AP#
zDnrX~Q~UYbim*YgH6R7<1%Jb4uR~HqU>n$|@QQE%X<Os3aV9Rn;{7Gw_^}HtD9kA<
zEIPyEg)8+-N1S13Nr~Lg=K`x6cT08<UE$hfv4~?32}G**)7BeF5FtvD;i__l0+qp3
z`j`{!u^v=)cCdqw^?qg{{+3(wff6vJXao~&`bAHs)uC|swD~=8QP8y3iPufW0S=!_
za6HZh8B#BFa*y(X++W&*ney8}G+v=AB71^0zgFu(DqkZUZ~Jp5>;(?p&nXQ|bX_I<
zl&8?=(fpjnbMehSVYTlp$G?8w*!P|tyd(KE$_FM{bzP(w(d*lhoVi7Isg4qAD04aS
z>c}ocdTBkBd(Ie{L|5tFt+hm--#F_$`(lTV)%2xo8*@dvL>;_GFBx4VlPJ?=UZ^*~
zm-V&48)e-y?K^eP2dQiI)~}F#5q7ady{OL@WjyUpxVu#!-?X3-`|gMIK4t0MY4S&p
zSgt!lyaKlB=~F@O)c_>)_VJ%C-T{ceHH(z0=#LEgqmSs$_#pr3g=piqUdSm}c+@SI
zj4I#BT^g1nA?4e*ju~+}p^1(%F42<?NG8-N67@NvV)^#ze9Bh+=4wtGcDbY1-a{^X
zz!PO=D^;g7d!zK5V<%#q{ZLw#=>lCl0F4=5lnqy*Al$a|p=>4;^rUjuV5uYk<t%0@
zKaTZBrjDr--YdQ+?1{AdZ9^Z#ST4>C#~(o2(xo+1pFNPnJ$u>tyY5ING3`b=m4u$&
z+r6@A?~Ls0I;N(*9FfnZ{uW1?10v2^`xR~4qbGAt+AZhp(I0jp^%WI%XqlNm9b0dM
zh*9+5<r747SmXK|#S7+WS7K07N`pSy;~0MG&6FZC`&DpOGi#2e==0DbSws<r^W}-r
zky;Qa)FqIXqz4<D5=uc$x-i4Gr|9mQ2E=y7<~}{72$9y~_H}^*V1M&#+!f9j1p6%?
z6fVYt%)&nKf6oy__=Q=A%4Hmyb`#TA3;4(q-9E8wJuMD6QKpsmh$;j*@>9$B@vso$
z`6^*TA54}rYHvO_ggcpcEx8u<fn@fnjb&s4A88LMl4B-dw=d@u?+p`(p7UNjQf>@!
zY{r^psYc-UpYUBDRrW%Yhx5B40X-;nm84uNQ36`>`=7@BIiQ-y-=V%v82POTaV;~{
zP^7zaPyWkY=<M@iyw}OS=nrWtnQ|RNB+={scJe0!q#($xeu!TmJuFeXByd3orG;v=
zTW!^GfI;{%@7=tpezrp_EOVLFIb&(F^qVjYg;#Pfm`Z`gZ`>&=?9z~|)A`~zq6CPj
zUNsQDBm&Aq#T%88LSR{2pIz=E3_%rkVJ}z2AycUA<ugnHc;(i9_S07bwsq}9kyc&M
z5IA|<c0?aO>X@K04rA#5V5q4(ydSEcGFG(IiSXAWqGwQ@4Pc5)OzB^{s{F$^s*o|&
zzm4N@OznqRl{)g%{?p1sQx;Wi2F^M5{T8?rEBPjxiUdrZiD~?stN9~Wi;QXiFIVU9
pT-hY-fy|x}f`G)CQLXyBgy?%UWmUGH2$(Jt)1xsld^WZV|6jEhnUw$l

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/grasp.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/grasp.yaml
new file mode 100644
index 000000000..2bcb6488b
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/grasp.yaml
@@ -0,0 +1,1667 @@
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223,
+  -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096,
+  -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595,
+  -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776,
+  -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316,
+  -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115,
+  -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066,
+  -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736,
+  -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944,
+  -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777,
+  -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774,
+  -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614,
+  -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464,
+  -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514,
+  -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704,
+  -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118,
+  -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555,
+  -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456,
+  -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158,
+  -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113,
+  -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834,
+  -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257,
+  -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332,
+  -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204,
+  -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437,
+  -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826,
+  -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198,
+  -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525,
+  -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501,
+  -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623,
+  -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465,
+  -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925,
+  -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477,
+  -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792,
+  -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102,
+  -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663,
+  -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824,
+  -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146,
+  -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656,
+  -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234,
+  -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734,
+  -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934,
+  -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986,
+  -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363,
+  -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668,
+  -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839,
+  -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874,
+  -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192,
+  -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154,
+  -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287,
+  -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938,
+  -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163,
+  -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446,
+  -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945,
+  -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823,
+  -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033,
+  -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043,
+  -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923,
+  -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432,
+  -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889,
+  -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596,
+  -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364,
+  -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604,
+  -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642,
+  -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686,
+  -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615,
+  -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385,
+  -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576,
+  -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794,
+  -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084,
+  -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425,
+  40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273,
+  -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242,
+  -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348,
+  -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675,
+  -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794,
+  -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823,
+  -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525,
+  63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779,
+  -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285,
+  -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233,
+  -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494,
+  4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534,
+  5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616,
+  -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531,
+  -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582,
+  -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527,
+  -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938,
+  -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925,
+  13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073,
+  6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941,
+  -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595,
+  -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226,
+  -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772,
+  -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734,
+  29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656,
+  25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073,
+  -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377,
+  -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623,
+  -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374,
+  31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135,
+  71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668,
+  70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188,
+  -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115,
+  2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502,
+  101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024,
+  163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724,
+  80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944,
+  -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563,
+  -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664,
+  -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177,
+  -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046,
+  -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798,
+  -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077,
+  -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752,
+  -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202,
+  58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274,
+  48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283,
+  -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057,
+  -123.43864361815416, -140.9579908724867, -157.005848574077]
+y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111,
+  -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728,
+  -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983,
+  -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277,
+  -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074,
+  -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947,
+  -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737,
+  -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779,
+  -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293,
+  -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643,
+  -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577,
+  -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588,
+  -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851,
+  -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558,
+  -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179,
+  -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748,
+  -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684,
+  -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735,
+  -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668,
+  -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535,
+  -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122,
+  -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498,
+  -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707,
+  -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863,
+  -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154,
+  -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564,
+  -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214,
+  -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676,
+  -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744,
+  -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956,
+  -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839,
+  -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955,
+  -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735,
+  -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345,
+  -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231,
+  -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039,
+  -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063,
+  -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203,
+  -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401,
+  -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627,
+  -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747,
+  -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766,
+  -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141,
+  -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347,
+  -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216,
+  -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654,
+  -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015,
+  -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534,
+  -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731,
+  -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451,
+  -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993,
+  -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378,
+  -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472,
+  -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751,
+  -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873,
+  -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556,
+  -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702,
+  -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539,
+  -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257,
+  -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107,
+  -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192,
+  -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091,
+  -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265,
+  -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897,
+  -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183,
+  -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083,
+  -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788,
+  -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237,
+  -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944,
+  -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927,
+  -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068,
+  -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397,
+  -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006,
+  -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678,
+  -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265,
+  -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213,
+  -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764,
+  -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582,
+  67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551,
+  74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474,
+  35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262,
+  204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635,
+  246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412,
+  87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006,
+  66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695,
+  158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347,
+  150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332,
+  116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529,
+  42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666,
+  -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254,
+  -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793,
+  -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045,
+  -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597,
+  -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263,
+  -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724,
+  -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274,
+  -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294,
+  -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296,
+  -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972,
+  -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207,
+  -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173,
+  -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385,
+  -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214,
+  28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786,
+  46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807,
+  110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782,
+  173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245,
+  197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484,
+  210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167,
+  189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755,
+  62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095,
+  -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295,
+  -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374,
+  -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395,
+  -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038,
+  36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376,
+  65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127,
+  -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059,
+  -127.19340520577649, -154.63036780787843, -180.74870760607394]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567,
+  205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663,
+  -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808,
+  -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975,
+  -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551,
+  -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826,
+  -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272,
+  104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167,
+  82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434,
+  -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583,
+  -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846,
+  -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196,
+  -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552,
+  -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175,
+  -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248,
+  -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993,
+  -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733,
+  -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155,
+  -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093,
+  -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656,
+  -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873,
+  25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458,
+  40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028,
+  -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413,
+  -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641,
+  -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581,
+  22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803,
+  15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514,
+  -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898,
+  -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569,
+  -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616,
+  -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532,
+  -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893,
+  18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601,
+  22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818,
+  32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779,
+  48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617,
+  143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107,
+  130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158,
+  44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568,
+  115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683,
+  149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516,
+  80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459,
+  99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014,
+  172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139,
+  113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729,
+  96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513,
+  62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735,
+  -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649,
+  21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656,
+  65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635,
+  70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916,
+  74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807,
+  136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928,
+  168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346,
+  121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441,
+  96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392,
+  104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364,
+  114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289,
+  98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998,
+  49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645,
+  16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556,
+  50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409,
+  60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868,
+  51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244,
+  53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015,
+  64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777,
+  38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126,
+  -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391,
+  -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894,
+  -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211,
+  -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903,
+  -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216,
+  -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931,
+  -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333,
+  21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149,
+  42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823,
+  16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427,
+  -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764,
+  -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054,
+  -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404,
+  -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496,
+  -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292,
+  -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934,
+  -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776,
+  -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126,
+  -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246,
+  -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633,
+  -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934,
+  -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297,
+  15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244,
+  26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861,
+  15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945,
+  36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882,
+  48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032,
+  34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864,
+  10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116,
+  -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808,
+  -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259,
+  -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883,
+  -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713,
+  6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992,
+  15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006,
+  6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738,
+  -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728,
+  -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381,
+  -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837,
+  -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521,
+  -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423,
+  -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537,
+  -111.02550604149512]
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223,
+  -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096,
+  -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595,
+  -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776,
+  -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316,
+  -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115,
+  -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066,
+  -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736,
+  -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944,
+  -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777,
+  -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774,
+  -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614,
+  -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464,
+  -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514,
+  -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704,
+  -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118,
+  -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555,
+  -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456,
+  -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158,
+  -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113,
+  -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834,
+  -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257,
+  -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332,
+  -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204,
+  -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437,
+  -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826,
+  -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198,
+  -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525,
+  -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501,
+  -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623,
+  -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465,
+  -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925,
+  -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477,
+  -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792,
+  -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102,
+  -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663,
+  -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824,
+  -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146,
+  -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656,
+  -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234,
+  -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734,
+  -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934,
+  -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986,
+  -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363,
+  -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668,
+  -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839,
+  -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874,
+  -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192,
+  -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154,
+  -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287,
+  -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938,
+  -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163,
+  -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446,
+  -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945,
+  -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823,
+  -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033,
+  -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043,
+  -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923,
+  -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432,
+  -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889,
+  -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596,
+  -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364,
+  -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604,
+  -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642,
+  -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686,
+  -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615,
+  -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385,
+  -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576,
+  -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794,
+  -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084,
+  -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425,
+  40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273,
+  -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242,
+  -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348,
+  -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675,
+  -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794,
+  -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823,
+  -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525,
+  63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779,
+  -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285,
+  -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233,
+  -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494,
+  4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534,
+  5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616,
+  -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531,
+  -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582,
+  -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527,
+  -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938,
+  -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925,
+  13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073,
+  6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941,
+  -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595,
+  -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226,
+  -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772,
+  -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734,
+  29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656,
+  25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073,
+  -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377,
+  -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623,
+  -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374,
+  31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135,
+  71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668,
+  70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188,
+  -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115,
+  2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502,
+  101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024,
+  163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724,
+  80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944,
+  -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563,
+  -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664,
+  -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177,
+  -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046,
+  -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798,
+  -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077,
+  -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752,
+  -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202,
+  58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274,
+  48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283,
+  -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057,
+  -123.43864361815416, -140.9579908724867, -157.005848574077]
+y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111,
+  -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728,
+  -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983,
+  -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277,
+  -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074,
+  -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947,
+  -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737,
+  -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779,
+  -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293,
+  -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643,
+  -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577,
+  -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588,
+  -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851,
+  -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558,
+  -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179,
+  -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748,
+  -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684,
+  -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735,
+  -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668,
+  -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535,
+  -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122,
+  -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498,
+  -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707,
+  -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863,
+  -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154,
+  -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564,
+  -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214,
+  -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676,
+  -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744,
+  -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956,
+  -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839,
+  -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955,
+  -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735,
+  -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345,
+  -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231,
+  -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039,
+  -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063,
+  -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203,
+  -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401,
+  -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627,
+  -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747,
+  -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766,
+  -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141,
+  -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347,
+  -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216,
+  -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654,
+  -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015,
+  -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534,
+  -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731,
+  -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451,
+  -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993,
+  -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378,
+  -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472,
+  -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751,
+  -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873,
+  -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556,
+  -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702,
+  -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539,
+  -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257,
+  -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107,
+  -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192,
+  -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091,
+  -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265,
+  -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897,
+  -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183,
+  -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083,
+  -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788,
+  -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237,
+  -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944,
+  -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927,
+  -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068,
+  -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397,
+  -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006,
+  -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678,
+  -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265,
+  -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213,
+  -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764,
+  -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582,
+  67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551,
+  74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474,
+  35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262,
+  204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635,
+  246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412,
+  87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006,
+  66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695,
+  158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347,
+  150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332,
+  116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529,
+  42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666,
+  -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254,
+  -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793,
+  -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045,
+  -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597,
+  -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263,
+  -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724,
+  -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274,
+  -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294,
+  -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296,
+  -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972,
+  -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207,
+  -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173,
+  -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385,
+  -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214,
+  28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786,
+  46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807,
+  110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782,
+  173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245,
+  197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484,
+  210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167,
+  189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755,
+  62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095,
+  -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295,
+  -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374,
+  -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395,
+  -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038,
+  36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376,
+  65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127,
+  -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059,
+  -127.19340520577649, -154.63036780787843, -180.74870760607394]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567,
+  205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663,
+  -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808,
+  -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975,
+  -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551,
+  -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826,
+  -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272,
+  104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167,
+  82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434,
+  -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583,
+  -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846,
+  -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196,
+  -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552,
+  -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175,
+  -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248,
+  -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993,
+  -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733,
+  -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155,
+  -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093,
+  -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656,
+  -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873,
+  25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458,
+  40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028,
+  -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413,
+  -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641,
+  -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581,
+  22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803,
+  15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514,
+  -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898,
+  -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569,
+  -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616,
+  -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532,
+  -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893,
+  18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601,
+  22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818,
+  32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779,
+  48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617,
+  143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107,
+  130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158,
+  44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568,
+  115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683,
+  149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516,
+  80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459,
+  99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014,
+  172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139,
+  113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729,
+  96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513,
+  62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735,
+  -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649,
+  21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656,
+  65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635,
+  70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916,
+  74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807,
+  136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928,
+  168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346,
+  121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441,
+  96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392,
+  104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364,
+  114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289,
+  98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998,
+  49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645,
+  16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556,
+  50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409,
+  60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868,
+  51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244,
+  53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015,
+  64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777,
+  38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126,
+  -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391,
+  -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894,
+  -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211,
+  -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903,
+  -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216,
+  -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931,
+  -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333,
+  21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149,
+  42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823,
+  16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427,
+  -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764,
+  -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054,
+  -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404,
+  -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496,
+  -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292,
+  -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934,
+  -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776,
+  -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126,
+  -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246,
+  -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633,
+  -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934,
+  -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297,
+  15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244,
+  26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861,
+  15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945,
+  36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882,
+  48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032,
+  34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864,
+  10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116,
+  -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808,
+  -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259,
+  -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883,
+  -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713,
+  6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992,
+  15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006,
+  6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738,
+  -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728,
+  -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381,
+  -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837,
+  -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521,
+  -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423,
+  -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537,
+  -111.02550604149512]
+pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703,
+  -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488,
+  -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888,
+  -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515,
+  -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485,
+  -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643,
+  -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232,
+  -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416,
+  -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985,
+  -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466,
+  -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374,
+  -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516,
+  -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498,
+  -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307,
+  -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517,
+  -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573,
+  -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114,
+  -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282,
+  -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618,
+  -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058,
+  -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321,
+  -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244,
+  -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404,
+  -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978,
+  -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411,
+  -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874,
+  -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595,
+  -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251,
+  -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766,
+  -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436,
+  20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966,
+  50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989,
+  53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028,
+  0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609,
+  -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265,
+  -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877]
+y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552,
+  -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052,
+  -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495,
+  -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031,
+  -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644,
+  -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909,
+  -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699,
+  -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666,
+  -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736,
+  -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901,
+  -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841,
+  -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048,
+  -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707,
+  -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386,
+  -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789,
+  -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165,
+  -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913,
+  -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266,
+  -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426,
+  -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594,
+  -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051,
+  -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277,
+  -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796,
+  -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399,
+  66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038,
+  133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598,
+  87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964,
+  -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265,
+  -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668,
+  -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168,
+  -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977,
+  2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377,
+  118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348,
+  85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387,
+  -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273,
+  -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885]
+yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878,
+  -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717,
+  -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224,
+  -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236,
+  -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699,
+  -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219,
+  -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572,
+  -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576,
+  -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458,
+  -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585,
+  3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895,
+  47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762,
+  98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556,
+  110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317,
+  108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497,
+  54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635,
+  83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588,
+  116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785,
+  88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314,
+  52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302,
+  43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712,
+  -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181,
+  -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577,
+  0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294,
+  -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832,
+  -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253,
+  -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826,
+  -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276,
+  10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813,
+  20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483,
+  -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721,
+  -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323,
+  -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184,
+  -29.84051753345265, -36.298155831550744, -43.32322171459229]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_obstacle_avoiding_weights.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_obstacle_avoiding_weights.yaml
new file mode 100644
index 000000000..4eee98089
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_obstacle_avoiding_weights.yaml
@@ -0,0 +1,828 @@
+x:
+- -69.87348796991277
+- -249.21900239547236
+- -67.84043243431186
+- -258.58285408554707
+- -188.7502609318067
+- -119.36924220761209
+- -352.5679785446982
+- -66.93165398538018
+- -296.56594649445543
+- -245.21143051620075
+- -294.16084343719865
+- -224.14057831802506
+- -306.41882670056583
+- -354.1215162988688
+- -170.12380217457115
+- -357.138104139715
+- -40.64275275372652
+- -135.26070385096273
+- -142.7924611877583
+- -87.09441603179418
+- -296.2516042512325
+- -147.38743022946406
+- -245.25060850179528
+- -180.4047071358973
+- -208.816557933262
+- -238.89472622454193
+- -199.49406057213355
+- -52.19028091210485
+- -217.4029465116399
+- -199.21075414745042
+- -139.48029922852695
+- -155.5043596080818
+- -1.64796773844034
+- -234.48096780722176
+- -61.800841737934654
+- -137.99987740648345
+- 9.105484328037676
+- -171.95791082549107
+- -147.90769802432126
+- -261.3365186441707
+- -169.58677350915372
+- -203.27571303170723
+- -266.5321973740621
+- -238.1210841285796
+- -198.1705210623072
+- -80.85663283027539
+- -145.06037800555129
+- -233.02117316469122
+- -87.88662963971761
+- -334.6241961665668
+- 60.038017618293054
+- -91.34095460068832
+- -12.31350804258898
+- -50.64383482500599
+- 46.383786048181165
+- -66.16880972125207
+- -231.62008971554272
+- -5.769763763458376
+- -121.24781553532844
+- -186.0356593018772
+- -37.266798862400535
+- -15.729361018214437
+- -279.5467593178302
+- -30.369467819357787
+- -42.47363800563517
+- -65.49726694396745
+- -164.29732909549625
+- -265.8982952407367
+- -258.84275991973743
+- -132.04859421462223
+- -169.55048658566966
+- -73.652309952296
+- -107.98574210892473
+- -50.81661354375743
+- -46.28377820392269
+- -137.71629008464157
+- -43.988156982955566
+- -70.86022094831522
+- -81.73747216446617
+- -8.416712107682471
+- 51.779128762006465
+- -13.704040655522906
+- 155.82627021956336
+- -15.919474106652785
+- 78.99129801613547
+- -29.05982749927712
+- -38.0220528334006
+- 17.972540991252107
+- 71.08654722463753
+- 34.843096595825045
+- 42.96854224219094
+- 122.80993017979677
+- 118.4521174237558
+- 87.29753460785082
+- 81.67924647948526
+- 18.07615092706082
+- 257.38318216628164
+- 52.773219127998935
+- 167.31265788815173
+- -61.540999301947764
+- -126.63667490618003
+- 137.33044266200358
+- 11.349077848092858
+- 92.81236152529415
+- -113.79865403819959
+- 208.58169552907614
+- 70.6757288207106
+- -107.35836079314619
+- 1.4643792086613052
+- 50.546401412163775
+- 146.94468094948863
+- 6.8546380396165585
+- -121.13740932529086
+- 148.1603235895151
+- -11.972502318856847
+- 32.19091475271171
+- 11.594083689006677
+- 7.8860925818078025
+- 109.95020670426437
+- 130.10738182081502
+- 184.74070418435576
+- 76.33449624543773
+- 144.2362755178293
+- 152.90365810900374
+- 107.06142151144562
+- 285.1507718028838
+- 67.86238131934115
+- 189.8565065793963
+- 346.3264761087587
+- 267.2945059252409
+- 231.17318025641273
+- -4.893330968496536
+- 140.00949141321567
+- 194.0667984026207
+- 238.51815440590195
+- 219.25318868147897
+- -48.93525386986038
+- 83.57849234051434
+- 140.16156625665158
+- 71.10806627454033
+- -96.83359161932513
+- 142.11449324291047
+- 20.266132421004347
+- 151.45329681011762
+- 30.037025872275514
+- 186.8725728018123
+- 30.615397902300938
+- 335.2746095520564
+- 67.71597837454001
+- -14.404270779646337
+y:
+- -69.87348796991277
+- -249.21900239547236
+- -67.84043243431186
+- -258.58285408554707
+- -188.7502609318067
+- -119.36924220761209
+- -352.5679785446982
+- -66.93165398538018
+- -296.56594649445543
+- -245.21143051620075
+- -294.16084343719865
+- -224.14057831802506
+- -306.41882670056583
+- -354.1215162988688
+- -170.12380217457115
+- -357.138104139715
+- -40.64275275372652
+- -135.26070385096273
+- -142.7924611877583
+- -87.09441603179418
+- -296.2516042512325
+- -147.38743022946406
+- -245.25060850179528
+- -180.4047071358973
+- -208.816557933262
+- -238.89472622454193
+- -199.49406057213355
+- -52.19028091210485
+- -217.4029465116399
+- -199.21075414745042
+- -139.48029922852695
+- -155.5043596080818
+- -1.64796773844034
+- -234.48096780722176
+- -61.800841737934654
+- -137.99987740648345
+- 9.105484328037676
+- -171.95791082549107
+- -147.90769802432126
+- -261.3365186441707
+- -169.58677350915372
+- -203.27571303170723
+- -266.5321973740621
+- -238.1210841285796
+- -198.1705210623072
+- -80.85663283027539
+- -145.06037800555129
+- -233.02117316469122
+- -87.88662963971761
+- -334.6241961665668
+- 60.038017618293054
+- -91.34095460068832
+- -12.31350804258898
+- -50.64383482500599
+- 46.383786048181165
+- -66.16880972125207
+- -231.62008971554272
+- -5.769763763458376
+- -121.24781553532844
+- -186.0356593018772
+- -37.266798862400535
+- -15.729361018214437
+- -279.5467593178302
+- -30.369467819357787
+- -42.47363800563517
+- -65.49726694396745
+- -164.29732909549625
+- -265.8982952407367
+- -258.84275991973743
+- -132.04859421462223
+- -169.55048658566966
+- -73.652309952296
+- -107.98574210892473
+- -50.81661354375743
+- -46.28377820392269
+- -137.71629008464157
+- -43.988156982955566
+- -70.86022094831522
+- -81.73747216446617
+- -8.416712107682471
+- 51.779128762006465
+- -13.704040655522906
+- 155.82627021956336
+- -15.919474106652785
+- 78.99129801613547
+- -29.05982749927712
+- -38.0220528334006
+- 17.972540991252107
+- 71.08654722463753
+- 34.843096595825045
+- 42.96854224219094
+- 122.80993017979677
+- 118.4521174237558
+- 87.29753460785082
+- 81.67924647948526
+- 18.07615092706082
+- 257.38318216628164
+- 52.773219127998935
+- 167.31265788815173
+- -61.540999301947764
+- -126.63667490618003
+- 137.33044266200358
+- 11.349077848092858
+- 92.81236152529415
+- -113.79865403819959
+- 208.58169552907614
+- 70.6757288207106
+- -107.35836079314619
+- 1.4643792086613052
+- 50.546401412163775
+- 146.94468094948863
+- 6.8546380396165585
+- -121.13740932529086
+- 148.1603235895151
+- -11.972502318856847
+- 32.19091475271171
+- 11.594083689006677
+- 7.8860925818078025
+- 109.95020670426437
+- 130.10738182081502
+- 184.74070418435576
+- 76.33449624543773
+- 144.2362755178293
+- 152.90365810900374
+- 107.06142151144562
+- 285.1507718028838
+- 67.86238131934115
+- 189.8565065793963
+- 346.3264761087587
+- 267.2945059252409
+- 231.17318025641273
+- -4.893330968496536
+- 140.00949141321567
+- 194.0667984026207
+- 238.51815440590195
+- 219.25318868147897
+- -48.93525386986038
+- 83.57849234051434
+- 140.16156625665158
+- 71.10806627454033
+- -96.83359161932513
+- 142.11449324291047
+- 20.266132421004347
+- 151.45329681011762
+- 30.037025872275514
+- 186.8725728018123
+- 30.615397902300938
+- 335.2746095520564
+- 67.71597837454001
+- -14.404270779646337
+z:
+- 130.76554053100793
+- 206.2662179940513
+- 189.28710370899157
+- 89.30496477388637
+- -24.46129711522901
+- 160.0646950098165
+- 51.78725667295301
+- -32.47828590726969
+- 312.26359628541155
+- 7.447125395819278
+- 48.73493042385533
+- 96.117587767791
+- 13.903095726216442
+- -30.39002789289115
+- 13.234035138042891
+- 44.16129752864455
+- 146.37705164248865
+- 273.2123327553373
+- 144.98079021618724
+- -4.417117422816288
+- -28.8668548615078
+- 139.28403795367868
+- -27.233283702152278
+- 7.305413222187726
+- 67.58589837884186
+- -76.26501051494712
+- 55.77016254678754
+- -44.64746619064322
+- 105.53261423441313
+- 4.817187791940008
+- 131.2623241238824
+- 208.85536859551934
+- 253.74407858074366
+- 83.90909989364087
+- 80.92692417316945
+- 194.67602042485152
+- 63.03833973913277
+- 152.33439587047596
+- -76.74735975798015
+- 240.08685547823546
+- 179.77342722543065
+- 131.78524056310084
+- -52.80891044526778
+- 162.65520770755816
+- 57.58419185088288
+- 142.27829787627454
+- 92.21318423380453
+- 84.74955511824889
+- 184.3220392862146
+- 394.2022248821202
+- 289.2529715561212
+- 330.4656288986008
+- 210.0210268365906
+- 468.81205357118586
+- 141.09691040806604
+- 331.1613365678323
+- 352.44693314342567
+- 247.42679910318498
+- 203.0930782296137
+- 214.42147286155867
+- 197.76076217589122
+- 249.16240564302552
+- 299.35494921345946
+- 361.76718190278035
+- 253.96545545591263
+- 180.13721378657286
+- 71.31933154999962
+- 223.72084988134068
+- 271.71442942934715
+- 308.82269796234385
+- 326.7380262879334
+- 310.3112256768682
+- 335.6051228694421
+- 328.5005403048852
+- 342.5682668099493
+- 366.6578605682522
+- 267.0774937688001
+- 157.7203985907876
+- 37.917009749897886
+- 382.7985547730168
+- 258.67384713827244
+- 418.23392809525217
+- 302.96018343174933
+- 38.22812829601864
+- 474.64862115056616
+- 184.08414533879056
+- 325.2570063837224
+- 211.9921455186777
+- 127.77180806842179
+- 125.09788032138539
+- 336.88930745728
+- 260.69397091445734
+- 196.76303434153536
+- 48.89139979109624
+- 20.9143995761367
+- 63.37523624233475
+- 19.266057618520215
+- 21.275073800442655
+- 67.18882915192955
+- 38.94661784528503
+- 178.31857938561723
+- 150.26482180396746
+- 27.060081864972375
+- 163.7015991594033
+- 58.39787054624568
+- 230.06563647929534
+- 81.98416322991498
+- 138.24995257726928
+- 69.35227706629378
+- 130.5953729083205
+- 230.75254743785743
+- 151.4962176360373
+- 130.43154588896357
+- 70.28697204298307
+- 240.89877257238317
+- 142.4236621999828
+- 166.7374295038086
+- 266.12750321364575
+- -17.267426570282705
+- 71.10109270203833
+- 121.35280302388519
+- -113.14605608500368
+- 104.03423032067323
+- 74.3512226446386
+- 85.20991337076298
+- 123.37233014177619
+- 145.91528915191563
+- 227.875743243453
+- 42.338971686171696
+- 241.7238402530031
+- 357.4328594021252
+- 44.50910544598379
+- 86.88686662653419
+- 143.97035907943243
+- 149.11686844861978
+- -2.290935929881382
+- 221.66399026184848
+- 107.05048111834631
+- 69.34480413579045
+- 36.90122680783556
+- 143.53785427402153
+- 74.5659335297027
+- 15.546729945368211
+- -18.8444798624607
+- -23.302032502050047
+- -49.364806317047105
+- -21.033749125494733
+- 102.21973357552142
+- -78.893199567806
+- 34.34192900894079
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
new file mode 100644
index 000000000..366175f9c
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -187.49195238797435
+- -169.52512966931013
+- -35.69274095939003
+- -110.38385915264884
+- -68.02909986220745
+- -128.4282807320246
+- -173.64174760465733
+- 26.417804453060466
+- -265.1511968093563
+- -208.2345262812316
+- -236.22305824029408
+- -292.1579456291514
+- -192.16922081868017
+- -73.72506100791689
+- -227.88416746725505
+- -245.88529994956338
+- -87.801284061494
+- -51.537954121202
+- -218.31943366179135
+- -19.00084265897026
+- -50.73202708213524
+- -156.08245180901318
+- -52.57445775081901
+- -342.5208997441454
+- -81.77810876739974
+- -174.1958381815731
+- -272.58419638629414
+- -22.427456591272964
+- -67.94830216667698
+- -131.55116573738906
+- -51.52359182361245
+- -288.7416926303152
+- -66.70671027140779
+- -342.4841150689501
+- -166.87744597718262
+- -135.67945475651916
+- -115.80670276581463
+- -392.53333883637134
+- -36.90191158618114
+- -173.80064748268455
+- -163.10641272099767
+- 7.960374041073862
+- -45.734150579455836
+- -190.36730444634858
+- -64.03257774809776
+- 27.78625095467416
+- -118.17315931763699
+- -57.888007878300165
+- -292.44891798678657
+- -85.33059310547326
+- -100.37854674098924
+- 1.9952291209454511
+- -167.6282204094909
+- -133.57127509235013
+- -225.0075163851039
+- -111.17327201802011
+- -242.4716831428446
+- -284.25269822833735
+- -251.59393903872822
+- -13.405909200151447
+- -267.09360040817
+- -193.2961594890842
+- -173.45487948432415
+- -86.87912728215895
+- 137.3803737089481
+- 14.729982103733787
+- -20.43551374181382
+- -183.23852169109657
+- 39.56949470282565
+- -11.746766870171504
+- -165.16494799780983
+- -17.72624341018731
+- 5.573884446233519
+- 119.93511795969849
+- 5.406619106751105
+- -28.33944076716923
+- -93.42445394305315
+- 31.358207958221207
+- -127.19272668631712
+- 47.61360021679253
+- 177.24358365165094
+- -33.28653474678124
+- 113.66223403198586
+- 23.55404380676361
+- 18.589236158322773
+- 290.5105976047401
+- -21.873343777140963
+- 34.340774653167635
+- 123.0549442804791
+- -50.73662541049178
+- -1.7314731873450575
+- 82.52433599394868
+- 47.321040690639705
+- 126.87991145000166
+- 268.17453639949167
+- 78.9263070743615
+- -46.985695686376765
+- -33.15861159657017
+- 121.44805915038098
+- -106.33355817627528
+- 33.220209348132734
+- 222.96938720583944
+- 47.52574406413015
+- 200.37330393698673
+- -37.25494538781959
+- 99.80531016763302
+- -89.93774989127296
+- 105.89444419617598
+- 56.03368438186566
+- 40.309626973350035
+- 270.8644142655386
+- 43.16449749064583
+- 55.65751358535125
+- -30.452922323427053
+- 107.68085844090612
+- 206.11784411834313
+- 185.86376583310394
+- 77.09904197384182
+- 113.38528890435701
+- 154.3326219589348
+- 53.403925267536046
+- 115.85374833333367
+- 153.4670922296937
+- 291.63757494319856
+- 203.61974431849197
+- 213.60396469189791
+- 170.24873764605687
+- 136.85461496589278
+- 25.058861185881806
+- 145.00467183992313
+- 60.785220824967766
+- 236.67380042158126
+- 386.4651972576292
+- 149.45913716260057
+- 265.2513397719353
+- 264.91108470657554
+- 35.60643577997812
+- 53.2349506925678
+- 67.29109718480608
+- 177.09866691473454
+- 296.3408306128541
+- 165.8042545452507
+- 22.43858524866842
+- 107.52321752590935
+- 123.90852051072142
+- -40.14079797607926
+- 161.7140116579578
+- 284.8878210665913
+- 7.717525716715916
+- 125.9785847752208
+y:
+- -187.49195238797435
+- -169.52512966931013
+- -35.69274095939003
+- -110.38385915264884
+- -68.02909986220745
+- -128.4282807320246
+- -173.64174760465733
+- 26.417804453060466
+- -265.1511968093563
+- -208.2345262812316
+- -236.22305824029408
+- -292.1579456291514
+- -192.16922081868017
+- -73.72506100791689
+- -227.88416746725505
+- -245.88529994956338
+- -87.801284061494
+- -51.537954121202
+- -218.31943366179135
+- -19.00084265897026
+- -50.73202708213524
+- -156.08245180901318
+- -52.57445775081901
+- -342.5208997441454
+- -81.77810876739974
+- -174.1958381815731
+- -272.58419638629414
+- -22.427456591272964
+- -67.94830216667698
+- -131.55116573738906
+- -51.52359182361245
+- -288.7416926303152
+- -66.70671027140779
+- -342.4841150689501
+- -166.87744597718262
+- -135.67945475651916
+- -115.80670276581463
+- -392.53333883637134
+- -36.90191158618114
+- -173.80064748268455
+- -163.10641272099767
+- 7.960374041073862
+- -45.734150579455836
+- -190.36730444634858
+- -64.03257774809776
+- 27.78625095467416
+- -118.17315931763699
+- -57.888007878300165
+- -292.44891798678657
+- -85.33059310547326
+- -100.37854674098924
+- 1.9952291209454511
+- -167.6282204094909
+- -133.57127509235013
+- -225.0075163851039
+- -111.17327201802011
+- -242.4716831428446
+- -284.25269822833735
+- -251.59393903872822
+- -13.405909200151447
+- -267.09360040817
+- -193.2961594890842
+- -173.45487948432415
+- -86.87912728215895
+- 137.3803737089481
+- 14.729982103733787
+- -20.43551374181382
+- -183.23852169109657
+- 39.56949470282565
+- -11.746766870171504
+- -165.16494799780983
+- -17.72624341018731
+- 5.573884446233519
+- 119.93511795969849
+- 5.406619106751105
+- -28.33944076716923
+- -93.42445394305315
+- 31.358207958221207
+- -127.19272668631712
+- 47.61360021679253
+- 177.24358365165094
+- -33.28653474678124
+- 113.66223403198586
+- 23.55404380676361
+- 18.589236158322773
+- 290.5105976047401
+- -21.873343777140963
+- 34.340774653167635
+- 123.0549442804791
+- -50.73662541049178
+- -1.7314731873450575
+- 82.52433599394868
+- 47.321040690639705
+- 126.87991145000166
+- 268.17453639949167
+- 78.9263070743615
+- -46.985695686376765
+- -33.15861159657017
+- 121.44805915038098
+- -106.33355817627528
+- 33.220209348132734
+- 222.96938720583944
+- 47.52574406413015
+- 200.37330393698673
+- -37.25494538781959
+- 99.80531016763302
+- -89.93774989127296
+- 105.89444419617598
+- 56.03368438186566
+- 40.309626973350035
+- 270.8644142655386
+- 43.16449749064583
+- 55.65751358535125
+- -30.452922323427053
+- 107.68085844090612
+- 206.11784411834313
+- 185.86376583310394
+- 77.09904197384182
+- 113.38528890435701
+- 154.3326219589348
+- 53.403925267536046
+- 115.85374833333367
+- 153.4670922296937
+- 291.63757494319856
+- 203.61974431849197
+- 213.60396469189791
+- 170.24873764605687
+- 136.85461496589278
+- 25.058861185881806
+- 145.00467183992313
+- 60.785220824967766
+- 236.67380042158126
+- 386.4651972576292
+- 149.45913716260057
+- 265.2513397719353
+- 264.91108470657554
+- 35.60643577997812
+- 53.2349506925678
+- 67.29109718480608
+- 177.09866691473454
+- 296.3408306128541
+- 165.8042545452507
+- 22.43858524866842
+- 107.52321752590935
+- 123.90852051072142
+- -40.14079797607926
+- 161.7140116579578
+- 284.8878210665913
+- 7.717525716715916
+- 125.9785847752208
+z:
+- 147.0697286588254
+- 147.74588784221456
+- 112.65566266136821
+- 18.359232177402554
+- -111.9742511820463
+- 15.31172882006791
+- 6.609434781575087
+- 135.57361669924134
+- 89.8413925818162
+- 191.02570989931755
+- 121.68098347990863
+- 310.18469024245763
+- -107.20626443396715
+- 14.210755115910601
+- 68.47681492369398
+- -53.39821489494769
+- -160.0670830406598
+- 57.91893447524476
+- 143.91248619820385
+- 85.5563450002862
+- -17.376218538929496
+- 109.06118809206095
+- 0.12632833243893538
+- -71.9225993333965
+- 7.2990835702683645
+- 192.10773090491654
+- 121.89379742888093
+- 179.66036483456833
+- 127.17688444271954
+- 241.32545602197393
+- 104.89248092035686
+- 16.381743635433864
+- 112.18795371996411
+- 119.33459557930752
+- 242.74757392701636
+- -117.28935615734264
+- 117.16627871389215
+- 13.469368866837582
+- -4.529395518536364
+- 245.09004846012132
+- 142.67893335445106
+- 113.42158548718666
+- 171.89491633957874
+- 37.58001564045737
+- 297.6674674270416
+- -38.36747257612112
+- 322.9240932227268
+- 334.65733991230314
+- 335.0318034746627
+- 450.14589353644396
+- 355.3340767107325
+- 436.97904739499404
+- 99.95734081688825
+- 291.63120762804675
+- 341.0811466216531
+- 470.60709366837676
+- 271.24215997323773
+- 332.59719180039184
+- 363.15201384486556
+- 370.54329377801184
+- 352.4324627185788
+- 272.1076188601076
+- 193.5154978871954
+- 285.19615003876777
+- 485.2043077127898
+- 199.85713032051675
+- 232.79115369372064
+- 368.7763609306992
+- 258.2277315298386
+- 431.8340999377656
+- 281.05661759873607
+- 303.9240761547915
+- 439.89157193620275
+- 399.2473016024376
+- 352.686354922071
+- 271.8967458082695
+- 342.3068343729032
+- 363.9795427725253
+- 522.1903026124583
+- 468.83337875499006
+- 391.5060739650679
+- 381.5820600899362
+- 151.22523327357072
+- 439.29634480040147
+- 189.10718421103832
+- 407.65805256539613
+- 251.12864627901752
+- 173.68656601733437
+- 64.4314952848051
+- 231.5543646649201
+- 115.04391184200999
+- 419.5292522009196
+- 63.02135408071294
+- 335.2168582715738
+- 190.13034431554857
+- 10.247919743513544
+- 173.420969982708
+- 234.86765440011254
+- 124.04387428861862
+- 212.67267316149872
+- 222.2869721518006
+- 51.072435123440755
+- 119.96146399624313
+- 235.15905582840983
+- -6.449655145401507
+- 153.40182343075242
+- 290.30123878513825
+- 100.81602872463795
+- 55.66033162087278
+- 123.46886876126574
+- 51.24113167810543
+- -31.79771727056099
+- 188.4114987950756
+- 231.83198061552278
+- 50.83621979811791
+- 226.83855617509082
+- -71.98439831506886
+- 235.6002360525736
+- -1.4830821361137023
+- 71.25253644792186
+- 12.868868326837722
+- 95.86686059120905
+- 141.00922400017984
+- 248.8011363353016
+- 34.531073331684986
+- 254.651501486854
+- 158.52662670520084
+- 211.19999911135668
+- 203.44046518137026
+- 147.85661102132698
+- 140.80016347894465
+- 222.31673242383246
+- 282.0261807013606
+- 22.487483903790896
+- 77.77002397719545
+- 65.01869511417857
+- 131.4165488618371
+- 117.95918980344477
+- 175.77414442932096
+- 125.11179322132699
+- 172.4173230733195
+- 25.243442056303145
+- 36.26024110752745
+- 316.36891113395444
+- 243.97128445773103
+- 130.61931796016933
+- 92.40271625000324
+- -149.04938390927575
+- 84.38242545119284
+- 80.72949701618471
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
new file mode 100644
index 000000000..4ebb766b6
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -201.04147127024706
+- -167.2523471707599
+- -23.50135210896892
+- -153.13294915070543
+- -30.619781729849734
+- -125.57368293643788
+- -178.08439653188327
+- 62.83275160635712
+- -239.15284128925578
+- -228.03087956235342
+- -344.8906958677936
+- -275.33749943999624
+- -255.7609565519209
+- 2.279808122905889
+- -214.27116514894607
+- -371.1288460507514
+- -39.959069906873296
+- 25.69977166716337
+- -246.29003388552277
+- -66.56237063945625
+- -85.47673594534648
+- -216.557964259047
+- -44.256620618839115
+- -308.41706660891793
+- -141.83656062154273
+- -186.5737208290361
+- -264.8159580639565
+- 21.09428847793057
+- -99.02263135039374
+- -241.45140915041816
+- -60.0153002212281
+- -281.4226021933602
+- -139.67077463266855
+- -398.3843932248262
+- -191.4638108446459
+- -145.68718027802007
+- -105.96600739077311
+- -434.71412153803016
+- -65.94347855017091
+- -143.58957932066278
+- -220.24193966712153
+- 42.23677704057419
+- -26.489960612303424
+- -208.84164265949022
+- 2.0147261992437735
+- 42.95136987656696
+- -34.66600593993395
+- -137.23795534595922
+- -330.6151271417497
+- -142.72606625502956
+- -96.49725745562209
+- 52.24933122390811
+- -172.44942696005236
+- -197.14756176873803
+- -239.51094761047256
+- -35.42406003348334
+- -189.90600347995877
+- -263.0995480025847
+- -261.38202538433524
+- 19.031078473612904
+- -340.8858161607905
+- -174.35873238049166
+- -205.86303683202112
+- -72.65734662654334
+- 279.4061625440003
+- 64.68537566450004
+- -13.186075154329556
+- -112.23346234543293
+- 134.3271343911593
+- 19.329528222566616
+- -43.112315515462235
+- 59.18580744908775
+- -7.367179176849072
+- 156.46592900612185
+- -21.0628015724817
+- 40.062659109965736
+- -121.09100219333905
+- 55.58073784666084
+- -98.7634737409404
+- -44.222306993784265
+- 260.6374894645528
+- -74.24302907024753
+- 139.06408596403213
+- 74.5132284592363
+- 37.782902039733045
+- 308.6128578878652
+- 56.210995895351495
+- 44.096706376260315
+- 245.95854032779206
+- -71.04479576485645
+- 67.66139996703141
+- 73.74526490662923
+- 106.90223500221825
+- 166.94351470134933
+- 285.5251346087024
+- -5.567322951604867
+- -72.68258307194573
+- -45.151988554119214
+- 67.80374005810233
+- -58.072895339898224
+- 90.24905134888387
+- 254.37723583013167
+- 72.29549320333595
+- 260.34176464676153
+- -62.531555267807704
+- 141.35374244849817
+- -62.18005929453309
+- 153.92359000530257
+- 105.23951868310732
+- 47.27948737622441
+- 253.80568421218246
+- 44.67792618406605
+- -4.102749792143297
+- 27.411615717986262
+- 91.51731269469626
+- 258.9568884245361
+- 302.1519261293808
+- 171.60350739134424
+- 161.66629226379598
+- 173.82461332097418
+- 0.006196471457665639
+- 133.11550045790452
+- 130.85157497111507
+- 270.6715274563171
+- 242.52754536508093
+- 68.74489243472402
+- 171.8568371983085
+- 90.14523822958101
+- 22.18229039361379
+- 183.18193882456524
+- 91.78535767377343
+- 277.4074125496988
+- 471.0134425098973
+- 181.0957245116598
+- 191.63677933453334
+- 306.46640679256234
+- 114.76828630347208
+- 20.965552811472538
+- 20.716058857589132
+- 193.08059043698137
+- 390.55719089887765
+- 210.57986191532072
+- 52.15275437102519
+- 199.58823721871573
+- 65.72488807252493
+- -123.34417482594056
+- 232.20209175345101
+- 272.29857135174734
+- -106.0729419699781
+- 234.37020913355548
+y:
+- -201.04147127024706
+- -167.2523471707599
+- -23.50135210896892
+- -153.13294915070543
+- -30.619781729849734
+- -125.57368293643788
+- -178.08439653188327
+- 62.83275160635712
+- -239.15284128925578
+- -228.03087956235342
+- -344.8906958677936
+- -275.33749943999624
+- -255.7609565519209
+- 2.279808122905889
+- -214.27116514894607
+- -371.1288460507514
+- -39.959069906873296
+- 25.69977166716337
+- -246.29003388552277
+- -66.56237063945625
+- -85.47673594534648
+- -216.557964259047
+- -44.256620618839115
+- -308.41706660891793
+- -141.83656062154273
+- -186.5737208290361
+- -264.8159580639565
+- 21.09428847793057
+- -99.02263135039374
+- -241.45140915041816
+- -60.0153002212281
+- -281.4226021933602
+- -139.67077463266855
+- -398.3843932248262
+- -191.4638108446459
+- -145.68718027802007
+- -105.96600739077311
+- -434.71412153803016
+- -65.94347855017091
+- -143.58957932066278
+- -220.24193966712153
+- 42.23677704057419
+- -26.489960612303424
+- -208.84164265949022
+- 2.0147261992437735
+- 42.95136987656696
+- -34.66600593993395
+- -137.23795534595922
+- -330.6151271417497
+- -142.72606625502956
+- -96.49725745562209
+- 52.24933122390811
+- -172.44942696005236
+- -197.14756176873803
+- -239.51094761047256
+- -35.42406003348334
+- -189.90600347995877
+- -263.0995480025847
+- -261.38202538433524
+- 19.031078473612904
+- -340.8858161607905
+- -174.35873238049166
+- -205.86303683202112
+- -72.65734662654334
+- 279.4061625440003
+- 64.68537566450004
+- -13.186075154329556
+- -112.23346234543293
+- 134.3271343911593
+- 19.329528222566616
+- -43.112315515462235
+- 59.18580744908775
+- -7.367179176849072
+- 156.46592900612185
+- -21.0628015724817
+- 40.062659109965736
+- -121.09100219333905
+- 55.58073784666084
+- -98.7634737409404
+- -44.222306993784265
+- 260.6374894645528
+- -74.24302907024753
+- 139.06408596403213
+- 74.5132284592363
+- 37.782902039733045
+- 308.6128578878652
+- 56.210995895351495
+- 44.096706376260315
+- 245.95854032779206
+- -71.04479576485645
+- 67.66139996703141
+- 73.74526490662923
+- 106.90223500221825
+- 166.94351470134933
+- 285.5251346087024
+- -5.567322951604867
+- -72.68258307194573
+- -45.151988554119214
+- 67.80374005810233
+- -58.072895339898224
+- 90.24905134888387
+- 254.37723583013167
+- 72.29549320333595
+- 260.34176464676153
+- -62.531555267807704
+- 141.35374244849817
+- -62.18005929453309
+- 153.92359000530257
+- 105.23951868310732
+- 47.27948737622441
+- 253.80568421218246
+- 44.67792618406605
+- -4.102749792143297
+- 27.411615717986262
+- 91.51731269469626
+- 258.9568884245361
+- 302.1519261293808
+- 171.60350739134424
+- 161.66629226379598
+- 173.82461332097418
+- 0.006196471457665639
+- 133.11550045790452
+- 130.85157497111507
+- 270.6715274563171
+- 242.52754536508093
+- 68.74489243472402
+- 171.8568371983085
+- 90.14523822958101
+- 22.18229039361379
+- 183.18193882456524
+- 91.78535767377343
+- 277.4074125496988
+- 471.0134425098973
+- 181.0957245116598
+- 191.63677933453334
+- 306.46640679256234
+- 114.76828630347208
+- 20.965552811472538
+- 20.716058857589132
+- 193.08059043698137
+- 390.55719089887765
+- 210.57986191532072
+- 52.15275437102519
+- 199.58823721871573
+- 65.72488807252493
+- -123.34417482594056
+- 232.20209175345101
+- 272.29857135174734
+- -106.0729419699781
+- 234.37020913355548
+z:
+- 249.30936970701725
+- 120.61855374577983
+- 88.51943638748523
+- -37.27638287099523
+- -249.83266708212986
+- -18.177204468805662
+- -98.74885491605013
+- 176.97962575030346
+- 149.45102926216487
+- 222.50692355023767
+- 96.84267677464365
+- 237.66034703685702
+- -126.70915445427974
+- -44.65420067168505
+- 38.04639606786077
+- -196.01010647253312
+- -200.78799593386236
+- 15.036720386788945
+- 52.54811037578521
+- 18.07877118426772
+- -57.4093398932157
+- 3.6025400862264245
+- -53.156746387694376
+- -221.40087385758676
+- -31.326460682719077
+- 153.89632299904753
+- 98.72067944380399
+- 170.5520255489363
+- 51.58451076299889
+- 231.261612637923
+- -3.026138910427871
+- 108.99064480497259
+- 33.0448811253988
+- 70.16689432543035
+- 230.10084011498412
+- -188.66331130041365
+- 59.77004478430251
+- 55.247626585229504
+- -15.111463163670479
+- 301.9343390150364
+- 53.12875931817165
+- 86.47901096035412
+- 126.9803736646344
+- 6.657043193290982
+- 338.2610723382575
+- -35.089663976047895
+- 362.7873744869064
+- 408.6517567046632
+- 313.1947391612431
+- 462.4823592374066
+- 384.1008702702842
+- 488.94229060927137
+- 164.79345374558173
+- 419.4527587833925
+- 346.3038587428371
+- 530.5884101358595
+- 360.79908258410836
+- 389.01921341251165
+- 415.67617029093947
+- 308.08129249135584
+- 403.01728601048393
+- 311.8332733496349
+- 267.61198616890204
+- 256.1061830113214
+- 621.1958712803905
+- 161.83008760845297
+- 257.4914767960563
+- 437.9875316315236
+- 250.86316212986466
+- 412.0685566775606
+- 256.37068422284017
+- 319.58769118215133
+- 509.44667572187643
+- 425.7620586153827
+- 340.25901185268805
+- 299.69580910803
+- 387.0631841218825
+- 430.35087816709466
+- 572.347267701468
+- 519.4380676617408
+- 448.3695343523369
+- 310.71202956241456
+- 203.55516496425884
+- 357.1802053274863
+- 132.37004571025543
+- 328.9035863936466
+- 235.01277959089552
+- 280.5841730327106
+- 118.81667680234366
+- 188.02938084047207
+- 161.64114366187053
+- 326.2691306784173
+- 112.06424178318245
+- 469.6477058640201
+- 285.5232385757413
+- -29.545754882842296
+- 307.49874707157056
+- 290.13524210030073
+- 212.48909881041584
+- 264.18144289665895
+- 238.84314137575788
+- 19.26112277757816
+- 164.7374235094159
+- 248.53827390660908
+- 48.11169185251769
+- 221.3731851843131
+- 349.9103175456074
+- 139.79215196283744
+- -7.323857813999613
+- 122.06816138591932
+- 70.63873784282016
+- 23.763489310211057
+- 129.79665714678893
+- 280.0777290498324
+- 28.619509788763942
+- 284.9047996431252
+- -121.14505589764663
+- 257.8155542450373
+- -136.80047380386117
+- 20.54770093401279
+- 25.641797135780646
+- 27.022963128249547
+- 85.60048054120156
+- 187.47694604471644
+- 56.51758064821176
+- 235.83209314242077
+- 134.74271756936014
+- 271.3715291700889
+- 303.3303493797271
+- 63.678494901385974
+- 172.13689452739587
+- 173.90286722217647
+- 255.38444320773692
+- 8.809125152185375
+- 59.733563534633056
+- 86.13954853681179
+- 57.258267215494065
+- 198.46902308963632
+- 111.29466235293138
+- 151.92572265717888
+- 148.99031509355103
+- -30.005864551173687
+- 7.613704816481292
+- 375.0646843902285
+- 277.2043147617383
+- 195.06683472314032
+- 96.87061248081244
+- -105.89449755489385
+- 33.73473466239024
+- 136.2264101780908
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
new file mode 100644
index 000000000..ab15b0753
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -179.2934822686993
+- -135.76649637987583
+- -90.40305102872078
+- -270.530960411821
+- -63.51863071515627
+- -215.09297800270357
+- -257.82008347952575
+- 25.781114159768027
+- -179.77654904198235
+- -307.4174057380679
+- -363.47071352279596
+- -274.1569617328421
+- -275.18497907347097
+- -41.06302412369343
+- -295.198199954953
+- -415.4146695794232
+- 34.72872514630607
+- 93.76413819213398
+- -230.69777481668504
+- -49.89855687551775
+- -117.63588749771333
+- -247.42941395654694
+- -123.91496570278437
+- -285.37687764188803
+- -155.64459559052918
+- -205.43275126262583
+- -296.69059052197656
+- 12.303764101659395
+- -122.63690771806206
+- -211.9532405959818
+- 76.808403691593
+- -308.2116756258198
+- -184.30909064181478
+- -437.0414947548512
+- -282.50405312407815
+- -171.74753786667986
+- -116.91424992621994
+- -487.04009193295417
+- -72.92738790974472
+- -214.20563685144288
+- -259.7419978695749
+- 85.20470083711679
+- -57.9436873065203
+- -142.86539215814176
+- 120.09958517741079
+- 72.13501240945514
+- 25.440459624469593
+- -177.24493599973866
+- -258.00927429019686
+- -68.87191144413146
+- -27.62387700319308
+- 89.77917719918135
+- -83.41635101771399
+- -143.65479560804735
+- -255.30009372016136
+- 12.46632642860062
+- -260.19481478829834
+- -306.60554944032737
+- -298.14374479342405
+- 61.29970452741079
+- -283.34940237960154
+- -126.43540560707793
+- -176.35810727347345
+- 4.195777413120059
+- 310.640764418201
+- 143.6171510940722
+- 17.7678913637698
+- -78.30081165245478
+- 122.89686088856361
+- -41.5154750860532
+- -1.7130474236701687
+- 90.372127392264
+- -51.01928374845602
+- 60.325131134970505
+- 132.21273921271037
+- 27.233578839260126
+- -123.28053266552254
+- 58.739968982248655
+- -95.82344658822986
+- -90.68142745664106
+- 203.0313413174497
+- -57.285714919736755
+- 207.62021798231837
+- 181.84307164026205
+- 22.689238060935143
+- 256.37726901081186
+- 152.7240190731704
+- 39.17676858574742
+- 299.81469264572013
+- -5.332119060962668
+- 22.341969591749287
+- 64.99231929268271
+- 104.24125245246285
+- 205.85187592171644
+- 323.0282116994283
+- 68.69983321981688
+- -38.206184662002784
+- -51.401884319924584
+- 111.16945231469653
+- -26.588704768394674
+- 122.34082733072867
+- 313.4665931576804
+- -33.229595756114506
+- 221.81180370569442
+- 13.920085609845604
+- 244.8286645436503
+- -21.974914472196495
+- 153.94162729369413
+- 104.11602704693118
+- -10.838982286591328
+- 252.40474796440583
+- 86.4035070532815
+- -71.78057541126167
+- 51.00359197098217
+- 129.7347904056021
+- 219.23322539116725
+- 272.5591470984837
+- 192.93730373543843
+- 177.72474708610804
+- 224.8126820443087
+- -51.47603514260063
+- 210.53572590831124
+- 124.02113731156228
+- 369.66157496639664
+- 286.3003304911244
+- 93.17540782263704
+- 183.08093857503735
+- 55.844862383135414
+- 18.38697302258902
+- 162.283377821975
+- 100.85430304069654
+- 314.8591799824604
+- 459.4182294727302
+- 267.6308256577334
+- 170.43025106511766
+- 359.2722274353442
+- 164.41835323012955
+- 170.0367539921112
+- -13.839365738552324
+- 178.1125356375244
+- 370.9848064377404
+- 306.53040308294425
+- -42.587553545316844
+- 103.03713001045284
+- 70.39061314742493
+- -57.74941512966541
+- 302.22377456453205
+- 215.6049398688888
+- -84.82678229107678
+- 151.992534614046
+y:
+- -179.2934822686993
+- -135.76649637987583
+- -90.40305102872078
+- -270.530960411821
+- -63.51863071515627
+- -215.09297800270357
+- -257.82008347952575
+- 25.781114159768027
+- -179.77654904198235
+- -307.4174057380679
+- -363.47071352279596
+- -274.1569617328421
+- -275.18497907347097
+- -41.06302412369343
+- -295.198199954953
+- -415.4146695794232
+- 34.72872514630607
+- 93.76413819213398
+- -230.69777481668504
+- -49.89855687551775
+- -117.63588749771333
+- -247.42941395654694
+- -123.91496570278437
+- -285.37687764188803
+- -155.64459559052918
+- -205.43275126262583
+- -296.69059052197656
+- 12.303764101659395
+- -122.63690771806206
+- -211.9532405959818
+- 76.808403691593
+- -308.2116756258198
+- -184.30909064181478
+- -437.0414947548512
+- -282.50405312407815
+- -171.74753786667986
+- -116.91424992621994
+- -487.04009193295417
+- -72.92738790974472
+- -214.20563685144288
+- -259.7419978695749
+- 85.20470083711679
+- -57.9436873065203
+- -142.86539215814176
+- 120.09958517741079
+- 72.13501240945514
+- 25.440459624469593
+- -177.24493599973866
+- -258.00927429019686
+- -68.87191144413146
+- -27.62387700319308
+- 89.77917719918135
+- -83.41635101771399
+- -143.65479560804735
+- -255.30009372016136
+- 12.46632642860062
+- -260.19481478829834
+- -306.60554944032737
+- -298.14374479342405
+- 61.29970452741079
+- -283.34940237960154
+- -126.43540560707793
+- -176.35810727347345
+- 4.195777413120059
+- 310.640764418201
+- 143.6171510940722
+- 17.7678913637698
+- -78.30081165245478
+- 122.89686088856361
+- -41.5154750860532
+- -1.7130474236701687
+- 90.372127392264
+- -51.01928374845602
+- 60.325131134970505
+- 132.21273921271037
+- 27.233578839260126
+- -123.28053266552254
+- 58.739968982248655
+- -95.82344658822986
+- -90.68142745664106
+- 203.0313413174497
+- -57.285714919736755
+- 207.62021798231837
+- 181.84307164026205
+- 22.689238060935143
+- 256.37726901081186
+- 152.7240190731704
+- 39.17676858574742
+- 299.81469264572013
+- -5.332119060962668
+- 22.341969591749287
+- 64.99231929268271
+- 104.24125245246285
+- 205.85187592171644
+- 323.0282116994283
+- 68.69983321981688
+- -38.206184662002784
+- -51.401884319924584
+- 111.16945231469653
+- -26.588704768394674
+- 122.34082733072867
+- 313.4665931576804
+- -33.229595756114506
+- 221.81180370569442
+- 13.920085609845604
+- 244.8286645436503
+- -21.974914472196495
+- 153.94162729369413
+- 104.11602704693118
+- -10.838982286591328
+- 252.40474796440583
+- 86.4035070532815
+- -71.78057541126167
+- 51.00359197098217
+- 129.7347904056021
+- 219.23322539116725
+- 272.5591470984837
+- 192.93730373543843
+- 177.72474708610804
+- 224.8126820443087
+- -51.47603514260063
+- 210.53572590831124
+- 124.02113731156228
+- 369.66157496639664
+- 286.3003304911244
+- 93.17540782263704
+- 183.08093857503735
+- 55.844862383135414
+- 18.38697302258902
+- 162.283377821975
+- 100.85430304069654
+- 314.8591799824604
+- 459.4182294727302
+- 267.6308256577334
+- 170.43025106511766
+- 359.2722274353442
+- 164.41835323012955
+- 170.0367539921112
+- -13.839365738552324
+- 178.1125356375244
+- 370.9848064377404
+- 306.53040308294425
+- -42.587553545316844
+- 103.03713001045284
+- 70.39061314742493
+- -57.74941512966541
+- 302.22377456453205
+- 215.6049398688888
+- -84.82678229107678
+- 151.992534614046
+z:
+- 214.37440673711168
+- 44.00672407976349
+- 115.31195321611116
+- -20.29545126938044
+- -415.55284697963526
+- 13.851309643161283
+- -110.49518583396743
+- 210.39020040967443
+- 76.73384447900135
+- 216.99877646760936
+- 20.075767607107007
+- 284.08527699093594
+- -280.40862472849636
+- -71.6349308104166
+- 37.52312815893663
+- -223.74281311601015
+- -213.6882424238962
+- -22.742280362928664
+- -8.27980004507392
+- -52.865816576583356
+- -32.533513252392765
+- 106.96122730557236
+- -135.1451882548292
+- -314.1886237045455
+- -4.364592631038872
+- 45.800323950588684
+- 58.45533791309369
+- 109.5028661828198
+- -113.56861793744787
+- 166.7536555302581
+- -46.32814486706701
+- 36.25649755245916
+- -58.03189778020469
+- -10.3482729954698
+- 188.6366435164062
+- -327.73528742496393
+- 60.83239316329532
+- -13.180843072021858
+- -44.87381680119968
+- 261.3595776975404
+- 25.89814905139929
+- 139.40808646745523
+- 144.54605252965092
+- 48.73036114858244
+- 306.7210428543346
+- -110.37467388924706
+- 415.75479338868314
+- 414.17299368977666
+- 332.8407998038196
+- 476.19351226821004
+- 339.75966682022835
+- 487.047383224643
+- 62.78942448070585
+- 502.236808521147
+- 317.4037290525457
+- 538.9869602617855
+- 401.0616049213975
+- 345.5746269145603
+- 522.7990566646884
+- 279.5031435459519
+- 437.0968186961899
+- 290.54009063679956
+- 300.2782288439606
+- 291.9289517424361
+- 656.6650733867958
+- 205.9684735278789
+- 307.78156999820226
+- 435.5527058410805
+- 277.2101104515263
+- 444.8216395354565
+- 222.4745208028537
+- 277.0731850758149
+- 461.3158645032461
+- 428.5650157763214
+- 427.114185853697
+- 314.19524862805565
+- 447.0140830839152
+- 435.1456037599901
+- 632.1589941524588
+- 501.54284738620464
+- 589.9611015767636
+- 277.42177329876245
+- 281.14469077180456
+- 441.4113750539261
+- 173.98458091362917
+- 262.72403190697736
+- 333.8228656552859
+- 273.5133844805392
+- 111.35254213573505
+- 259.8896525621784
+- 217.98528295483166
+- 310.06800926769256
+- 117.11584864710484
+- 471.4522743647168
+- 384.3303077899318
+- -10.60091662447261
+- 331.08304250940273
+- 221.58920209215313
+- 224.79429552865372
+- 338.9593888386391
+- 250.4853549019945
+- -65.88006429110675
+- 178.2163790426598
+- 226.38068796681418
+- 34.01725375682388
+- 174.35359086419734
+- 396.4018146155407
+- 107.48545280640383
+- 3.377900407383113
+- 131.97849361920083
+- 142.36308581067266
+- -20.080375234085047
+- 60.22121340943852
+- 374.31666332717725
+- -72.74869690629166
+- 288.356896772892
+- -155.59586645860654
+- 183.77281679571325
+- -240.7204544113106
+- 3.060224520174071
+- 35.864627690616736
+- 102.60376314668531
+- 152.60492934576612
+- 102.74316424193954
+- 56.56468544921291
+- 187.61548789400493
+- 52.2724681556218
+- 360.13487768675805
+- 251.39159102736744
+- 67.85547148748974
+- 206.1368531327159
+- 264.01861873881614
+- 275.24635219707744
+- 82.18173114510144
+- -23.3079664181338
+- 150.61999948136992
+- 151.60669651443555
+- 266.76286407111866
+- 19.830200078894972
+- 88.4364362745114
+- 176.02013116459312
+- -44.175868736612514
+- -34.72167933433163
+- 425.51862448659296
+- 297.25625157092946
+- 181.1848791308081
+- 153.17020291593585
+- -63.11945703033657
+- 48.081395646440086
+- 157.13565809180034
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
new file mode 100644
index 000000000..6343958e6
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -137.45017437663398
+- -168.4801777684752
+- -84.03258737390696
+- -138.8989087267409
+- -2.082247962073727
+- -179.31723443328136
+- -88.42880815947395
+- -4.445333881119488
+- -242.64902467142977
+- -154.9719472652605
+- -242.0149929666361
+- -249.6424766495926
+- -187.5763161104164
+- -103.11645543685971
+- -156.91824303049796
+- -194.47157838400528
+- -83.05585362938487
+- -80.71853643395194
+- -258.3820101897301
+- -43.4329355576356
+- -82.8625965667567
+- -110.65002805845242
+- -84.10590340102833
+- -244.3473862277645
+- -87.37067637111183
+- -209.3757736776577
+- -194.4547880511376
+- -54.453791166857265
+- -47.85715533265113
+- -140.85880857135035
+- -82.301704209561
+- -305.036589417813
+- -88.51830760605337
+- -315.93045937552273
+- -198.54928242188186
+- -51.39778560761003
+- -117.76813593842118
+- -304.60018141822195
+- -154.20041553041935
+- -114.05395035507439
+- -144.7469051481917
+- -17.032567351447295
+- -126.08846452513698
+- -108.60229805313843
+- -61.70125602392602
+- -28.798590721569045
+- -105.1350744180009
+- -88.33021111454664
+- -227.77270995236424
+- -134.5288083889458
+- -108.11291016528452
+- -82.56938099982168
+- -217.23078139291084
+- -129.31246309353085
+- -265.2911672445468
+- -165.85228547123336
+- -289.52740630277026
+- -227.80079397765286
+- -226.78578207175408
+- 2.9445483397250083
+- -264.27926385368545
+- -161.63836424107424
+- -175.12507393760865
+- -61.83230511477935
+- 24.481477359040678
+- -111.267164036688
+- -62.89273463909397
+- -193.98677985400676
+- 75.07146587979011
+- 2.674657153233626
+- -89.63278756755514
+- -20.601486640645298
+- -2.744145913602563
+- 55.30311671136163
+- -34.9435583572032
+- -100.99868192265687
+- -85.29395233795546
+- 85.20039568455698
+- -176.76835907614668
+- 52.48593025487103
+- 97.31060265994239
+- -30.929742511464113
+- 106.63932096002576
+- 70.18662462848711
+- -10.065425939117953
+- 233.25664578824694
+- -6.447420142414308
+- 70.90920106859251
+- 49.249585826868326
+- -36.66468333200608
+- -8.240139082380157
+- 10.465166172261355
+- 31.417228186284632
+- 55.539924515738676
+- 202.74956671021314
+- 48.025112189715855
+- 44.361990849823506
+- -49.71439627804482
+- 105.97308932272578
+- -54.681766545070616
+- 98.75944838370496
+- 102.01544848216479
+- 25.080873351041458
+- 162.22640090937497
+- -56.31446975078492
+- 85.7752854141861
+- -62.429637512669245
+- 126.79934690184507
+- 131.7605060120888
+- 96.83130931872333
+- 175.34372538316487
+- -0.6565281246829979
+- 48.37504287501842
+- -21.534370462439625
+- 189.09948694132626
+- 180.52136054926117
+- 194.04524677409466
+- 87.25803792296719
+- 69.52770132418094
+- 114.85244694985934
+- 42.35868287770348
+- 170.59899218234273
+- 162.11679795069406
+- 212.00537694330902
+- 183.0337295001356
+- 195.29117473562292
+- 243.69238172951475
+- 95.26529740030269
+- 80.35386774469501
+- 187.86256733907143
+- 89.96477987585646
+- 190.7813245297523
+- 347.311248546426
+- 247.19234149548112
+- 204.08466094102704
+- 226.51696102301247
+- -6.892250336537785
+- 8.575084972868773
+- 164.82689518856662
+- 81.02253267814615
+- 232.71530374822984
+- 167.5681009949404
+- -8.604598362567197
+- 130.16109198377552
+- 146.88239969621665
+- 55.797281491238046
+- 165.4153603423753
+- 178.21146609083027
+- 17.487743041172827
+- 126.81818383969758
+y:
+- -137.45017437663398
+- -168.4801777684752
+- -84.03258737390696
+- -138.8989087267409
+- -2.082247962073727
+- -179.31723443328136
+- -88.42880815947395
+- -4.445333881119488
+- -242.64902467142977
+- -154.9719472652605
+- -242.0149929666361
+- -249.6424766495926
+- -187.5763161104164
+- -103.11645543685971
+- -156.91824303049796
+- -194.47157838400528
+- -83.05585362938487
+- -80.71853643395194
+- -258.3820101897301
+- -43.4329355576356
+- -82.8625965667567
+- -110.65002805845242
+- -84.10590340102833
+- -244.3473862277645
+- -87.37067637111183
+- -209.3757736776577
+- -194.4547880511376
+- -54.453791166857265
+- -47.85715533265113
+- -140.85880857135035
+- -82.301704209561
+- -305.036589417813
+- -88.51830760605337
+- -315.93045937552273
+- -198.54928242188186
+- -51.39778560761003
+- -117.76813593842118
+- -304.60018141822195
+- -154.20041553041935
+- -114.05395035507439
+- -144.7469051481917
+- -17.032567351447295
+- -126.08846452513698
+- -108.60229805313843
+- -61.70125602392602
+- -28.798590721569045
+- -105.1350744180009
+- -88.33021111454664
+- -227.77270995236424
+- -134.5288083889458
+- -108.11291016528452
+- -82.56938099982168
+- -217.23078139291084
+- -129.31246309353085
+- -265.2911672445468
+- -165.85228547123336
+- -289.52740630277026
+- -227.80079397765286
+- -226.78578207175408
+- 2.9445483397250083
+- -264.27926385368545
+- -161.63836424107424
+- -175.12507393760865
+- -61.83230511477935
+- 24.481477359040678
+- -111.267164036688
+- -62.89273463909397
+- -193.98677985400676
+- 75.07146587979011
+- 2.674657153233626
+- -89.63278756755514
+- -20.601486640645298
+- -2.744145913602563
+- 55.30311671136163
+- -34.9435583572032
+- -100.99868192265687
+- -85.29395233795546
+- 85.20039568455698
+- -176.76835907614668
+- 52.48593025487103
+- 97.31060265994239
+- -30.929742511464113
+- 106.63932096002576
+- 70.18662462848711
+- -10.065425939117953
+- 233.25664578824694
+- -6.447420142414308
+- 70.90920106859251
+- 49.249585826868326
+- -36.66468333200608
+- -8.240139082380157
+- 10.465166172261355
+- 31.417228186284632
+- 55.539924515738676
+- 202.74956671021314
+- 48.025112189715855
+- 44.361990849823506
+- -49.71439627804482
+- 105.97308932272578
+- -54.681766545070616
+- 98.75944838370496
+- 102.01544848216479
+- 25.080873351041458
+- 162.22640090937497
+- -56.31446975078492
+- 85.7752854141861
+- -62.429637512669245
+- 126.79934690184507
+- 131.7605060120888
+- 96.83130931872333
+- 175.34372538316487
+- -0.6565281246829979
+- 48.37504287501842
+- -21.534370462439625
+- 189.09948694132626
+- 180.52136054926117
+- 194.04524677409466
+- 87.25803792296719
+- 69.52770132418094
+- 114.85244694985934
+- 42.35868287770348
+- 170.59899218234273
+- 162.11679795069406
+- 212.00537694330902
+- 183.0337295001356
+- 195.29117473562292
+- 243.69238172951475
+- 95.26529740030269
+- 80.35386774469501
+- 187.86256733907143
+- 89.96477987585646
+- 190.7813245297523
+- 347.311248546426
+- 247.19234149548112
+- 204.08466094102704
+- 226.51696102301247
+- -6.892250336537785
+- 8.575084972868773
+- 164.82689518856662
+- 81.02253267814615
+- 232.71530374822984
+- 167.5681009949404
+- -8.604598362567197
+- 130.16109198377552
+- 146.88239969621665
+- 55.797281491238046
+- 165.4153603423753
+- 178.21146609083027
+- 17.487743041172827
+- 126.81818383969758
+z:
+- 158.02326918578157
+- 198.08270713502085
+- 150.40732024803066
+- 146.3603572079876
+- -41.19500332089733
+- 117.37981494138036
+- 2.302923059645904
+- 181.04525381157558
+- 172.63396790383476
+- 178.07163815708606
+- 161.19685538675498
+- 235.8854511963726
+- -19.226544814188046
+- -74.31841279243989
+- 127.29680678830495
+- -25.953878744418766
+- -110.50814340389257
+- 82.83930976554892
+- 120.91697818606839
+- 88.87446888789336
+- -16.902812891557343
+- 56.85478338905072
+- 69.00934463754652
+- -49.68741744154214
+- -9.101679383661015
+- 163.95892454666617
+- 61.43258379892186
+- 216.84420247404822
+- 185.22218746765108
+- 203.21048014672095
+- 97.38371513193675
+- 101.42786754109076
+- 168.99377875245335
+- 170.32463594119045
+- 161.06439769521265
+- -67.42510279636797
+- 159.8491514275903
+- 15.744297785606754
+- 36.96042354896769
+- 204.83440133079174
+- 164.7283072076716
+- 202.1410248341884
+- 77.70482171897073
+- 168.7922911943027
+- 244.00097801951026
+- 2.9843023004946025
+- 273.0393607004863
+- 256.41347082002375
+- 320.6619059534485
+- 401.71251984834294
+- 341.72170179270955
+- 350.1329908329461
+- 91.87121610461129
+- 255.47703533999365
+- 274.5606498965892
+- 342.2655000927155
+- 321.1279205451655
+- 285.48466346517364
+- 308.34554329406774
+- 369.36211101141447
+- 364.1232760577248
+- 240.19948245263313
+- 171.9618405697482
+- 284.3654144937668
+- 518.510436206006
+- 271.4532208272329
+- 138.7198458101441
+- 275.7730273345756
+- 188.89329236924783
+- 343.3625050778569
+- 275.2280291538374
+- 296.65913913594903
+- 405.5704033246094
+- 393.2103893208043
+- 370.27149169906664
+- 317.53326527607913
+- 380.31814269515246
+- 233.59783512066912
+- 342.147557717733
+- 362.9535851810041
+- 419.67160399069854
+- 265.45835389381506
+- 100.90811921422763
+- 354.0969035622084
+- 230.89361930084132
+- 324.40583740601636
+- 244.63191105585182
+- 99.70165437657711
+- 76.0001399413896
+- 261.90507855888444
+- 173.61056687477335
+- 329.9591859361577
+- 132.02454477101037
+- 219.9810010049257
+- 194.0331551005202
+- 55.502571068916865
+- 201.82715465650645
+- 215.71368923197895
+- 78.57045961284781
+- 91.17072503894212
+- 175.74982186894604
+- 41.27330521388918
+- 115.06087974810093
+- 187.2033099661109
+- 89.25423227897329
+- 187.79758532081587
+- 257.12729321932153
+- 125.31070453998093
+- 7.955194514485095
+- 153.56522817739673
+- 70.43107619025358
+- 24.83903870702466
+- 230.27040641909835
+- 307.4996805449256
+- 26.17525713219486
+- 218.64259246883253
+- -20.36896974357387
+- 223.9102604418838
+- 23.735580544108043
+- 109.11430168072079
+- 34.253522859618116
+- 142.25785742496066
+- 98.61000683239934
+- 260.3342095432354
+- 55.89480661163995
+- 311.1381924564003
+- 184.92109716413043
+- 157.19414087415316
+- 226.5880298111837
+- 151.92276857957907
+- 185.68381485151733
+- 215.93525411408908
+- 224.76084322528237
+- 55.86519063765026
+- 84.67151183335257
+- 85.30795753273236
+- 115.51125717589561
+- 37.6626762064897
+- 176.87293034901475
+- 84.54961955131282
+- 174.2064416597917
+- 133.86149628523765
+- 37.05246748249443
+- 288.640906832307
+- 169.94704947901835
+- 132.27640210349853
+- 75.31353581349363
+- -101.88672783775839
+- 83.0232257931865
+- 43.383037925239414
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
new file mode 100644
index 000000000..04bcac2c3
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -174.74077331965347
+- -317.77599446383755
+- -219.5467121612789
+- -34.26527761832079
+- -147.44317330782587
+- -98.57620030231213
+- -151.65272221714105
+- -44.52912641332917
+- -120.52842073352534
+- -243.33107798985682
+- -146.3566941741793
+- -66.8814686102606
+- -161.4122190812015
+- -114.40013392269593
+- -38.20685411333554
+- -76.49757335132858
+- -197.17504303148857
+- -210.53168931461644
+- -143.66662711064376
+- -233.84458118237575
+- -242.51407729894962
+- -104.91425287388788
+- -275.61592897105527
+- -243.75920267794496
+- -148.99487137986938
+- -284.76720175195896
+- -153.2791290521435
+- -106.26379155244385
+- -58.315973561353324
+- -202.43553120675654
+- -271.8081796267609
+- -128.4538837575546
+- -256.8534165017398
+- -167.40086328050833
+- -203.42908148748006
+- -44.36420499045635
+- -128.6498221073226
+- -269.27474389913846
+- -187.54103386976064
+- -315.71738715196653
+- -94.25734610085317
+- -144.04783055304117
+- -175.92457602755331
+- -213.4156276574594
+- -230.8093559485084
+- -185.84014917003304
+- -291.64188526827536
+- -241.01551292767718
+- -130.3470358146145
+- -198.0324329405659
+- -118.50455467300665
+- -109.13480914568198
+- -193.32769271564604
+- -85.61335922073
+- -124.51840287108118
+- -60.773949608455375
+- -92.21173022310548
+- -182.67613016758864
+- -260.849987866747
+- -78.95419597293886
+- -183.53742148620532
+- -126.55170547396766
+- -125.66020139022245
+- -109.19253515982136
+- -249.09387545053977
+- -126.6997017235669
+- -165.4853241880315
+- -183.1706321258363
+- -199.8006557550405
+- -142.0527901441929
+- -167.10657425912635
+- -72.13940725945159
+- -279.8488971483063
+- -190.76855531083888
+- -132.5052445914984
+- -186.31908792026428
+- -209.89053502992988
+- -194.54755649800435
+- -153.0563005077373
+- -175.31810037916114
+- -41.63516928736321
+- -289.6292074142173
+- -92.88580865593525
+- -201.57744071303082
+- -78.38496389675379
+- -102.37369818911844
+- -74.6941585482721
+- -237.56831160628943
+- -83.0022006028396
+- -242.9428963930302
+- -85.03874434022363
+- -105.36384492482497
+- -28.85214575091783
+- -121.28597754698242
+- -112.98964492193501
+- -2.9704925482979343
+- -119.57960415938308
+- -54.39173259806981
+- -60.4551546110456
+- -185.4739817360681
+- -164.71170765702635
+- -149.98905385415978
+- -211.76348838481712
+- -154.637051920341
+- -91.12614465455991
+- -95.57479207224236
+- -101.00267532749719
+- -111.93048321131528
+- -107.33955687007294
+- -52.93771532559152
+- -209.68143299171118
+- -64.20048990968746
+- -135.17922853492266
+- 42.619874283916594
+- -158.7504767982497
+- -139.61399692578732
+- -81.7830212971333
+- -93.49980450264374
+- -84.55991328286864
+- -183.16373315868958
+- -92.29208141458975
+- -33.488686584469505
+- -134.74813619936114
+- -99.31665471393255
+- -19.88126490813449
+- -16.409316431238704
+- -51.283508604150256
+- 22.053827055762667
+- 40.94648542458586
+- 4.900737059650758
+- -187.48534202576323
+- 34.45397696624966
+- -147.99163815898328
+- -60.54858213778748
+- -60.44634044362913
+- -115.13511819292981
+- -77.03381078997329
+- -39.63800983797401
+- -63.16467555367657
+- -96.24228971822636
+- -112.2598000100672
+- -232.1311472797242
+- -64.84084437939381
+- -112.0269519592484
+- -185.77267272757916
+- -80.48193848533441
+- -52.301216673442084
+- -52.03501539130916
+- -74.83519492876805
+- -170.03463198683247
+y:
+- -174.74077331965347
+- -317.77599446383755
+- -219.5467121612789
+- -34.26527761832079
+- -147.44317330782587
+- -98.57620030231213
+- -151.65272221714105
+- -44.52912641332917
+- -120.52842073352534
+- -243.33107798985682
+- -146.3566941741793
+- -66.8814686102606
+- -161.4122190812015
+- -114.40013392269593
+- -38.20685411333554
+- -76.49757335132858
+- -197.17504303148857
+- -210.53168931461644
+- -143.66662711064376
+- -233.84458118237575
+- -242.51407729894962
+- -104.91425287388788
+- -275.61592897105527
+- -243.75920267794496
+- -148.99487137986938
+- -284.76720175195896
+- -153.2791290521435
+- -106.26379155244385
+- -58.315973561353324
+- -202.43553120675654
+- -271.8081796267609
+- -128.4538837575546
+- -256.8534165017398
+- -167.40086328050833
+- -203.42908148748006
+- -44.36420499045635
+- -128.6498221073226
+- -269.27474389913846
+- -187.54103386976064
+- -315.71738715196653
+- -94.25734610085317
+- -144.04783055304117
+- -175.92457602755331
+- -213.4156276574594
+- -230.8093559485084
+- -185.84014917003304
+- -291.64188526827536
+- -241.01551292767718
+- -130.3470358146145
+- -198.0324329405659
+- -118.50455467300665
+- -109.13480914568198
+- -193.32769271564604
+- -85.61335922073
+- -124.51840287108118
+- -60.773949608455375
+- -92.21173022310548
+- -182.67613016758864
+- -260.849987866747
+- -78.95419597293886
+- -183.53742148620532
+- -126.55170547396766
+- -125.66020139022245
+- -109.19253515982136
+- -249.09387545053977
+- -126.6997017235669
+- -165.4853241880315
+- -183.1706321258363
+- -199.8006557550405
+- -142.0527901441929
+- -167.10657425912635
+- -72.13940725945159
+- -279.8488971483063
+- -190.76855531083888
+- -132.5052445914984
+- -186.31908792026428
+- -209.89053502992988
+- -194.54755649800435
+- -153.0563005077373
+- -175.31810037916114
+- -41.63516928736321
+- -289.6292074142173
+- -92.88580865593525
+- -201.57744071303082
+- -78.38496389675379
+- -102.37369818911844
+- -74.6941585482721
+- -237.56831160628943
+- -83.0022006028396
+- -242.9428963930302
+- -85.03874434022363
+- -105.36384492482497
+- -28.85214575091783
+- -121.28597754698242
+- -112.98964492193501
+- -2.9704925482979343
+- -119.57960415938308
+- -54.39173259806981
+- -60.4551546110456
+- -185.4739817360681
+- -164.71170765702635
+- -149.98905385415978
+- -211.76348838481712
+- -154.637051920341
+- -91.12614465455991
+- -95.57479207224236
+- -101.00267532749719
+- -111.93048321131528
+- -107.33955687007294
+- -52.93771532559152
+- -209.68143299171118
+- -64.20048990968746
+- -135.17922853492266
+- 42.619874283916594
+- -158.7504767982497
+- -139.61399692578732
+- -81.7830212971333
+- -93.49980450264374
+- -84.55991328286864
+- -183.16373315868958
+- -92.29208141458975
+- -33.488686584469505
+- -134.74813619936114
+- -99.31665471393255
+- -19.88126490813449
+- -16.409316431238704
+- -51.283508604150256
+- 22.053827055762667
+- 40.94648542458586
+- 4.900737059650758
+- -187.48534202576323
+- 34.45397696624966
+- -147.99163815898328
+- -60.54858213778748
+- -60.44634044362913
+- -115.13511819292981
+- -77.03381078997329
+- -39.63800983797401
+- -63.16467555367657
+- -96.24228971822636
+- -112.2598000100672
+- -232.1311472797242
+- -64.84084437939381
+- -112.0269519592484
+- -185.77267272757916
+- -80.48193848533441
+- -52.301216673442084
+- -52.03501539130916
+- -74.83519492876805
+- -170.03463198683247
+z:
+- -14.76633010405184
+- -77.08097113274366
+- -105.94106972116744
+- -168.2090926517627
+- -65.26551043544558
+- -20.6880089422812
+- -209.32696148448377
+- -24.40713791495554
+- -15.75365922487368
+- -254.89155753037986
+- -152.5052404899702
+- -1.1269359922330804
+- -116.56557358745104
+- -13.061463181014744
+- -271.5965621707225
+- -152.31276368325067
+- -92.2902567904771
+- -141.07917492886594
+- 21.69011355459171
+- -183.34206401478107
+- 10.140801536451853
+- -35.95343467815701
+- -26.211057572707027
+- -112.78898766720883
+- -133.29063998526433
+- -61.18238284950703
+- -80.06503202029097
+- 12.422039484044944
+- -5.12101512804493
+- -73.39808051629576
+- -110.4416005782697
+- -111.83179889807782
+- -179.2128322791779
+- -79.85810810889649
+- -126.23405124577191
+- -96.50900697911743
+- -18.125097261698677
+- -36.57092491856446
+- 51.943730895861094
+- -157.448224578843
+- -53.56098577197906
+- 36.21099531070003
+- 30.264159110500028
+- -128.81850270881264
+- -81.23129461063239
+- -16.456676449321943
+- -86.63729199837879
+- 24.60007469359832
+- -6.6448094065609515
+- -60.979163653950344
+- 48.874877564359494
+- 17.360966984906003
+- -23.29246474410196
+- -29.828427844083933
+- -33.31738239206807
+- -122.86436652286201
+- -111.42238758800114
+- 56.19728485994878
+- 115.70687319588221
+- -83.61672979309319
+- -120.59739183438491
+- -115.46930927809868
+- -139.47286172515123
+- -110.90601146953438
+- -121.22900189214741
+- -43.967041616093915
+- -63.48893808876695
+- -89.77046020404639
+- -102.48135990357295
+- -3.941185563845633
+- -48.45091405021118
+- -45.43625821365923
+- -69.85638375546341
+- -103.33442348332125
+- -23.02226952758559
+- -223.90372628887343
+- -103.82545014924584
+- -31.392814108062087
+- -99.22982433555555
+- -122.95666748923703
+- -31.6125389866287
+- -214.46597305215195
+- -33.16077540127025
+- 24.654472285743843
+- -126.06881773113969
+- -67.15036556927743
+- -103.51112759867529
+- -112.49273446342748
+- -30.58986293113707
+- -163.59145625259794
+- -134.38496336676516
+- -20.866372315040756
+- 65.51547950531781
+- -130.19304612277347
+- -64.13112509975545
+- 17.532331154672615
+- -152.28636639791674
+- -55.41347054315299
+- 10.891619057837156
+- -96.59563281422255
+- -48.856284332006204
+- 19.128375666118075
+- -100.39353753428146
+- -62.00837479965283
+- -97.80252980411373
+- -172.83316495582778
+- -145.1309805338278
+- -98.20698079942892
+- -190.75540032222162
+- -185.38968463427997
+- -139.3738320774594
+- -9.253472269775633
+- -139.07770738320693
+- -46.58774101188062
+- -67.47683277359685
+- -46.000235484769
+- -128.71964463728597
+- -170.5977947504874
+- -171.99109962011153
+- -211.3584965405287
+- -80.57163032129169
+- -59.78250802996865
+- -62.23533058218616
+- -117.40181697835742
+- -241.97887047936482
+- -142.29890186792912
+- -28.72563853627365
+- -36.10084290447162
+- -81.1411882563499
+- -153.2271564287201
+- -21.680286042756165
+- -169.83235917233586
+- -81.7474230057096
+- 24.328386528281765
+- -155.73154838932504
+- -123.70063663965863
+- -217.83196054157236
+- -48.865261345157315
+- -64.46898611738675
+- -27.99725847937989
+- -150.85674510009306
+- -275.2284467608632
+- -54.48520332289754
+- -169.19356035925688
+- -71.20531706370637
+- -148.68804594177016
+- -81.77316440334143
+- -169.1920506442319
+- -108.23662676360165
+- -207.56861909360228
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
new file mode 100644
index 000000000..6929a11c6
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -205.5073692403897
+- -333.8446422195872
+- -240.82848008347725
+- -34.622646511974
+- -231.33240480034596
+- -85.15589672049516
+- -197.97259670710187
+- -96.13649621053189
+- -117.25003949936774
+- -265.745212141466
+- -165.66798157239478
+- -67.0350020400283
+- -165.6821548611103
+- -157.30589525965246
+- -67.42267898495412
+- -106.78925116367964
+- -231.98113815964334
+- -186.94504476809632
+- -80.1500018270306
+- -243.5120854871698
+- -201.97475895474537
+- -117.3697995059603
+- -314.71066180673483
+- -172.4539994573838
+- -121.71782922037677
+- -271.2296926982156
+- -149.53285959062183
+- -72.32153197984707
+- -127.63059956232463
+- -243.22740742519383
+- -280.3560979666143
+- -135.13571134484133
+- -297.1012270733985
+- -193.27899031735822
+- -213.4462636898764
+- 42.10367780595005
+- -32.122599213996295
+- -380.69262010862843
+- -200.5824703573119
+- -301.90687604870493
+- -161.08961691428095
+- -161.0344471628078
+- -110.00772679029647
+- -228.99670042626514
+- -207.5491966101806
+- -186.80013895821764
+- -387.589252741825
+- -225.66502306034883
+- -83.31435689593634
+- -218.66335734305827
+- -69.25486177770466
+- -105.4457913281818
+- -223.41387263225707
+- -85.45899984407099
+- -95.90034310580366
+- -32.63396447619502
+- -109.12913821181753
+- -141.64720436748453
+- -227.12572391663437
+- -66.33070888759842
+- -230.11521592700862
+- -86.86377324570762
+- -224.71639415324486
+- -102.77590262940494
+- -273.64514899235655
+- -127.23005150607317
+- -209.51848772760508
+- -163.6540354561236
+- -247.55780106506907
+- -179.36027234893194
+- -140.49840958849435
+- -132.17841652905238
+- -418.40899382971077
+- -206.7592009101982
+- -214.24443972554084
+- -201.04947109656618
+- -243.868201376032
+- -215.0975985449305
+- -139.17310216223078
+- -234.36410147342414
+- -29.96159604631133
+- -288.22566714032934
+- -176.4287024356889
+- -192.76384105737392
+- -14.631224403025561
+- -53.805996201129986
+- 44.83703287605151
+- -202.49438461174935
+- -129.37833935516377
+- -250.081667630867
+- -122.89175588898127
+- -194.1740790035445
+- -59.98724901084508
+- -139.2262557594861
+- -177.03448844433083
+- -50.669669531058986
+- -183.80272176628054
+- -28.837170523677816
+- -49.162610117278746
+- -181.3411409109815
+- -168.89529428518708
+- -138.03375359420133
+- -199.49155690007035
+- -205.62261337454152
+- -136.40088062435441
+- -88.088204348715
+- -57.10571256735283
+- -65.98440925687532
+- -131.2768328086759
+- -62.00050974039761
+- -238.72599963259066
+- -163.19014187616423
+- -119.54986616157132
+- -5.142747722953934
+- -145.7991459659883
+- -193.01963318383585
+- -43.09932930022754
+- -153.77073722641566
+- -57.395317195035936
+- -178.41875877799393
+- -76.69822796371398
+- -20.398463084317548
+- -126.71888439794202
+- -110.05448964238323
+- -8.083631023659926
+- -87.75469300588043
+- -89.17418101790668
+- -3.0415198841386974
+- 19.66450814303073
+- 89.06742995211957
+- -226.98416387334797
+- 56.38445627805352
+- -90.97952413487529
+- -4.854519176660294
+- -94.72569691060215
+- -151.39086769029157
+- -87.57889059581153
+- 24.53879952610265
+- 25.100432078623555
+- -85.44774617358404
+- -53.161816966462084
+- -238.04823141630519
+- -50.98954520951396
+- -95.77405665909292
+- -172.29037747270138
+- -117.37996263003917
+- -23.16330476817926
+- -70.40785375242959
+- -65.3325153105929
+- -207.12566352810137
+y:
+- -205.5073692403897
+- -333.8446422195872
+- -240.82848008347725
+- -34.622646511974
+- -231.33240480034596
+- -85.15589672049516
+- -197.97259670710187
+- -96.13649621053189
+- -117.25003949936774
+- -265.745212141466
+- -165.66798157239478
+- -67.0350020400283
+- -165.6821548611103
+- -157.30589525965246
+- -67.42267898495412
+- -106.78925116367964
+- -231.98113815964334
+- -186.94504476809632
+- -80.1500018270306
+- -243.5120854871698
+- -201.97475895474537
+- -117.3697995059603
+- -314.71066180673483
+- -172.4539994573838
+- -121.71782922037677
+- -271.2296926982156
+- -149.53285959062183
+- -72.32153197984707
+- -127.63059956232463
+- -243.22740742519383
+- -280.3560979666143
+- -135.13571134484133
+- -297.1012270733985
+- -193.27899031735822
+- -213.4462636898764
+- 42.10367780595005
+- -32.122599213996295
+- -380.69262010862843
+- -200.5824703573119
+- -301.90687604870493
+- -161.08961691428095
+- -161.0344471628078
+- -110.00772679029647
+- -228.99670042626514
+- -207.5491966101806
+- -186.80013895821764
+- -387.589252741825
+- -225.66502306034883
+- -83.31435689593634
+- -218.66335734305827
+- -69.25486177770466
+- -105.4457913281818
+- -223.41387263225707
+- -85.45899984407099
+- -95.90034310580366
+- -32.63396447619502
+- -109.12913821181753
+- -141.64720436748453
+- -227.12572391663437
+- -66.33070888759842
+- -230.11521592700862
+- -86.86377324570762
+- -224.71639415324486
+- -102.77590262940494
+- -273.64514899235655
+- -127.23005150607317
+- -209.51848772760508
+- -163.6540354561236
+- -247.55780106506907
+- -179.36027234893194
+- -140.49840958849435
+- -132.17841652905238
+- -418.40899382971077
+- -206.7592009101982
+- -214.24443972554084
+- -201.04947109656618
+- -243.868201376032
+- -215.0975985449305
+- -139.17310216223078
+- -234.36410147342414
+- -29.96159604631133
+- -288.22566714032934
+- -176.4287024356889
+- -192.76384105737392
+- -14.631224403025561
+- -53.805996201129986
+- 44.83703287605151
+- -202.49438461174935
+- -129.37833935516377
+- -250.081667630867
+- -122.89175588898127
+- -194.1740790035445
+- -59.98724901084508
+- -139.2262557594861
+- -177.03448844433083
+- -50.669669531058986
+- -183.80272176628054
+- -28.837170523677816
+- -49.162610117278746
+- -181.3411409109815
+- -168.89529428518708
+- -138.03375359420133
+- -199.49155690007035
+- -205.62261337454152
+- -136.40088062435441
+- -88.088204348715
+- -57.10571256735283
+- -65.98440925687532
+- -131.2768328086759
+- -62.00050974039761
+- -238.72599963259066
+- -163.19014187616423
+- -119.54986616157132
+- -5.142747722953934
+- -145.7991459659883
+- -193.01963318383585
+- -43.09932930022754
+- -153.77073722641566
+- -57.395317195035936
+- -178.41875877799393
+- -76.69822796371398
+- -20.398463084317548
+- -126.71888439794202
+- -110.05448964238323
+- -8.083631023659926
+- -87.75469300588043
+- -89.17418101790668
+- -3.0415198841386974
+- 19.66450814303073
+- 89.06742995211957
+- -226.98416387334797
+- 56.38445627805352
+- -90.97952413487529
+- -4.854519176660294
+- -94.72569691060215
+- -151.39086769029157
+- -87.57889059581153
+- 24.53879952610265
+- 25.100432078623555
+- -85.44774617358404
+- -53.161816966462084
+- -238.04823141630519
+- -50.98954520951396
+- -95.77405665909292
+- -172.29037747270138
+- -117.37996263003917
+- -23.16330476817926
+- -70.40785375242959
+- -65.3325153105929
+- -207.12566352810137
+z:
+- 44.54967297927881
+- -117.16757118661646
+- -68.33755789455105
+- -138.74005721401338
+- 34.58285849524722
+- 15.330254703611928
+- -114.05530604971351
+- -47.60804643043136
+- 19.271767405679974
+- -256.8454546028996
+- -157.19920585852938
+- 104.15974226892733
+- -104.65170305868624
+- 61.493441947002204
+- -291.7568700944766
+- -159.40968476667373
+- -56.03453672222339
+- -63.55137320513116
+- 50.822290127165665
+- -175.0549242860695
+- 40.85264895848032
+- -41.0224334277619
+- -24.534338456391033
+- -110.377582980965
+- -105.47069495224905
+- -94.71463256051653
+- -34.156812190008395
+- 46.61081171776186
+- 53.89730961060682
+- -147.50050733652608
+- -99.60710327037035
+- -141.62986356696658
+- -75.46486054768573
+- -86.83438468006057
+- -175.76748346837178
+- -53.66361281353443
+- 76.2349046407901
+- 42.89867715681811
+- 44.36087324888505
+- -151.31488346461322
+- 11.001424898658124
+- -42.033029630440815
+- 34.201178146930665
+- -119.7652257231709
+- -114.09731803946741
+- -43.453364804491706
+- -89.35198575438656
+- 65.32418853178122
+- 18.053772029339843
+- -128.16621400153113
+- 10.818175764679484
+- 20.613799151053524
+- 56.03312143173241
+- 32.480263524770805
+- -90.97817406112694
+- -206.39409072735708
+- -82.34300404109355
+- 146.6328223094321
+- 109.44929187541884
+- -195.38953562541084
+- -119.78042498811313
+- -162.42497560793868
+- -141.46675576366178
+- -222.9598593555579
+- -219.3985470081475
+- 8.371432339208495
+- -27.515931464708974
+- -66.8034902186602
+- -147.90505169357098
+- 9.04071412975171
+- -102.34268473601138
+- -9.010132318158092
+- -101.88906053637167
+- -213.7318632222936
+- -40.37989777188767
+- -229.46131818547656
+- -115.73436418036627
+- -112.09995046482273
+- -133.85742307890467
+- -170.61867168059226
+- -114.13383761396045
+- -276.03491377429583
+- -53.72887029097178
+- -3.0950423485502823
+- -225.02704377362778
+- -29.942434206076715
+- -142.10677112799746
+- -144.5980482004309
+- -78.30768393814688
+- -163.39440966340845
+- -168.84461183155804
+- -19.517681806181628
+- -20.355612948011064
+- -139.50687114077869
+- -34.00713601611943
+- 72.90963935350096
+- -149.97649850251122
+- -13.058609028247039
+- -5.112707222930133
+- -82.83757405410596
+- -44.62838684114432
+- -16.376617136832387
+- -129.74816320140673
+- -25.679562224728357
+- -127.47448603673031
+- -170.8167299619752
+- -132.9305810037043
+- -89.62982210011754
+- -262.3178944687407
+- -167.29115281640043
+- -205.44334214206472
+- -30.46881931069258
+- -140.0084272382212
+- -41.43019684893294
+- -29.382703424136345
+- -88.73459328600485
+- -195.60376665146785
+- -104.52072341663803
+- -123.57116121667725
+- -206.44903101292434
+- -42.518860984416555
+- -32.839369965735386
+- -133.79440841705545
+- -76.985328068373
+- -252.76459902899148
+- -150.44906624211123
+- -87.0292319728042
+- 80.414598235004
+- -46.457279898543376
+- -135.1024382257102
+- -72.86667366907751
+- -119.61553450500578
+- -42.07280456612249
+- 17.36961952148788
+- -135.28675415983665
+- -91.28263410661891
+- -178.57408057229455
+- -104.37552702097844
+- -66.67739184951493
+- -32.0515952864862
+- -167.64973060057116
+- -271.996689188636
+- -37.09870662152558
+- -154.41410030569045
+- -121.17914986100394
+- -169.10121709846476
+- -87.52442642504427
+- -144.308647591676
+- -170.29484389514315
+- -162.37157500544316
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
new file mode 100644
index 000000000..eba346c3d
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -272.43926763181287
+- -448.95097433506004
+- -236.16535004240544
+- -37.76856392971942
+- -180.658962619701
+- -109.20163171085781
+- -199.945554996803
+- -68.90194452971632
+- -135.30613178737158
+- -223.60190482205596
+- -190.55699992587364
+- -130.6451273673297
+- -200.68440674896308
+- -262.2682066040087
+- -47.7449269217694
+- -95.04431831226468
+- -145.35864396914786
+- -200.6743090884891
+- 3.7251780830580787
+- -294.93243387150204
+- -313.69099949174336
+- -99.11066833399032
+- -313.3951354508127
+- -205.5559672495546
+- -139.1387047489721
+- -275.63217701758543
+- -107.07211702595991
+- -5.9469051443245045
+- -163.72650355207833
+- -270.99411753819425
+- -277.4419276241587
+- -18.788685401667642
+- -253.62296756705157
+- -142.5147933674709
+- -187.72562194153429
+- 36.43638736764589
+- 34.48678419943111
+- -289.83782982823584
+- -176.40183289325586
+- -351.33906087051105
+- -121.36788858125071
+- -174.24380692956095
+- -98.62036069690163
+- -240.9168724677785
+- -231.4324677236045
+- -206.6613718247246
+- -406.4727718202573
+- -225.1610009072952
+- 29.49527751562229
+- -273.65646034143714
+- -96.59222344210355
+- -162.23353009771722
+- -285.8236665686258
+- -199.74511755862125
+- -215.07805281823997
+- -36.18075902907856
+- -191.31623789060947
+- -101.6375852133703
+- -243.13184112407623
+- -19.2220979651675
+- -308.0664791104669
+- -83.4234720411692
+- -221.7978116147051
+- -54.60773482968091
+- -298.4940871611296
+- -44.74363435246878
+- -333.532488561383
+- -217.03368408170132
+- -264.2771572798883
+- -154.1746872913021
+- -197.51980936266472
+- -158.70581789257093
+- -335.9275171564795
+- -222.87545271783046
+- -157.3683247757442
+- -233.7513146482435
+- -296.57684315928617
+- -219.48252748393207
+- -48.133547220421235
+- -155.26662426992243
+- -36.10288902977462
+- -194.83693975598814
+- -241.6387007049145
+- -242.48913974641644
+- 36.02518983336963
+- -0.1576404832974825
+- 113.58065834324691
+- -271.2810951422604
+- -174.88764583856945
+- -124.37969764150394
+- -84.85385273337539
+- -181.70365569998665
+- -73.74984082720982
+- -129.10692313606563
+- -134.64557613856581
+- 30.91736025594859
+- -167.3711207815298
+- -49.8615388491973
+- -4.6387999504108945
+- -215.2502217817841
+- -156.89246466465733
+- -108.41620488140606
+- -183.8464655986025
+- -264.7692849942317
+- -97.35901020025415
+- -162.16983307806206
+- -52.2525646803888
+- 17.23526023845099
+- -94.96670981662531
+- -55.02980165867298
+- -257.3030900525307
+- -110.99333526558722
+- -83.18280817502327
+- 43.3175391799086
+- -252.90093098239393
+- -194.0767310127264
+- -90.02653456048056
+- -161.17870613607627
+- -66.10552713628458
+- -184.4202808248762
+- -8.88045546106721
+- -18.59324777165611
+- -56.03783797870011
+- -53.2062297088678
+- 41.482966759745146
+- -65.49093472082363
+- -49.90731228303498
+- 36.708761810916734
+- -3.7616971171956592
+- 96.68901411086348
+- -169.96599244788874
+- 11.53652344408845
+- -107.20161855221879
+- -33.57451345086991
+- -24.044150838980794
+- -132.7895303438664
+- -29.23104392019398
+- 14.998460109321712
+- 49.94060138120499
+- -78.2161582237574
+- -104.2790849906903
+- -313.4798845657733
+- -6.362771935071262
+- -38.65859508262323
+- -149.16338626268612
+- -118.58877401216215
+- -8.565606447612362
+- -185.41328952312594
+- -26.70780831753186
+- -158.37188125783825
+y:
+- -272.43926763181287
+- -448.95097433506004
+- -236.16535004240544
+- -37.76856392971942
+- -180.658962619701
+- -109.20163171085781
+- -199.945554996803
+- -68.90194452971632
+- -135.30613178737158
+- -223.60190482205596
+- -190.55699992587364
+- -130.6451273673297
+- -200.68440674896308
+- -262.2682066040087
+- -47.7449269217694
+- -95.04431831226468
+- -145.35864396914786
+- -200.6743090884891
+- 3.7251780830580787
+- -294.93243387150204
+- -313.69099949174336
+- -99.11066833399032
+- -313.3951354508127
+- -205.5559672495546
+- -139.1387047489721
+- -275.63217701758543
+- -107.07211702595991
+- -5.9469051443245045
+- -163.72650355207833
+- -270.99411753819425
+- -277.4419276241587
+- -18.788685401667642
+- -253.62296756705157
+- -142.5147933674709
+- -187.72562194153429
+- 36.43638736764589
+- 34.48678419943111
+- -289.83782982823584
+- -176.40183289325586
+- -351.33906087051105
+- -121.36788858125071
+- -174.24380692956095
+- -98.62036069690163
+- -240.9168724677785
+- -231.4324677236045
+- -206.6613718247246
+- -406.4727718202573
+- -225.1610009072952
+- 29.49527751562229
+- -273.65646034143714
+- -96.59222344210355
+- -162.23353009771722
+- -285.8236665686258
+- -199.74511755862125
+- -215.07805281823997
+- -36.18075902907856
+- -191.31623789060947
+- -101.6375852133703
+- -243.13184112407623
+- -19.2220979651675
+- -308.0664791104669
+- -83.4234720411692
+- -221.7978116147051
+- -54.60773482968091
+- -298.4940871611296
+- -44.74363435246878
+- -333.532488561383
+- -217.03368408170132
+- -264.2771572798883
+- -154.1746872913021
+- -197.51980936266472
+- -158.70581789257093
+- -335.9275171564795
+- -222.87545271783046
+- -157.3683247757442
+- -233.7513146482435
+- -296.57684315928617
+- -219.48252748393207
+- -48.133547220421235
+- -155.26662426992243
+- -36.10288902977462
+- -194.83693975598814
+- -241.6387007049145
+- -242.48913974641644
+- 36.02518983336963
+- -0.1576404832974825
+- 113.58065834324691
+- -271.2810951422604
+- -174.88764583856945
+- -124.37969764150394
+- -84.85385273337539
+- -181.70365569998665
+- -73.74984082720982
+- -129.10692313606563
+- -134.64557613856581
+- 30.91736025594859
+- -167.3711207815298
+- -49.8615388491973
+- -4.6387999504108945
+- -215.2502217817841
+- -156.89246466465733
+- -108.41620488140606
+- -183.8464655986025
+- -264.7692849942317
+- -97.35901020025415
+- -162.16983307806206
+- -52.2525646803888
+- 17.23526023845099
+- -94.96670981662531
+- -55.02980165867298
+- -257.3030900525307
+- -110.99333526558722
+- -83.18280817502327
+- 43.3175391799086
+- -252.90093098239393
+- -194.0767310127264
+- -90.02653456048056
+- -161.17870613607627
+- -66.10552713628458
+- -184.4202808248762
+- -8.88045546106721
+- -18.59324777165611
+- -56.03783797870011
+- -53.2062297088678
+- 41.482966759745146
+- -65.49093472082363
+- -49.90731228303498
+- 36.708761810916734
+- -3.7616971171956592
+- 96.68901411086348
+- -169.96599244788874
+- 11.53652344408845
+- -107.20161855221879
+- -33.57451345086991
+- -24.044150838980794
+- -132.7895303438664
+- -29.23104392019398
+- 14.998460109321712
+- 49.94060138120499
+- -78.2161582237574
+- -104.2790849906903
+- -313.4798845657733
+- -6.362771935071262
+- -38.65859508262323
+- -149.16338626268612
+- -118.58877401216215
+- -8.565606447612362
+- -185.41328952312594
+- -26.70780831753186
+- -158.37188125783825
+z:
+- 124.57747204454569
+- -121.94731283243988
+- -126.56271330646996
+- -112.52506529681581
+- 111.84006675738534
+- -10.239780687477998
+- -129.40171936532252
+- -71.93133365614328
+- 129.35372191916255
+- -283.9623901835431
+- -134.82889622818732
+- 155.40490774304192
+- -68.72362884968071
+- 14.641506473353738
+- -316.88396368414703
+- -166.2550474058331
+- -60.294336072837574
+- -124.66984347102944
+- 57.989467179887384
+- -213.96827177603473
+- 67.39398238079399
+- -16.49897735603517
+- 57.81813545001022
+- -151.0875822842148
+- -72.40843493966484
+- -86.13286036511306
+- 57.0274088708988
+- 123.813661903704
+- 70.81124080975727
+- -141.03475610168258
+- -20.6009597723412
+- -220.90071425002117
+- -7.155264997956961
+- -93.68018930666585
+- -86.33561755686196
+- -88.70235774148611
+- 38.600483227150185
+- -23.490281209885353
+- 100.17141885122756
+- -117.51420568382622
+- 61.27652531381027
+- -91.31899669642479
+- 79.99658412138454
+- -155.73416936908632
+- -192.04143425045172
+- 43.506173590381586
+- -68.85001006111823
+- 29.3336986485646
+- -51.37955584463
+- -156.31200332582014
+- -44.36632869522208
+- 37.371284814496
+- -19.038473678991394
+- 119.03995979661157
+- -138.3768289372469
+- -232.00562414557456
+- -142.37593295321378
+- 176.91568433929385
+- -10.932853233306853
+- -143.59868184934047
+- -204.4487164006846
+- -151.05438910443263
+- -222.4148720716179
+- -248.81173690786622
+- -384.9068023836999
+- -82.01848217627835
+- -20.57408724153294
+- -117.97140666411855
+- -195.48809940928075
+- -139.90701538379025
+- -83.34090458005305
+- -42.79122366878542
+- -109.30063060111074
+- -165.32982134003694
+- -65.80340068091773
+- -239.48836465386094
+- -158.6173832006208
+- -172.20378206597073
+- -203.64645596162404
+- -229.7719051374468
+- -122.47150344241452
+- -213.2823807404075
+- -87.10066114492069
+- -13.167198095349415
+- -243.93714190837866
+- -28.453369455803983
+- -188.7196445212271
+- -105.41711481724624
+- -108.1354006318777
+- -56.242518098961604
+- -257.6777248502073
+- 58.84899341475319
+- -27.42180667742504
+- -157.38936980766042
+- -47.344595672173625
+- 74.97224991724782
+- -120.52346040193677
+- -79.7146187030521
+- -40.82187666019393
+- -190.56672793182375
+- -40.006537212489285
+- -93.30526465734067
+- -134.78802918564884
+- 31.923504674280327
+- -87.29057209262477
+- -196.03259697679803
+- -98.4343674791983
+- -126.94109522819964
+- -349.76581839417526
+- -176.07681326431566
+- -224.66008538510738
+- 29.933741187412725
+- -123.95944303183958
+- -20.720566803283322
+- 90.38755013043577
+- -46.52055130423836
+- -190.36554085387718
+- -37.275677534262485
+- -108.5631784099128
+- -259.8011713719343
+- -95.08005177883054
+- -86.75138906952814
+- -122.0460091387302
+- -49.062248288068865
+- -229.7146564892436
+- -201.92998766642376
+- -108.76032605399638
+- 85.28221084750679
+- -11.81228223985709
+- -185.78475524974968
+- -20.90891174074232
+- -74.86710648006371
+- -128.38830281127127
+- 131.395680993159
+- -118.01473503047335
+- -64.05863974700276
+- -219.6977471095694
+- -19.38851411650937
+- -47.82382740185165
+- 67.6280800778232
+- -208.00733502584072
+- -263.12702086481744
+- -52.01001338587882
+- -152.6557062149739
+- -95.56890304553689
+- -200.29100674666495
+- -47.512498615468914
+- -103.85651781817371
+- -131.53030224442517
+- -122.21710160107816
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
new file mode 100644
index 000000000..8627242ee
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -318.33839265125806
+- -503.8732576178618
+- -177.77775964139093
+- -6.7492803260761995
+- -242.03813817935685
+- -77.71979481050029
+- -220.73421600647907
+- -90.27435824161067
+- -153.0275889294634
+- -200.48419383845444
+- -156.471884436833
+- -103.21870798204141
+- -222.84791678035685
+- -303.96349284278716
+- -76.73068739780327
+- -31.624809568016307
+- -153.82357481298533
+- -234.22262159091488
+- 86.72169514942202
+- -256.36840286665006
+- -309.9685665351012
+- -63.1932884859172
+- -322.6519017052047
+- -243.47703866255534
+- -158.51341919021516
+- -275.6868839717208
+- -104.84781924329988
+- 55.86783354402407
+- -148.4564593378266
+- -295.1219553718082
+- -276.5514759444907
+- -9.19806696670726
+- -226.3586168241507
+- -135.77779938569918
+- -124.7290399124978
+- 40.97147405813215
+- 47.9572155566829
+- -209.71160766724336
+- -206.8022387130818
+- -346.19115925055246
+- -194.68448470169963
+- -232.38547362052498
+- -46.81884319670084
+- -284.83539234521766
+- -291.55024983848756
+- -215.3595844343681
+- -469.2967940360148
+- -230.07806535442114
+- 15.756084988812708
+- -183.70819458902477
+- -115.7163594887839
+- -215.5334693249218
+- -280.6752838173983
+- -221.57247204002815
+- -300.5664169621706
+- -79.32812332194474
+- -179.40836211563925
+- -168.79969478638898
+- -276.29289348584774
+- -46.37362441829391
+- -231.29505409608572
+- -70.18085780324427
+- -252.23939541471805
+- -19.12968984192443
+- -296.13422955076334
+- -100.16294002462993
+- -342.90714896056124
+- -215.61769241860063
+- -296.75155474429926
+- -152.6591211649526
+- -188.693765231233
+- -205.91018839738655
+- -416.03035939248605
+- -199.40149048359942
+- -126.83154515228814
+- -222.1926200930844
+- -309.4416518904729
+- -254.09193980366175
+- -0.5418580888411685
+- -155.89526333672188
+- -9.890265411787345
+- -193.7291905240682
+- -263.22603077790814
+- -290.4470899118978
+- 42.80107774241674
+- 16.546189863000382
+- 87.11142726739735
+- -266.371727169003
+- -167.52530632906425
+- -194.22729876901454
+- -119.62496574880039
+- -251.5452385054409
+- -85.58574830889465
+- -175.86565293488013
+- -168.68157901172373
+- 43.27471001227153
+- -211.51235301490655
+- -33.882881166671396
+- 17.816896055177363
+- -220.08370985785794
+- -126.94814249553465
+- -123.36080588341422
+- -191.66925871785097
+- -217.41619524997745
+- -82.53148473527165
+- -119.01021633447931
+- -18.880832531609574
+- 94.810132809961
+- -126.10340115041222
+- -65.41712024089234
+- -242.27824026663114
+- -127.62421001785404
+- -109.29264926087524
+- 105.00869338854466
+- -264.95747004603106
+- -170.19316659517534
+- -62.51174523604564
+- -139.03783023029405
+- 7.091799579847809
+- -100.1137463217048
+- 40.52331999512637
+- -0.3977207066748197
+- -6.042863622712464
+- -10.462392479298
+- 82.40140712057627
+- -52.05978955014146
+- -59.374221005472805
+- 8.062674595752625
+- 48.37570132908054
+- 125.45986768578996
+- -167.0755789114162
+- 78.7851699212989
+- -135.53045791212642
+- -58.05486635241099
+- -56.274920510975655
+- -140.67880641684334
+- 8.52274740872368
+- 36.394675838985634
+- 74.04058190042416
+- -40.43735106510951
+- -97.63861742488488
+- -228.5431282058966
+- 25.72368610182627
+- 21.425417388147995
+- -109.71892342624676
+- -72.98589345917944
+- -33.15774660044493
+- -233.72394361694236
+- 10.062230522679108
+- -127.33721300361697
+y:
+- -318.33839265125806
+- -503.8732576178618
+- -177.77775964139093
+- -6.7492803260761995
+- -242.03813817935685
+- -77.71979481050029
+- -220.73421600647907
+- -90.27435824161067
+- -153.0275889294634
+- -200.48419383845444
+- -156.471884436833
+- -103.21870798204141
+- -222.84791678035685
+- -303.96349284278716
+- -76.73068739780327
+- -31.624809568016307
+- -153.82357481298533
+- -234.22262159091488
+- 86.72169514942202
+- -256.36840286665006
+- -309.9685665351012
+- -63.1932884859172
+- -322.6519017052047
+- -243.47703866255534
+- -158.51341919021516
+- -275.6868839717208
+- -104.84781924329988
+- 55.86783354402407
+- -148.4564593378266
+- -295.1219553718082
+- -276.5514759444907
+- -9.19806696670726
+- -226.3586168241507
+- -135.77779938569918
+- -124.7290399124978
+- 40.97147405813215
+- 47.9572155566829
+- -209.71160766724336
+- -206.8022387130818
+- -346.19115925055246
+- -194.68448470169963
+- -232.38547362052498
+- -46.81884319670084
+- -284.83539234521766
+- -291.55024983848756
+- -215.3595844343681
+- -469.2967940360148
+- -230.07806535442114
+- 15.756084988812708
+- -183.70819458902477
+- -115.7163594887839
+- -215.5334693249218
+- -280.6752838173983
+- -221.57247204002815
+- -300.5664169621706
+- -79.32812332194474
+- -179.40836211563925
+- -168.79969478638898
+- -276.29289348584774
+- -46.37362441829391
+- -231.29505409608572
+- -70.18085780324427
+- -252.23939541471805
+- -19.12968984192443
+- -296.13422955076334
+- -100.16294002462993
+- -342.90714896056124
+- -215.61769241860063
+- -296.75155474429926
+- -152.6591211649526
+- -188.693765231233
+- -205.91018839738655
+- -416.03035939248605
+- -199.40149048359942
+- -126.83154515228814
+- -222.1926200930844
+- -309.4416518904729
+- -254.09193980366175
+- -0.5418580888411685
+- -155.89526333672188
+- -9.890265411787345
+- -193.7291905240682
+- -263.22603077790814
+- -290.4470899118978
+- 42.80107774241674
+- 16.546189863000382
+- 87.11142726739735
+- -266.371727169003
+- -167.52530632906425
+- -194.22729876901454
+- -119.62496574880039
+- -251.5452385054409
+- -85.58574830889465
+- -175.86565293488013
+- -168.68157901172373
+- 43.27471001227153
+- -211.51235301490655
+- -33.882881166671396
+- 17.816896055177363
+- -220.08370985785794
+- -126.94814249553465
+- -123.36080588341422
+- -191.66925871785097
+- -217.41619524997745
+- -82.53148473527165
+- -119.01021633447931
+- -18.880832531609574
+- 94.810132809961
+- -126.10340115041222
+- -65.41712024089234
+- -242.27824026663114
+- -127.62421001785404
+- -109.29264926087524
+- 105.00869338854466
+- -264.95747004603106
+- -170.19316659517534
+- -62.51174523604564
+- -139.03783023029405
+- 7.091799579847809
+- -100.1137463217048
+- 40.52331999512637
+- -0.3977207066748197
+- -6.042863622712464
+- -10.462392479298
+- 82.40140712057627
+- -52.05978955014146
+- -59.374221005472805
+- 8.062674595752625
+- 48.37570132908054
+- 125.45986768578996
+- -167.0755789114162
+- 78.7851699212989
+- -135.53045791212642
+- -58.05486635241099
+- -56.274920510975655
+- -140.67880641684334
+- 8.52274740872368
+- 36.394675838985634
+- 74.04058190042416
+- -40.43735106510951
+- -97.63861742488488
+- -228.5431282058966
+- 25.72368610182627
+- 21.425417388147995
+- -109.71892342624676
+- -72.98589345917944
+- -33.15774660044493
+- -233.72394361694236
+- 10.062230522679108
+- -127.33721300361697
+z:
+- 181.0863154483668
+- -31.17771450941217
+- -131.0439545278052
+- -86.90343977743969
+- 139.16394124520718
+- -37.32702121025883
+- -146.42539146983646
+- -62.26586514105592
+- 208.69645305438297
+- -251.368482905382
+- -186.97390109596176
+- 175.69142574945545
+- -107.32570635749657
+- 56.249565649821996
+- -311.4308506465853
+- -86.25609939656523
+- 12.339637683177527
+- -227.4292833033472
+- 42.942002678669795
+- -153.39218203343864
+- 119.12930254316275
+- -10.107340020103656
+- 79.62407823144244
+- -156.02869946276996
+- -26.206848777533665
+- -50.43475326943761
+- 85.94651594284883
+- 106.48464201450267
+- 100.2685013112651
+- -121.87667475466651
+- -86.38750249227874
+- -218.0532151541864
+- 15.37185934879268
+- -70.02864771017883
+- -101.09853560534076
+- -92.46275643231517
+- 46.297066297691934
+- -14.555285738533223
+- 129.5173471667389
+- -46.180364966683705
+- 65.4830928379543
+- -58.17953821547616
+- 84.28074434004336
+- -155.1641325587134
+- -180.3359495774464
+- 88.00479798175125
+- -17.850090482332654
+- 15.372150066234237
+- -27.647258581804355
+- -192.91010730104782
+- -81.20236428206617
+- 6.467237778465594
+- -78.40533655530155
+- 172.22679093564656
+- -228.36796902747824
+- -225.6689941838593
+- -206.28994187021976
+- 131.48524184946956
+- -57.81804179017013
+- -147.16545572823645
+- -240.67299674890594
+- -116.14786022256757
+- -201.58749895550676
+- -308.07009408010566
+- -381.51470627309277
+- -137.39140431935613
+- -10.63189954057172
+- -118.493935903444
+- -164.1140641970783
+- -160.18465262262697
+- -117.57657250910667
+- -43.836529664241546
+- -112.71822942406038
+- -145.30820262460696
+- -90.81939319176844
+- -311.7923659232393
+- -175.3190301125731
+- -163.64011238983196
+- -247.39627434657896
+- -281.6861325550744
+- -175.08297411333973
+- -211.5739326575852
+- -10.988979072514425
+- -63.21629300640135
+- -202.53635529860406
+- -79.6957224677569
+- -177.162143434471
+- -92.84307934741052
+- -49.25448884632998
+- -44.780500209675225
+- -306.68069860558774
+- 4.39395972183376
+- -16.597486223665378
+- -206.44476503921166
+- -57.53302317731632
+- 105.23684385084307
+- -90.6662697103156
+- -60.903732325556234
+- -89.5421941328788
+- -220.0909473496956
+- -80.48441922343073
+- -123.94102877280065
+- -82.24771338364724
+- 25.78095332212615
+- -18.137890561524372
+- -220.67832950606126
+- -80.3513128301996
+- -116.30708725243996
+- -376.2694198773608
+- -90.08644334520498
+- -226.72579589042797
+- -41.920975507066316
+- -123.01540567914972
+- -10.086676126013069
+- 146.20193939546007
+- -7.818257625137203
+- -220.36826843029272
+- -48.97386136378574
+- -100.66630784546683
+- -247.64603285451435
+- -13.685647568036465
+- -19.602285641253204
+- -156.06514791914643
+- -82.58806190097447
+- -250.156508003731
+- -179.87376837556948
+- -18.67009670602568
+- 118.57247270205394
+- 11.177827711965824
+- -200.67282682077555
+- 26.279149574743716
+- -49.32413067128099
+- -149.32913362268766
+- 117.21131129001682
+- -81.87955805358436
+- -147.37638486925798
+- -180.83020297576527
+- 33.182852456140495
+- -8.764771582780666
+- 29.41976078723741
+- -169.09783435506606
+- -243.5270112908626
+- -45.83266948178252
+- -76.83693008095817
+- -84.60599041816408
+- -143.657215226681
+- -161.8069368693742
+- -151.858761996727
+- -161.10172004637832
+- -135.60897240328893
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
new file mode 100644
index 000000000..c12337b06
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -307.0050762731142
+- -481.078226408732
+- -167.35765769210175
+- 3.2155857868734596
+- -271.1441857672259
+- -65.71942771314525
+- -298.20344582051285
+- -56.839033663601775
+- -108.79519844570966
+- -230.15275347697178
+- -116.11665259730907
+- -40.1020956889466
+- -232.259229024552
+- -310.3034303967311
+- -64.01604336364709
+- -6.871892053932016
+- -153.22202647516366
+- -231.04322544639018
+- 74.44877889082618
+- -284.214278295163
+- -361.9818510757614
+- -79.40574675282046
+- -364.6297374793298
+- -170.77036590217787
+- -97.41558710663041
+- -311.4691059677862
+- -56.73355477193985
+- -49.98251311430054
+- -67.40310998563878
+- -342.94082838674996
+- -309.2813674649258
+- -1.8255852913501187
+- -199.10081582866306
+- -107.45000108810967
+- -113.4256835187521
+- 52.922554582910614
+- 61.83451450204298
+- -210.3273655230436
+- -151.56655048276252
+- -371.60746274998536
+- -201.7018308944319
+- -240.17038850383202
+- -5.02027956779233
+- -312.0700042595691
+- -307.47880192647176
+- -207.10639493970586
+- -504.4596940836522
+- -178.0663511545844
+- 108.1741087313954
+- -230.75292320355743
+- -99.26999716801097
+- -379.1967698869467
+- -268.17451088412747
+- -180.49119043160024
+- -319.1171815112415
+- -123.57804031969677
+- -217.72584276080778
+- -169.0552370338435
+- -295.73093381122334
+- -6.1624140590478635
+- -288.5456167792755
+- 8.128368780179871
+- -273.4409600387709
+- -34.38967766878111
+- -296.19214044239845
+- -57.092905112259864
+- -453.772881518744
+- -268.29410838503196
+- -225.46523867454502
+- -156.1360371937783
+- -254.85169701862418
+- -213.79962660208486
+- -420.7133824901222
+- -153.41659041261036
+- -58.20115031531576
+- -174.32911654101838
+- -319.93632296786006
+- -277.2669761654481
+- 10.358418007309556
+- -228.58808121230973
+- -40.498796490170996
+- -183.78539945929293
+- -253.07966794632128
+- -267.91199415308455
+- 25.292479767968373
+- -40.55947065570325
+- 143.37142124642537
+- -307.9697948735147
+- -204.15879357109165
+- -196.19898274338985
+- -129.46743231083366
+- -183.52437037382262
+- -62.14512122772048
+- -162.5147403785735
+- -188.1042411180911
+- 57.69519127285521
+- -296.0368433425237
+- -35.76061843155276
+- -42.10739776259322
+- -185.2120503709206
+- -141.76104241334673
+- -92.19931773917446
+- -250.46694930472722
+- -258.90185824926914
+- -70.87022098637703
+- -80.03237743157227
+- 36.648975295721776
+- 141.91109829680673
+- -79.64190548182395
+- 16.837115098236232
+- -291.0429952038719
+- -121.18311505521861
+- -104.97029871240828
+- 114.87662910759532
+- -286.4749590409721
+- -139.17496546848045
+- -10.191441705467936
+- -91.36563686432785
+- 33.83164333840472
+- -103.6053919809268
+- 100.73893860726871
+- -4.228885833076896
+- 0.29518205436492817
+- -32.384367296407866
+- 138.5644133058073
+- -75.63515950163199
+- 10.364619026260018
+- 20.980765426247974
+- 38.57937515486196
+- 167.88603431318387
+- -222.7287631138857
+- 176.68860940434107
+- -173.79227333486338
+- -109.48595595747148
+- 19.34516917949695
+- -82.38344597164956
+- 39.61516695813081
+- 53.45483727835639
+- 52.8044146763662
+- -52.30604369572495
+- -80.70848404692364
+- -201.04026922813614
+- -2.6918820856202066
+- 17.067603376143943
+- -87.01859880298362
+- -51.461962229891306
+- -64.43473942423898
+- -275.7793739824045
+- 22.21096374603982
+- -193.56704806257392
+y:
+- -307.0050762731142
+- -481.078226408732
+- -167.35765769210175
+- 3.2155857868734596
+- -271.1441857672259
+- -65.71942771314525
+- -298.20344582051285
+- -56.839033663601775
+- -108.79519844570966
+- -230.15275347697178
+- -116.11665259730907
+- -40.1020956889466
+- -232.259229024552
+- -310.3034303967311
+- -64.01604336364709
+- -6.871892053932016
+- -153.22202647516366
+- -231.04322544639018
+- 74.44877889082618
+- -284.214278295163
+- -361.9818510757614
+- -79.40574675282046
+- -364.6297374793298
+- -170.77036590217787
+- -97.41558710663041
+- -311.4691059677862
+- -56.73355477193985
+- -49.98251311430054
+- -67.40310998563878
+- -342.94082838674996
+- -309.2813674649258
+- -1.8255852913501187
+- -199.10081582866306
+- -107.45000108810967
+- -113.4256835187521
+- 52.922554582910614
+- 61.83451450204298
+- -210.3273655230436
+- -151.56655048276252
+- -371.60746274998536
+- -201.7018308944319
+- -240.17038850383202
+- -5.02027956779233
+- -312.0700042595691
+- -307.47880192647176
+- -207.10639493970586
+- -504.4596940836522
+- -178.0663511545844
+- 108.1741087313954
+- -230.75292320355743
+- -99.26999716801097
+- -379.1967698869467
+- -268.17451088412747
+- -180.49119043160024
+- -319.1171815112415
+- -123.57804031969677
+- -217.72584276080778
+- -169.0552370338435
+- -295.73093381122334
+- -6.1624140590478635
+- -288.5456167792755
+- 8.128368780179871
+- -273.4409600387709
+- -34.38967766878111
+- -296.19214044239845
+- -57.092905112259864
+- -453.772881518744
+- -268.29410838503196
+- -225.46523867454502
+- -156.1360371937783
+- -254.85169701862418
+- -213.79962660208486
+- -420.7133824901222
+- -153.41659041261036
+- -58.20115031531576
+- -174.32911654101838
+- -319.93632296786006
+- -277.2669761654481
+- 10.358418007309556
+- -228.58808121230973
+- -40.498796490170996
+- -183.78539945929293
+- -253.07966794632128
+- -267.91199415308455
+- 25.292479767968373
+- -40.55947065570325
+- 143.37142124642537
+- -307.9697948735147
+- -204.15879357109165
+- -196.19898274338985
+- -129.46743231083366
+- -183.52437037382262
+- -62.14512122772048
+- -162.5147403785735
+- -188.1042411180911
+- 57.69519127285521
+- -296.0368433425237
+- -35.76061843155276
+- -42.10739776259322
+- -185.2120503709206
+- -141.76104241334673
+- -92.19931773917446
+- -250.46694930472722
+- -258.90185824926914
+- -70.87022098637703
+- -80.03237743157227
+- 36.648975295721776
+- 141.91109829680673
+- -79.64190548182395
+- 16.837115098236232
+- -291.0429952038719
+- -121.18311505521861
+- -104.97029871240828
+- 114.87662910759532
+- -286.4749590409721
+- -139.17496546848045
+- -10.191441705467936
+- -91.36563686432785
+- 33.83164333840472
+- -103.6053919809268
+- 100.73893860726871
+- -4.228885833076896
+- 0.29518205436492817
+- -32.384367296407866
+- 138.5644133058073
+- -75.63515950163199
+- 10.364619026260018
+- 20.980765426247974
+- 38.57937515486196
+- 167.88603431318387
+- -222.7287631138857
+- 176.68860940434107
+- -173.79227333486338
+- -109.48595595747148
+- 19.34516917949695
+- -82.38344597164956
+- 39.61516695813081
+- 53.45483727835639
+- 52.8044146763662
+- -52.30604369572495
+- -80.70848404692364
+- -201.04026922813614
+- -2.6918820856202066
+- 17.067603376143943
+- -87.01859880298362
+- -51.461962229891306
+- -64.43473942423898
+- -275.7793739824045
+- 22.21096374603982
+- -193.56704806257392
+z:
+- 254.52608087703956
+- -31.460089338187732
+- -192.58073184894693
+- -118.52116549001673
+- 210.1737596156136
+- 36.78739348943514
+- -157.9024815236414
+- -116.11308392329033
+- 287.7843644908441
+- -250.69566068504813
+- -162.2856828167312
+- 252.064773772356
+- -152.16425981798878
+- 132.7459879913642
+- -409.98377371714093
+- -154.08563400540893
+- 112.03451444957001
+- -185.6484590597303
+- 80.54736096333752
+- -95.1442097426821
+- 160.07801628924997
+- 69.17803357017898
+- 77.52491351187382
+- -236.0127743143916
+- 56.169164878231655
+- -42.52977141410982
+- 123.84846531219658
+- 117.34049688631372
+- 158.4614707975258
+- -110.80063553388132
+- -51.751849761337894
+- -217.18757070277127
+- 29.294022225722266
+- -159.80189813545167
+- -98.0363360284864
+- -65.89005493794272
+- 97.23585021726144
+- 64.70535341404307
+- 122.88420470169062
+- 39.83731820265176
+- 80.76191426920725
+- -107.67937238334108
+- 32.73966581258587
+- -176.31679697677316
+- -157.05398798176645
+- 181.93695908987735
+- -22.204754687753276
+- 59.99996717282246
+- -43.62795843310611
+- -312.53381496997486
+- -39.741000135179796
+- 53.067107180690954
+- -94.02174504681692
+- 177.35460868059923
+- -278.39968211818746
+- -225.12024774102943
+- -261.18242012509285
+- 115.62981538281554
+- -98.77693755528435
+- -210.2718582996535
+- -272.01811798799883
+- -62.45265432971509
+- -253.15023775303436
+- -384.92067732976045
+- -475.3826677890834
+- -228.20692134710097
+- -96.13894507456048
+- -141.4995708372378
+- -134.28722392293884
+- -150.64608080511906
+- -127.56128538438503
+- -69.51462954026584
+- -165.96070501197542
+- -166.97446231475635
+- -105.01986203061864
+- -321.02750772665246
+- -179.74190590873235
+- -185.54048781133775
+- -283.0771623774315
+- -334.92854929352
+- -258.5036346594376
+- -340.2881399991007
+- 2.9251683017013326
+- -168.65125561864767
+- -274.7806613989737
+- -83.04249868201047
+- -180.92286540243515
+- -89.92432207277365
+- -11.627718736702299
+- -30.520520524489353
+- -308.2836685615376
+- 55.79159635947377
+- -1.028721057308561
+- -189.6408838060553
+- -75.39509171836379
+- 109.37174480760984
+- -47.151999749684684
+- -112.82172688972774
+- -122.46439859266991
+- -198.31238169600485
+- -110.08302317236765
+- -50.59160254068123
+- -79.08748078315834
+- 77.19380631825918
+- 14.248904365055418
+- -159.51798216420912
+- -56.11555257880312
+- -90.2440339055217
+- -392.1570105168502
+- -106.62345638777008
+- -180.5086141785024
+- -29.97062046444948
+- -117.84327291890007
+- -8.7540768117932
+- 129.46179350310734
+- 76.24881788567006
+- -183.12799672686822
+- -57.21196583403689
+- -48.789839744624985
+- -279.2667029477613
+- 12.943836342975453
+- 78.83995804778267
+- -126.25983712568782
+- -103.81640988851102
+- -183.7736218366929
+- -213.2531756682584
+- 50.39518284318662
+- 139.2192226443896
+- 67.49626762271218
+- -237.96811773997374
+- 25.08274372002822
+- -33.560313013774774
+- -173.01918177503993
+- 113.66472259892426
+- -121.97747942264799
+- -180.02930875136644
+- -170.19088534655762
+- 136.35987821597578
+- 96.6350544673526
+- 66.84156244822071
+- -195.28568574540293
+- -220.00818881027183
+- -52.78103015741021
+- -31.713914220009627
+- -49.72178214507511
+- -131.51961371834844
+- -96.90912203690274
+- -137.19949307427692
+- -164.01851373080234
+- -202.79022329301824
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
new file mode 100644
index 000000000..1b1eaa6fa
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -350.1756893430689
+- -541.839591849715
+- -168.09051010792922
+- 23.891080268019962
+- -190.48961933517865
+- -67.44798262750935
+- -283.8058731578255
+- -56.15096503224349
+- -101.2346887712504
+- -195.33894651640188
+- -87.71099758636797
+- 39.17429077313801
+- -218.88876034230438
+- -352.47526826165256
+- -15.556951705333246
+- -41.73090530661492
+- -188.87796866892165
+- -260.69546435839635
+- 41.921004001942755
+- -321.3496047921541
+- -340.9114314869833
+- -88.6149858528528
+- -336.9313792206388
+- -116.6713232366248
+- -79.01195649215286
+- -283.13525196591814
+- -98.29442399057879
+- -83.05468453334208
+- -89.30784504344854
+- -291.31917135721966
+- -286.2304713566369
+- 23.119800906054724
+- -203.4129006780318
+- -143.90917559464495
+- -96.44351771382031
+- 53.980492632643795
+- 103.0322664275742
+- -255.00922777323314
+- -154.46041207654082
+- -364.5926198488748
+- -198.6546844201885
+- -212.9173461870956
+- 59.658033602559534
+- -270.04984736677096
+- -336.35657260629785
+- -266.65686399693897
+- -514.2196806515818
+- -120.10722036554137
+- 113.52869021149299
+- -237.6369679704182
+- -74.26798003541333
+- -339.72200748643263
+- -235.0124746907095
+- -176.56605105155353
+- -278.31327634763636
+- -116.14672694815732
+- -222.6616976333995
+- -145.76263098777707
+- -372.19799397554095
+- -25.853912380006452
+- -330.4202124057533
+- 16.43185888956116
+- -254.91043143425807
+- 3.3308684759710268
+- -324.51006240626964
+- -96.86723694649756
+- -508.4929776644908
+- -259.10577651514296
+- -318.8078203008651
+- -139.98429087522803
+- -248.63840539191472
+- -202.26919987841785
+- -426.2186369681213
+- -169.06364530328938
+- -33.35515658672454
+- -164.09394972387616
+- -337.1130694168003
+- -282.0444988365109
+- 6.06943893903796
+- -166.1827223918531
+- -30.224050520413314
+- -218.74552179593502
+- -269.3423722562611
+- -325.0804145865783
+- 12.855214821346305
+- -31.462306979422557
+- 142.08182548397568
+- -318.3073344575577
+- -275.4660164735274
+- -172.2856633527697
+- -136.00869665393006
+- -180.58338113264952
+- -53.867654762685056
+- -193.69227086106378
+- -153.48472522766124
+- 70.83394962373872
+- -312.20137806872106
+- -5.436211416654895
+- -16.86409970640226
+- -269.1224943182721
+- -150.57572960315383
+- -39.45088777140532
+- -234.46137210174433
+- -264.94918101667207
+- -97.71585111996367
+- -117.27256437465216
+- 12.254605128927793
+- 211.6909142036703
+- -50.47304500828327
+- 52.96115143895034
+- -325.13465560598604
+- -112.16005062195325
+- -90.48471683489367
+- 183.12977894674228
+- -261.35354077163066
+- -118.52294406996586
+- 35.12940340171155
+- -133.25150347597543
+- -19.21068382378661
+- -103.94333131956081
+- 154.95791767035342
+- 53.28836431186244
+- 23.311612591066716
+- 13.98251101050205
+- 98.39063100089848
+- 5.091491197537112
+- 4.187730240374253
+- 66.2047418570116
+- 86.79675792688921
+- 167.03614718355487
+- -203.9496008273403
+- 183.23521880780507
+- -135.12354001160443
+- -85.60522160005215
+- 63.878143391082126
+- -120.94036593531315
+- 2.028043073945513
+- 36.18909245474933
+- 3.5450117587459555
+- -73.31230476231222
+- -95.08115674171863
+- -184.89239069984512
+- 42.04986153347611
+- 18.10411440538753
+- -68.9587453387462
+- -58.7607039169071
+- -91.50981697394722
+- -215.65242975397166
+- 82.904752101266
+- -238.97406849426307
+y:
+- -350.1756893430689
+- -541.839591849715
+- -168.09051010792922
+- 23.891080268019962
+- -190.48961933517865
+- -67.44798262750935
+- -283.8058731578255
+- -56.15096503224349
+- -101.2346887712504
+- -195.33894651640188
+- -87.71099758636797
+- 39.17429077313801
+- -218.88876034230438
+- -352.47526826165256
+- -15.556951705333246
+- -41.73090530661492
+- -188.87796866892165
+- -260.69546435839635
+- 41.921004001942755
+- -321.3496047921541
+- -340.9114314869833
+- -88.6149858528528
+- -336.9313792206388
+- -116.6713232366248
+- -79.01195649215286
+- -283.13525196591814
+- -98.29442399057879
+- -83.05468453334208
+- -89.30784504344854
+- -291.31917135721966
+- -286.2304713566369
+- 23.119800906054724
+- -203.4129006780318
+- -143.90917559464495
+- -96.44351771382031
+- 53.980492632643795
+- 103.0322664275742
+- -255.00922777323314
+- -154.46041207654082
+- -364.5926198488748
+- -198.6546844201885
+- -212.9173461870956
+- 59.658033602559534
+- -270.04984736677096
+- -336.35657260629785
+- -266.65686399693897
+- -514.2196806515818
+- -120.10722036554137
+- 113.52869021149299
+- -237.6369679704182
+- -74.26798003541333
+- -339.72200748643263
+- -235.0124746907095
+- -176.56605105155353
+- -278.31327634763636
+- -116.14672694815732
+- -222.6616976333995
+- -145.76263098777707
+- -372.19799397554095
+- -25.853912380006452
+- -330.4202124057533
+- 16.43185888956116
+- -254.91043143425807
+- 3.3308684759710268
+- -324.51006240626964
+- -96.86723694649756
+- -508.4929776644908
+- -259.10577651514296
+- -318.8078203008651
+- -139.98429087522803
+- -248.63840539191472
+- -202.26919987841785
+- -426.2186369681213
+- -169.06364530328938
+- -33.35515658672454
+- -164.09394972387616
+- -337.1130694168003
+- -282.0444988365109
+- 6.06943893903796
+- -166.1827223918531
+- -30.224050520413314
+- -218.74552179593502
+- -269.3423722562611
+- -325.0804145865783
+- 12.855214821346305
+- -31.462306979422557
+- 142.08182548397568
+- -318.3073344575577
+- -275.4660164735274
+- -172.2856633527697
+- -136.00869665393006
+- -180.58338113264952
+- -53.867654762685056
+- -193.69227086106378
+- -153.48472522766124
+- 70.83394962373872
+- -312.20137806872106
+- -5.436211416654895
+- -16.86409970640226
+- -269.1224943182721
+- -150.57572960315383
+- -39.45088777140532
+- -234.46137210174433
+- -264.94918101667207
+- -97.71585111996367
+- -117.27256437465216
+- 12.254605128927793
+- 211.6909142036703
+- -50.47304500828327
+- 52.96115143895034
+- -325.13465560598604
+- -112.16005062195325
+- -90.48471683489367
+- 183.12977894674228
+- -261.35354077163066
+- -118.52294406996586
+- 35.12940340171155
+- -133.25150347597543
+- -19.21068382378661
+- -103.94333131956081
+- 154.95791767035342
+- 53.28836431186244
+- 23.311612591066716
+- 13.98251101050205
+- 98.39063100089848
+- 5.091491197537112
+- 4.187730240374253
+- 66.2047418570116
+- 86.79675792688921
+- 167.03614718355487
+- -203.9496008273403
+- 183.23521880780507
+- -135.12354001160443
+- -85.60522160005215
+- 63.878143391082126
+- -120.94036593531315
+- 2.028043073945513
+- 36.18909245474933
+- 3.5450117587459555
+- -73.31230476231222
+- -95.08115674171863
+- -184.89239069984512
+- 42.04986153347611
+- 18.10411440538753
+- -68.9587453387462
+- -58.7607039169071
+- -91.50981697394722
+- -215.65242975397166
+- 82.904752101266
+- -238.97406849426307
+z:
+- 257.61172539326617
+- -6.618130669387091
+- -206.00715019186396
+- -86.30976067908074
+- 183.94514580380573
+- 59.280825073240536
+- -186.11338025477139
+- -116.47715113741444
+- 287.51960729933734
+- -287.32377562080916
+- -131.33531487385537
+- 200.29634539732655
+- -190.83469360027274
+- 141.18011313450324
+- -425.6957359539522
+- -137.4351307293813
+- 84.2334387295801
+- -167.5500539151613
+- 82.03453459321035
+- -113.74144685312307
+- 185.94863632159903
+- 66.8247118350421
+- 94.78550550055948
+- -301.22319808793543
+- 45.06039341490167
+- -40.52042981744621
+- 56.92518720546565
+- 119.21807228089337
+- 202.48608879948932
+- -71.06337143901112
+- -81.1877093346863
+- -233.11684463745996
+- 12.399466726228109
+- -190.0882035555663
+- -127.1838737973734
+- -43.436042328212025
+- 78.97443709887077
+- 47.569203023743306
+- 104.55812227817013
+- 74.13873305090655
+- 52.23069966386027
+- -116.08259030101665
+- -18.09364660434971
+- -179.44847295083076
+- -209.74027182688067
+- 148.19708758709768
+- -39.33733183565245
+- 29.9935063917469
+- -104.23570486350854
+- -352.06341012938367
+- -6.635367260097158
+- 96.68614567574384
+- -72.2525547152538
+- 229.18525391302035
+- -219.35401435771962
+- -261.02973563045543
+- -238.30069050160995
+- 112.74859045888273
+- -118.74227954945005
+- -199.61277005395687
+- -304.73627419671664
+- -62.06110537692866
+- -197.7609297870173
+- -376.0564558353367
+- -515.140300110365
+- -258.0924715445956
+- -21.707570024800344
+- -173.909407836862
+- -103.80808560229377
+- -84.68307037830921
+- -191.2990090644455
+- -80.01627779115191
+- -157.55735202680265
+- -152.57390711919544
+- -182.87754184649867
+- -385.46930530861545
+- -177.4443837439734
+- -216.78276439827755
+- -280.9291853700707
+- -382.28182496758507
+- -267.3550572789101
+- -374.06938964318357
+- -10.187531902064277
+- -184.4263283234975
+- -308.5049554241409
+- -139.70029390370794
+- -256.9684816712291
+- -69.53075309959198
+- -41.28763965694097
+- -48.24368546249476
+- -292.3879930896808
+- 58.68474300197547
+- -35.70933708415813
+- -158.8934500668035
+- -60.39367654698243
+- 149.72891397183068
+- -136.238393107494
+- -94.29894742097432
+- -142.03863797248454
+- -197.85287296329656
+- -53.18282711678025
+- -48.873060782231356
+- -109.02214835056309
+- 79.86260442253865
+- 9.203603753015521
+- -162.70806374119755
+- -56.14554446151242
+- -44.009174944212475
+- -406.6073343830843
+- -104.38633207583894
+- -189.38781565670146
+- 22.646049157775945
+- -81.84696125013302
+- -8.64475300616172
+- 133.09252108208128
+- 146.51257652526837
+- -228.07723192799574
+- 5.988954533203216
+- -13.25364631535978
+- -272.79141272086486
+- 72.83925693399259
+- 90.30157311917212
+- -68.79915364910022
+- -145.3687115803004
+- -148.97947822743163
+- -226.74252325912974
+- 59.014515013192614
+- 142.69442446848947
+- 89.77504479152044
+- -228.46076257438364
+- 22.937464184836305
+- -55.01263843325899
+- -137.17279266455355
+- 138.08816457284763
+- -73.53670893966546
+- -210.0748941243596
+- -150.71463118578984
+- 126.05819899711024
+- 145.5305672018292
+- 77.74374513197803
+- -216.53309268844833
+- -247.89898518428765
+- -16.77660653440476
+- -33.81717836339604
+- -59.817905675544196
+- -79.9069061760625
+- -97.95723595746524
+- -144.99990784662083
+- -164.950361248946
+- -208.17585911641996
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
new file mode 100644
index 000000000..2b6dc75d7
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -354.37547672825644
+- -578.4076892945358
+- -107.7214176118289
+- -5.583194356537376
+- -174.8024529666291
+- -49.71176993081215
+- -235.2852709481214
+- -80.04293719508337
+- -40.22294207313038
+- -183.2105895721838
+- -73.28299934813013
+- 21.72680263310102
+- -198.4519648778521
+- -357.23691456093695
+- -12.366725869355843
+- 23.833973892903572
+- -196.49991574516423
+- -323.4728961928933
+- 56.60422744480935
+- -318.5150359750603
+- -264.4038404647167
+- -56.32823405461204
+- -306.3199961193478
+- -85.69903013083811
+- -63.39444878558671
+- -344.68250556952955
+- -125.3937652593784
+- -91.79964988527658
+- -61.16933951887898
+- -279.47332050663397
+- -316.4996515092117
+- -3.4622872707177628
+- -192.61775644693725
+- -139.0374595204804
+- -88.62035793625397
+- -1.328738456243122
+- 178.80725728588084
+- -299.70469920872335
+- -71.2304985986049
+- -404.5156670814688
+- -235.88938025975474
+- -161.95859759241196
+- 89.14052082237625
+- -305.37184796671727
+- -408.51417713480623
+- -294.075466208455
+- -559.6037961107711
+- -90.98429815623903
+- 226.06762923822137
+- -293.07833276547865
+- -42.66124934132576
+- -334.6843577841629
+- -233.96529376377381
+- -216.0911324687805
+- -331.0621260970212
+- -88.41873611381575
+- -247.46253796639644
+- -156.85694547149083
+- -379.0250466471387
+- 9.319291455900977
+- -311.8736746536096
+- 45.491505165810246
+- -207.71450965334355
+- -31.694731733508842
+- -336.1125221668325
+- -61.433142613975996
+- -577.0441442077778
+- -303.5310673802776
+- -352.01529804845586
+- -71.49213445848079
+- -206.50260697053224
+- -151.93994188874612
+- -515.6518855638963
+- -150.28461386623766
+- 10.603840189052226
+- -172.21810964866498
+- -351.5932887407071
+- -362.65366390513134
+- 25.11936106635793
+- -253.32021353565395
+- 24.051487321846913
+- -259.6102556958661
+- -308.84420582985393
+- -375.15089787666915
+- -28.30428619760369
+- -33.419786400218804
+- 217.68244288493824
+- -332.1423289064766
+- -334.22536159179964
+- -163.0294109054836
+- -130.80526578867776
+- -107.7354759742875
+- -91.4099529250846
+- -175.55952879483604
+- -164.81804504720563
+- 61.859148898200615
+- -309.7945367460495
+- -37.11275024272886
+- 3.0380902027438337
+- -290.4791670863065
+- -115.79720539484173
+- -52.35668217371858
+- -199.29036314591102
+- -255.02683975888831
+- -87.18777704131581
+- -47.189115460126914
+- 10.421528098201627
+- 204.27357257872373
+- -124.498080153961
+- 86.79126426810572
+- -305.1122133509904
+- -92.23333307004609
+- -84.66773944957221
+- 212.12045530161723
+- -274.9300636493045
+- -133.6688509480514
+- 111.91825767203322
+- -111.8149729122468
+- -9.80459749015382
+- -86.3350360735338
+- 207.65127689722652
+- 46.987818022734295
+- -45.78997057332551
+- 40.23318136554408
+- 137.78460311183883
+- 6.026689042247037
+- 44.599274553512274
+- 56.4244623184312
+- 90.79637174151507
+- 187.80050347548678
+- -241.5699612092039
+- 161.43853717767013
+- -122.96982180302591
+- -79.44977832618412
+- 157.05035144082998
+- -127.54906772046294
+- 34.87633076652711
+- 103.8954481247204
+- 17.011849578170438
+- -90.39381045710203
+- -95.52756259423225
+- -228.93535283985594
+- 55.35461895380736
+- 98.68583828345103
+- -40.729841980180694
+- -85.12237752832773
+- -77.01310301831063
+- -281.2572901636379
+- 92.09804458954045
+- -240.22038057546192
+y:
+- -354.37547672825644
+- -578.4076892945358
+- -107.7214176118289
+- -5.583194356537376
+- -174.8024529666291
+- -49.71176993081215
+- -235.2852709481214
+- -80.04293719508337
+- -40.22294207313038
+- -183.2105895721838
+- -73.28299934813013
+- 21.72680263310102
+- -198.4519648778521
+- -357.23691456093695
+- -12.366725869355843
+- 23.833973892903572
+- -196.49991574516423
+- -323.4728961928933
+- 56.60422744480935
+- -318.5150359750603
+- -264.4038404647167
+- -56.32823405461204
+- -306.3199961193478
+- -85.69903013083811
+- -63.39444878558671
+- -344.68250556952955
+- -125.3937652593784
+- -91.79964988527658
+- -61.16933951887898
+- -279.47332050663397
+- -316.4996515092117
+- -3.4622872707177628
+- -192.61775644693725
+- -139.0374595204804
+- -88.62035793625397
+- -1.328738456243122
+- 178.80725728588084
+- -299.70469920872335
+- -71.2304985986049
+- -404.5156670814688
+- -235.88938025975474
+- -161.95859759241196
+- 89.14052082237625
+- -305.37184796671727
+- -408.51417713480623
+- -294.075466208455
+- -559.6037961107711
+- -90.98429815623903
+- 226.06762923822137
+- -293.07833276547865
+- -42.66124934132576
+- -334.6843577841629
+- -233.96529376377381
+- -216.0911324687805
+- -331.0621260970212
+- -88.41873611381575
+- -247.46253796639644
+- -156.85694547149083
+- -379.0250466471387
+- 9.319291455900977
+- -311.8736746536096
+- 45.491505165810246
+- -207.71450965334355
+- -31.694731733508842
+- -336.1125221668325
+- -61.433142613975996
+- -577.0441442077778
+- -303.5310673802776
+- -352.01529804845586
+- -71.49213445848079
+- -206.50260697053224
+- -151.93994188874612
+- -515.6518855638963
+- -150.28461386623766
+- 10.603840189052226
+- -172.21810964866498
+- -351.5932887407071
+- -362.65366390513134
+- 25.11936106635793
+- -253.32021353565395
+- 24.051487321846913
+- -259.6102556958661
+- -308.84420582985393
+- -375.15089787666915
+- -28.30428619760369
+- -33.419786400218804
+- 217.68244288493824
+- -332.1423289064766
+- -334.22536159179964
+- -163.0294109054836
+- -130.80526578867776
+- -107.7354759742875
+- -91.4099529250846
+- -175.55952879483604
+- -164.81804504720563
+- 61.859148898200615
+- -309.7945367460495
+- -37.11275024272886
+- 3.0380902027438337
+- -290.4791670863065
+- -115.79720539484173
+- -52.35668217371858
+- -199.29036314591102
+- -255.02683975888831
+- -87.18777704131581
+- -47.189115460126914
+- 10.421528098201627
+- 204.27357257872373
+- -124.498080153961
+- 86.79126426810572
+- -305.1122133509904
+- -92.23333307004609
+- -84.66773944957221
+- 212.12045530161723
+- -274.9300636493045
+- -133.6688509480514
+- 111.91825767203322
+- -111.8149729122468
+- -9.80459749015382
+- -86.3350360735338
+- 207.65127689722652
+- 46.987818022734295
+- -45.78997057332551
+- 40.23318136554408
+- 137.78460311183883
+- 6.026689042247037
+- 44.599274553512274
+- 56.4244623184312
+- 90.79637174151507
+- 187.80050347548678
+- -241.5699612092039
+- 161.43853717767013
+- -122.96982180302591
+- -79.44977832618412
+- 157.05035144082998
+- -127.54906772046294
+- 34.87633076652711
+- 103.8954481247204
+- 17.011849578170438
+- -90.39381045710203
+- -95.52756259423225
+- -228.93535283985594
+- 55.35461895380736
+- 98.68583828345103
+- -40.729841980180694
+- -85.12237752832773
+- -77.01310301831063
+- -281.2572901636379
+- 92.09804458954045
+- -240.22038057546192
+z:
+- 293.92236286138564
+- 19.241452133628222
+- -178.41933038279544
+- -147.0074323909161
+- 192.94005499915357
+- 33.77595909683745
+- -161.44377017159454
+- -111.99113759130483
+- 267.74360394463275
+- -326.5374000425544
+- -69.30458356048919
+- 164.4084883508337
+- -220.27140830867683
+- 176.81453608793652
+- -434.15062967005457
+- -188.7408733123947
+- 85.69818502704535
+- -158.89138642636277
+- 50.20887652377516
+- -160.8305257642651
+- 182.73354476031733
+- 90.39211452153127
+- 126.39133212594713
+- -321.5323137580511
+- 64.61567481188047
+- -52.81716295547091
+- 105.79934092625282
+- 112.09310582523618
+- 224.0250325659121
+- -129.9806706961594
+- -70.56853529090154
+- -275.73295097316606
+- 31.232613884774455
+- -216.1477444370246
+- -69.53333636704727
+- -23.23087843940836
+- 61.49921720803479
+- 66.2511171803064
+- 76.43369675431458
+- 122.80839411993087
+- 78.008525125722
+- -97.93405791579586
+- -37.48326993315027
+- -155.07042759537129
+- -215.6980838717558
+- 156.93235852981812
+- -38.9553098237568
+- 64.34348593444682
+- -69.93608871721042
+- -406.93438179643607
+- 1.739395361893391
+- 93.70806390977725
+- -64.70249343408463
+- 271.45546365247975
+- -227.93573522432246
+- -282.05929950281734
+- -326.1545440373919
+- 141.45526175457692
+- -105.76836588318919
+- -230.11495205568335
+- -329.23423347911614
+- -45.71005031607355
+- -202.25994672806672
+- -359.49877791738425
+- -551.7278452351626
+- -301.07226339518064
+- -63.5150242100091
+- -148.67505599039512
+- -121.46053304970646
+- -93.37093156057227
+- -195.40151517449308
+- -79.92444602019467
+- -168.4095127647589
+- -148.11530917097863
+- -219.82234677078625
+- -436.2791163720004
+- -195.46775918565436
+- -259.64651580465033
+- -295.84083527656486
+- -411.3334310705196
+- -300.56886364344155
+- -408.76562128605383
+- -35.97498119708834
+- -124.35957334358373
+- -314.7502413312763
+- -166.90451719918136
+- -247.16628211001097
+- -115.38659431426734
+- -29.838930937522342
+- -33.27036950805728
+- -342.6852970289899
+- 58.049657180501754
+- -105.16251169888999
+- -189.99987624016552
+- -80.9738437935302
+- 159.1326775311695
+- -166.40754333444988
+- -71.4460796821466
+- -109.32677736714189
+- -240.19786312624714
+- -91.3264776290697
+- -34.51395036269785
+- -90.27194293651411
+- 84.26609542156059
+- 47.42899256617139
+- -113.03090691352375
+- -56.63492264635932
+- -45.9412345476945
+- -431.3194457594861
+- -106.43355072795664
+- -187.38528258445865
+- 66.75572114259693
+- -66.36411824095501
+- 42.22835793425958
+- 177.73845023522196
+- 171.3302268946976
+- -228.17141701604191
+- 27.260792680540664
+- 5.889945909417749
+- -285.89941821763455
+- 152.51764436502387
+- 158.34210364178855
+- 51.86098635830103
+- -105.06659889602022
+- -144.59533257551183
+- -199.7905570224701
+- 42.73820513063766
+- 214.89392708348026
+- 138.47767815649885
+- -230.8829240411559
+- 57.02100988993763
+- -31.750840864820702
+- -124.57036261647724
+- 197.56676054716746
+- -35.77568324466903
+- -217.23830600410275
+- -164.07271513415145
+- 185.6209126476897
+- 133.54127708571707
+- 59.302865225121735
+- -218.96327874462574
+- -271.1150876277143
+- -4.577854877414626
+- -0.872197402222953
+- -97.60415567801225
+- -26.193253475528515
+- -87.42289444036297
+- -183.92027475378654
+- -177.78294904663525
+- -194.0016286625082
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
new file mode 100644
index 000000000..ac3b7c76d
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -328.7146602458411
+- -607.8480109459788
+- -70.49741332152209
+- 3.796692637047578
+- -162.95954309910817
+- -19.33972914626468
+- -235.09499521014578
+- -107.81384573412645
+- -31.53291462742378
+- -212.14886603050897
+- -89.46516739532265
+- 19.667982651940264
+- -216.33514516843985
+- -323.94021603169455
+- 3.7495919998539446
+- -18.021739092082814
+- -178.17970317466126
+- -346.25709247409145
+- 54.58157208827058
+- -367.15453044986333
+- -268.56541940327196
+- -81.23299020243925
+- -309.8770966018138
+- -100.67991512115881
+- -67.07968197377089
+- -347.2361920567107
+- -158.80898708863495
+- -104.01739402591244
+- -49.383573776489996
+- -298.4424163083782
+- -284.59719829638186
+- 1.9523807141375829
+- -228.8291871431349
+- -164.38882205876453
+- -56.82795956826953
+- 29.181611857038934
+- 197.59244963556043
+- -291.65009486814546
+- -73.96025052809098
+- -413.24345176232146
+- -289.00049014132344
+- -138.12231990311398
+- 122.85209320325096
+- -292.21133060332454
+- -371.8834044482743
+- -292.7279115117123
+- -584.8788687697738
+- -95.33440230176042
+- 239.5339774386685
+- -265.12892232434035
+- -53.34571751364334
+- -319.31875247104443
+- -226.3464861485659
+- -227.7066292952969
+- -322.42046471465534
+- -66.77461133038851
+- -247.64260359977254
+- -128.70180326118776
+- -371.86119903327614
+- 25.939839889457698
+- -303.22971631311106
+- 68.73563774325528
+- -220.62010394260605
+- 1.913655145979548
+- -321.05786511717696
+- -7.480576699947837
+- -562.6168351641293
+- -272.59111196344
+- -352.89209393639175
+- -63.22199637626166
+- -190.819897528042
+- -171.66970452269976
+- -542.3756048762308
+- -147.5045699695532
+- -32.951634458915514
+- -178.31868993964395
+- -370.4598127663352
+- -382.7765759441242
+- 70.55718440121618
+- -275.84240189804007
+- 24.52055475154436
+- -267.85699106997123
+- -362.6936740571793
+- -394.94518637379
+- -42.62824006660958
+- -37.4206296874913
+- 228.65827424080354
+- -368.34337229210706
+- -335.9848146528929
+- -185.57919006723577
+- -182.8323636774351
+- -148.05736988839385
+- -97.9632827112206
+- -172.58472121166204
+- -193.5201220514574
+- 52.94458737541407
+- -321.57488746239267
+- -62.46443325224946
+- 31.459204508390602
+- -320.34494746894666
+- -133.35752677467624
+- -33.91251403894966
+- -212.1959457043817
+- -257.8482070032987
+- -123.65941355538556
+- -46.78896318283864
+- 9.432068300432947
+- 195.92446684229847
+- -92.47988107673412
+- 88.53287931482706
+- -282.7293574505644
+- -56.04024780508934
+- -82.88216300356626
+- 256.79684689104766
+- -260.74896707781465
+- -120.52637661802831
+- 116.14986091913589
+- -66.23371973712261
+- 15.895059395863738
+- -103.5226695817851
+- 210.70305548700676
+- 75.5169908210367
+- -37.984599501492156
+- 43.13787154468077
+- 129.34416144787176
+- 17.434899592911798
+- 56.42418539420781
+- 40.27183357779729
+- 98.78430657831635
+- 215.67691442422276
+- -252.7746044156219
+- 142.22049019547737
+- -123.82476100073752
+- -76.28157211598544
+- 176.92621790810435
+- -137.5335650117231
+- 53.6554360276977
+- 132.97662711499208
+- 40.88040400506932
+- -91.68926179011021
+- -117.31681032188678
+- -222.9524498197763
+- 46.32122638601927
+- 99.43427727081166
+- 19.197643617274366
+- -62.516626238707644
+- -85.33848766172152
+- -292.1531296078384
+- 126.56353108153971
+- -246.1397928436873
+y:
+- -328.7146602458411
+- -607.8480109459788
+- -70.49741332152209
+- 3.796692637047578
+- -162.95954309910817
+- -19.33972914626468
+- -235.09499521014578
+- -107.81384573412645
+- -31.53291462742378
+- -212.14886603050897
+- -89.46516739532265
+- 19.667982651940264
+- -216.33514516843985
+- -323.94021603169455
+- 3.7495919998539446
+- -18.021739092082814
+- -178.17970317466126
+- -346.25709247409145
+- 54.58157208827058
+- -367.15453044986333
+- -268.56541940327196
+- -81.23299020243925
+- -309.8770966018138
+- -100.67991512115881
+- -67.07968197377089
+- -347.2361920567107
+- -158.80898708863495
+- -104.01739402591244
+- -49.383573776489996
+- -298.4424163083782
+- -284.59719829638186
+- 1.9523807141375829
+- -228.8291871431349
+- -164.38882205876453
+- -56.82795956826953
+- 29.181611857038934
+- 197.59244963556043
+- -291.65009486814546
+- -73.96025052809098
+- -413.24345176232146
+- -289.00049014132344
+- -138.12231990311398
+- 122.85209320325096
+- -292.21133060332454
+- -371.8834044482743
+- -292.7279115117123
+- -584.8788687697738
+- -95.33440230176042
+- 239.5339774386685
+- -265.12892232434035
+- -53.34571751364334
+- -319.31875247104443
+- -226.3464861485659
+- -227.7066292952969
+- -322.42046471465534
+- -66.77461133038851
+- -247.64260359977254
+- -128.70180326118776
+- -371.86119903327614
+- 25.939839889457698
+- -303.22971631311106
+- 68.73563774325528
+- -220.62010394260605
+- 1.913655145979548
+- -321.05786511717696
+- -7.480576699947837
+- -562.6168351641293
+- -272.59111196344
+- -352.89209393639175
+- -63.22199637626166
+- -190.819897528042
+- -171.66970452269976
+- -542.3756048762308
+- -147.5045699695532
+- -32.951634458915514
+- -178.31868993964395
+- -370.4598127663352
+- -382.7765759441242
+- 70.55718440121618
+- -275.84240189804007
+- 24.52055475154436
+- -267.85699106997123
+- -362.6936740571793
+- -394.94518637379
+- -42.62824006660958
+- -37.4206296874913
+- 228.65827424080354
+- -368.34337229210706
+- -335.9848146528929
+- -185.57919006723577
+- -182.8323636774351
+- -148.05736988839385
+- -97.9632827112206
+- -172.58472121166204
+- -193.5201220514574
+- 52.94458737541407
+- -321.57488746239267
+- -62.46443325224946
+- 31.459204508390602
+- -320.34494746894666
+- -133.35752677467624
+- -33.91251403894966
+- -212.1959457043817
+- -257.8482070032987
+- -123.65941355538556
+- -46.78896318283864
+- 9.432068300432947
+- 195.92446684229847
+- -92.47988107673412
+- 88.53287931482706
+- -282.7293574505644
+- -56.04024780508934
+- -82.88216300356626
+- 256.79684689104766
+- -260.74896707781465
+- -120.52637661802831
+- 116.14986091913589
+- -66.23371973712261
+- 15.895059395863738
+- -103.5226695817851
+- 210.70305548700676
+- 75.5169908210367
+- -37.984599501492156
+- 43.13787154468077
+- 129.34416144787176
+- 17.434899592911798
+- 56.42418539420781
+- 40.27183357779729
+- 98.78430657831635
+- 215.67691442422276
+- -252.7746044156219
+- 142.22049019547737
+- -123.82476100073752
+- -76.28157211598544
+- 176.92621790810435
+- -137.5335650117231
+- 53.6554360276977
+- 132.97662711499208
+- 40.88040400506932
+- -91.68926179011021
+- -117.31681032188678
+- -222.9524498197763
+- 46.32122638601927
+- 99.43427727081166
+- 19.197643617274366
+- -62.516626238707644
+- -85.33848766172152
+- -292.1531296078384
+- 126.56353108153971
+- -246.1397928436873
+z:
+- 318.2524160356094
+- -12.81841367532651
+- -193.1287544958996
+- -133.63892640590154
+- 204.56723729302252
+- 64.89053292856171
+- -127.84584777350406
+- -136.72974321901606
+- 265.8967844680789
+- -336.67287081313646
+- -81.38469290012515
+- 184.97457481256922
+- -206.1684398552655
+- 178.00750310058874
+- -495.436688572458
+- -184.43643300262082
+- 70.61722830440735
+- -180.53381374269597
+- 61.15147986279117
+- -201.2155198196378
+- 216.42275755355072
+- 75.10195659923006
+- 105.89063668574734
+- -312.93764497623437
+- 67.16122480021578
+- -51.418462010262246
+- 112.0958000868312
+- 101.15634031816816
+- 208.6217673626878
+- -138.010997156574
+- -44.28128581710678
+- -236.62248445940946
+- 43.15533417477952
+- -225.65915729611567
+- -58.21445126098314
+- -15.866666458719003
+- 74.69944948664964
+- 64.08204316537748
+- 97.03165011394645
+- 114.56913004719722
+- 69.4787649122138
+- -105.5269029435403
+- -12.134569507386843
+- -164.8138790490863
+- -230.94019616856926
+- 169.85000089522825
+- -32.6976537911571
+- 61.63993704700873
+- -42.24733910474994
+- -419.57316919610236
+- 8.750229447832593
+- 79.69667187137283
+- -47.81355328320213
+- 287.15248208579277
+- -230.74405474895386
+- -282.5527547209933
+- -342.25135478038527
+- 187.2173874299178
+- -140.9169062236822
+- -230.96167702054072
+- -310.9662394801051
+- -17.32391046987855
+- -226.30122339576295
+- -358.4738424481016
+- -589.8343707622778
+- -278.7087857945023
+- -42.11374263217224
+- -100.79323152793262
+- -146.82423428698908
+- -114.09268567895346
+- -199.50434588100805
+- -112.1359537930152
+- -186.20288786412223
+- -120.66079987326846
+- -181.84723982168921
+- -418.97775621234376
+- -213.1222839881335
+- -291.8090957914463
+- -302.7289002214794
+- -464.26002291202616
+- -282.7260422501955
+- -427.3427612583488
+- -7.52533734197854
+- -141.15940652480538
+- -322.11798023734565
+- -205.3080403096517
+- -283.6248535441436
+- -121.85634073301752
+- -10.386750269561213
+- -60.56173074990178
+- -350.4564659625923
+- 68.19015121550102
+- -148.47295886483207
+- -185.75356805338285
+- -112.29743473122012
+- 189.92420830080493
+- -210.29869438123163
+- -71.53468992439852
+- -94.78836717876979
+- -273.2731007496173
+- -68.11104997702215
+- -52.98663520098969
+- -68.48341395216713
+- 120.20413190025302
+- 52.140332200565595
+- -115.75886751075116
+- -50.620987760471564
+- -42.80291792239785
+- -419.473604990822
+- -56.3871344677163
+- -201.06219586176312
+- 94.2974811911247
+- -34.53199240151439
+- 48.183596631482935
+- 196.31453096731906
+- 191.7814341469975
+- -226.3267741263968
+- 36.55784553908029
+- -4.444206276831911
+- -269.65346359957186
+- 187.22423003269267
+- 199.12359312594646
+- 59.384532474532065
+- -71.2652919102802
+- -148.28452193150292
+- -172.03941811352055
+- 53.119560217802615
+- 238.27762162827779
+- 132.7491820828763
+- -213.46909802749275
+- 82.48618282765918
+- -1.8356810571683047
+- -136.6766586962044
+- 191.14763171417005
+- -17.31448628033703
+- -201.57759561405004
+- -162.47596852664273
+- 262.32005188164777
+- 165.87873799997138
+- 78.67759494035175
+- -243.3700332084363
+- -293.33086145315184
+- 14.78420456461757
+- 14.394439887667057
+- -62.39227894825053
+- -5.181470901694216
+- -94.85502949056276
+- -173.18252574263116
+- -163.59185250182108
+- -184.34441485108246
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
new file mode 100644
index 000000000..c6ea837cc
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -329.0711855309372
+- -555.5866882180059
+- -104.58297874011392
+- -2.31610393784343
+- -145.10299805895374
+- -30.6423247888793
+- -203.40476571285322
+- -83.03145361443573
+- -21.369999629064097
+- -224.24770587079067
+- -94.31629759354433
+- 13.060557777720998
+- -200.50065555671955
+- -307.3617942873127
+- -8.044368106379597
+- -57.105728307945256
+- -170.13401850482617
+- -325.98967484159573
+- 65.0496382519191
+- -356.73775529810734
+- -284.91331368673525
+- -82.86540331135926
+- -331.8947389792898
+- -119.31552044076689
+- -75.51241513334885
+- -341.8558356608372
+- -151.8028947175887
+- -99.99950031706716
+- -38.09618745445778
+- -242.56731516683786
+- -309.3955184390734
+- -6.783199544047148
+- -224.81975335379195
+- -151.8657855607271
+- -44.501053886357965
+- -29.646738265946105
+- 164.45696460076235
+- -301.4316242031351
+- -85.68250429679401
+- -388.6575758303645
+- -307.6952758231452
+- -161.18626534059788
+- 80.78233838329142
+- -294.3586779667834
+- -359.0361508513471
+- -260.69164561914914
+- -576.5126324399652
+- -62.368222159889896
+- 222.18856182544508
+- -250.96680104569987
+- -34.20081057789081
+- -307.73422523772894
+- -221.63451724911042
+- -231.58479114966983
+- -306.12680425104674
+- -50.906293885031374
+- -246.98807481851077
+- -134.0157729229602
+- -357.8099140423182
+- 13.23980588771653
+- -312.58947096862306
+- 58.764387438230884
+- -209.4931772494878
+- 5.909518629257212
+- -314.52627694533714
+- -12.558458719775086
+- -555.9543715967784
+- -293.0218713586622
+- -323.7466334057262
+- -57.204912974388314
+- -191.88623597367595
+- -182.44128930988202
+- -548.655263245792
+- -142.64620590218237
+- -45.207105686411055
+- -161.17213864808951
+- -373.1832005398029
+- -405.2992418760756
+- 65.40672417007477
+- -256.5986325049997
+- 9.478512764563026
+- -253.50661708514255
+- -388.21307102091765
+- -416.93023383838096
+- -35.85533391550191
+- 23.9042326994027
+- 222.30998710432394
+- -385.4762562108745
+- -350.46567990715783
+- -224.1561906591944
+- -227.5284510412487
+- -175.5458706696473
+- -123.01874092574528
+- -195.18529421746595
+- -239.51637023074065
+- 77.63838035268526
+- -316.33442343379573
+- -48.9971054502618
+- 32.25728358433121
+- -323.6590268922707
+- -143.3890406055504
+- -16.169232598915645
+- -220.09526684002788
+- -236.7619741347166
+- -100.10927306238162
+- -36.760208428965015
+- 76.56419920016688
+- 199.33594639288535
+- -101.7778888114048
+- 143.39155149528443
+- -262.8498377607621
+- -58.285897623073375
+- -69.40666659129158
+- 219.6394251688235
+- -241.78623629043472
+- -128.73170276829336
+- 124.57046980302249
+- -92.3678562671916
+- 32.92484130822262
+- -100.8226497010224
+- 232.77221413261876
+- 59.579841636704046
+- -35.805837671775514
+- 28.748810015327066
+- 100.20228742167698
+- 38.62663991679109
+- 59.82005771880539
+- 51.421417520701674
+- 129.74002811640477
+- 232.49129738287866
+- -264.98106561521854
+- 166.9033532147802
+- -113.64413710244814
+- -71.34082247843298
+- 165.0870773712482
+- -122.08643661039227
+- 64.02189371851092
+- 129.93570259338148
+- -8.26175815010019
+- -90.60900990710952
+- -110.0890869585528
+- -202.53152964626926
+- 74.87080758603038
+- 82.11594060921145
+- 22.608892894721702
+- -76.76234575533432
+- -72.342143967381
+- -284.100402514327
+- 126.66025041347473
+- -228.50860803486466
+y:
+- -329.0711855309372
+- -555.5866882180059
+- -104.58297874011392
+- -2.31610393784343
+- -145.10299805895374
+- -30.6423247888793
+- -203.40476571285322
+- -83.03145361443573
+- -21.369999629064097
+- -224.24770587079067
+- -94.31629759354433
+- 13.060557777720998
+- -200.50065555671955
+- -307.3617942873127
+- -8.044368106379597
+- -57.105728307945256
+- -170.13401850482617
+- -325.98967484159573
+- 65.0496382519191
+- -356.73775529810734
+- -284.91331368673525
+- -82.86540331135926
+- -331.8947389792898
+- -119.31552044076689
+- -75.51241513334885
+- -341.8558356608372
+- -151.8028947175887
+- -99.99950031706716
+- -38.09618745445778
+- -242.56731516683786
+- -309.3955184390734
+- -6.783199544047148
+- -224.81975335379195
+- -151.8657855607271
+- -44.501053886357965
+- -29.646738265946105
+- 164.45696460076235
+- -301.4316242031351
+- -85.68250429679401
+- -388.6575758303645
+- -307.6952758231452
+- -161.18626534059788
+- 80.78233838329142
+- -294.3586779667834
+- -359.0361508513471
+- -260.69164561914914
+- -576.5126324399652
+- -62.368222159889896
+- 222.18856182544508
+- -250.96680104569987
+- -34.20081057789081
+- -307.73422523772894
+- -221.63451724911042
+- -231.58479114966983
+- -306.12680425104674
+- -50.906293885031374
+- -246.98807481851077
+- -134.0157729229602
+- -357.8099140423182
+- 13.23980588771653
+- -312.58947096862306
+- 58.764387438230884
+- -209.4931772494878
+- 5.909518629257212
+- -314.52627694533714
+- -12.558458719775086
+- -555.9543715967784
+- -293.0218713586622
+- -323.7466334057262
+- -57.204912974388314
+- -191.88623597367595
+- -182.44128930988202
+- -548.655263245792
+- -142.64620590218237
+- -45.207105686411055
+- -161.17213864808951
+- -373.1832005398029
+- -405.2992418760756
+- 65.40672417007477
+- -256.5986325049997
+- 9.478512764563026
+- -253.50661708514255
+- -388.21307102091765
+- -416.93023383838096
+- -35.85533391550191
+- 23.9042326994027
+- 222.30998710432394
+- -385.4762562108745
+- -350.46567990715783
+- -224.1561906591944
+- -227.5284510412487
+- -175.5458706696473
+- -123.01874092574528
+- -195.18529421746595
+- -239.51637023074065
+- 77.63838035268526
+- -316.33442343379573
+- -48.9971054502618
+- 32.25728358433121
+- -323.6590268922707
+- -143.3890406055504
+- -16.169232598915645
+- -220.09526684002788
+- -236.7619741347166
+- -100.10927306238162
+- -36.760208428965015
+- 76.56419920016688
+- 199.33594639288535
+- -101.7778888114048
+- 143.39155149528443
+- -262.8498377607621
+- -58.285897623073375
+- -69.40666659129158
+- 219.6394251688235
+- -241.78623629043472
+- -128.73170276829336
+- 124.57046980302249
+- -92.3678562671916
+- 32.92484130822262
+- -100.8226497010224
+- 232.77221413261876
+- 59.579841636704046
+- -35.805837671775514
+- 28.748810015327066
+- 100.20228742167698
+- 38.62663991679109
+- 59.82005771880539
+- 51.421417520701674
+- 129.74002811640477
+- 232.49129738287866
+- -264.98106561521854
+- 166.9033532147802
+- -113.64413710244814
+- -71.34082247843298
+- 165.0870773712482
+- -122.08643661039227
+- 64.02189371851092
+- 129.93570259338148
+- -8.26175815010019
+- -90.60900990710952
+- -110.0890869585528
+- -202.53152964626926
+- 74.87080758603038
+- 82.11594060921145
+- 22.608892894721702
+- -76.76234575533432
+- -72.342143967381
+- -284.100402514327
+- 126.66025041347473
+- -228.50860803486466
+z:
+- 297.79042429425226
+- 3.1137291840984784
+- -199.79804262884326
+- -166.20778458278707
+- 204.69266279707844
+- 97.70819525501882
+- -123.42721029973492
+- -138.9567928023258
+- 312.0141915765828
+- -319.7129166600936
+- -83.68518533371554
+- 186.28767447658555
+- -205.76641221583856
+- 183.83799060121967
+- -488.92986843051915
+- -215.93957129691216
+- 70.49780694767222
+- -203.50910815105814
+- 37.57643019183341
+- -233.44846199143038
+- 217.7948078551173
+- 3.5481481504188253
+- 120.35834473050457
+- -306.05732430191256
+- 52.488051623674
+- -55.692301355998985
+- 79.04662465301803
+- 81.03462003057886
+- 244.85764115262003
+- -111.22944019458778
+- -37.937831440999716
+- -237.19542016798638
+- 37.656299282744435
+- -247.44659505064843
+- -54.47235330512455
+- -24.198867485082072
+- 76.10357146916093
+- 71.04548660852791
+- 114.89380491785217
+- 142.20377531044872
+- 73.62647028378173
+- -78.72311946495823
+- -5.145052299241284
+- -113.37922122144694
+- -227.43779751113058
+- 187.87032439276646
+- -32.658355245811926
+- 37.082149962892075
+- -78.92288250965485
+- -430.98997596717254
+- 27.98709979532043
+- 62.312459092847945
+- -54.242383993849245
+- 253.3388108166754
+- -211.39550749207913
+- -279.79690400904605
+- -322.6584409192026
+- 136.56895616556213
+- -176.6985250092431
+- -227.4305972850767
+- -308.04966009779855
+- -10.708822516833797
+- -260.0233348053526
+- -358.79582744973607
+- -586.0567478456851
+- -275.617872376902
+- -76.05802327609088
+- -89.62979625962606
+- -103.88702762338285
+- -95.61016215643755
+- -177.32013376365276
+- -95.98136155864958
+- -179.6644382662621
+- -111.53238134733428
+- -204.8674055411026
+- -424.10844892078677
+- -197.60230728384653
+- -276.37064864119
+- -311.9214887840406
+- -430.7164806032802
+- -281.0705610118752
+- -410.68743854503833
+- -6.434572510633216
+- -126.52732679084649
+- -317.21362953276855
+- -211.69843859739618
+- -319.16811084191255
+- -124.13689408914638
+- -3.488585636282498
+- -65.33035632988309
+- -326.20640674073815
+- 39.27253860375597
+- -142.50237525654558
+- -197.53235995705268
+- -160.60316513284505
+- 178.80174150334656
+- -221.6071703129716
+- -109.03890715449374
+- -116.84324533692258
+- -278.86505738468713
+- -118.54468946016623
+- -56.32150412384613
+- -88.66693251992838
+- 123.62520275873422
+- 34.5944398638407
+- -117.5807686212899
+- -10.182963458022572
+- -32.27481818721868
+- -417.45068519416714
+- -58.541631258886845
+- -199.3110745951792
+- 85.6684357221267
+- 31.59736579183526
+- 52.309021854841205
+- 217.56880810985817
+- 227.37991751797878
+- -227.24645954982716
+- 19.460702541404583
+- -1.809934379054593
+- -297.94682220426375
+- 174.48386777867347
+- 189.37981065584972
+- 59.84928077601992
+- -64.87521918094619
+- -133.51913011814705
+- -188.35327366334673
+- 69.11953129673934
+- 266.294432175262
+- 138.08152826093485
+- -188.1741125571068
+- 83.15494234206733
+- -14.773643508528037
+- -135.64227164062697
+- 207.89273269134787
+- 3.712019153520307
+- -222.85961605054814
+- -126.2928409003393
+- 251.75993313045151
+- 165.1938803392583
+- 78.38935957552465
+- -250.11253826176915
+- -272.3912795752816
+- 50.81096047360382
+- 25.91812435561397
+- -34.15554633725905
+- 28.386579409927094
+- -62.37718307088183
+- -164.52979947684372
+- -161.41818051546772
+- -158.09310696825614
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
new file mode 100644
index 000000000..c203bf9e5
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -121.15478633343794
+- -130.4159808404925
+- -130.27646944026458
+- -113.64328655416284
+- -121.44628695154886
+- -166.2990451476203
+- -261.2636454017961
+- -184.76630901132344
+- -122.10815924542676
+- -282.2547492865744
+- -138.2182137765148
+- -109.88726609443934
+- -150.720646999703
+- -207.41906706153105
+- -75.78930140492815
+- -183.1141566294361
+- -145.86465327098085
+- -235.36504545335836
+- -281.0945547173969
+- -233.18214620754406
+- -207.12616237540004
+- -152.43461471702767
+- -203.04141984763248
+- -150.02455852136328
+- -187.34518426809518
+- -265.6111612741161
+- -129.04348845917977
+- -193.41142225312666
+- -90.7049859565704
+- -127.25169814282474
+- -182.64504946140917
+- -157.8249434663258
+- -217.7562185578996
+- -181.93348129015948
+- -242.71386084285342
+- -146.79164939442038
+- -146.71336252409114
+- -229.87361647392953
+- -133.77193019885993
+- -221.35140091427792
+- -155.94743088882197
+- -166.81724509985116
+- -113.09320678295467
+- -171.27027465849542
+- -159.377602672052
+- -187.21305388503686
+- -238.80090511164062
+- -253.1262646143503
+- -109.08487387042756
+- -184.49469971168386
+- -191.5132311229388
+- -70.44496776860801
+- -215.95165963817644
+- -107.85620819625484
+- -149.06971162669294
+- -119.58156072478897
+- -159.54179461640751
+- -230.98093410223288
+- -201.63392035010048
+- -90.5827797630511
+- -142.80374779196856
+- -95.45514778314696
+- -165.28086739578055
+- -88.62227912176544
+- -119.88567178023406
+- -135.31015574629714
+- -223.9174629585549
+- -146.8804645936043
+- -288.5096537730227
+- -176.28728372651457
+- -254.21306186323173
+- -57.47901626585383
+- -111.89370887483302
+- -167.58998789322217
+- -156.84442274791556
+- -178.862535446946
+- -131.98705585336998
+- -155.53653700458938
+- -114.87982704608743
+- -127.6053182111233
+- -169.29952702677664
+- -230.71280999323812
+- -110.69697751547955
+- -147.60343628352976
+- -77.0222276016208
+- -116.05610464762434
+- -59.98167452309284
+- -145.59731787917104
+- -161.10254812375408
+- -243.46339060931686
+- -60.274220954704276
+- -34.39861924673747
+- -126.93520435774637
+- -118.75874670731521
+- -73.14398889461458
+- -91.82329391402995
+- -124.65378712679401
+- -120.06852899132907
+- -38.848619248791735
+- -148.5131138691994
+- -96.11620901748239
+- -166.62503100670511
+- -146.99077907273647
+- -103.29466946283068
+- -129.278248987387
+- -40.509302977612506
+- -163.45472246452005
+- -13.143062898443247
+- -97.43882615461838
+- -86.60275662369912
+- -145.7810199124991
+- -125.2793809409674
+- -165.23145616311692
+- 132.4120951658318
+- -45.79313797853327
+- -127.40765968376884
+- -64.48246125134183
+- -70.96459335268244
+- -85.77909066650177
+- -94.10623918741744
+- -76.58296444224553
+- -4.976619258568157
+- 5.413038481518587
+- -10.133824365383301
+- -119.32397167560802
+- -25.615445843241176
+- -15.827317551960947
+- -21.449299818373746
+- 81.63003002905442
+- -23.37984925727442
+- -25.08432384280621
+- 29.63273278568562
+- -80.38488086520215
+- 33.28432641495689
+- -58.79565430878664
+- 13.956506074278591
+- -47.47275973086286
+- -0.16772532349929142
+- 21.53988916730403
+- -73.84273357578618
+- -72.00767054173988
+- -172.19982217673925
+- -1.5198048382999299
+- -116.49223499770191
+- -91.15236024297808
+- -55.5915083964266
+- -62.02646446322047
+- -131.33091040195643
+- -142.89761461974723
+- -129.5469960360492
+y:
+- -121.15478633343794
+- -130.4159808404925
+- -130.27646944026458
+- -113.64328655416284
+- -121.44628695154886
+- -166.2990451476203
+- -261.2636454017961
+- -184.76630901132344
+- -122.10815924542676
+- -282.2547492865744
+- -138.2182137765148
+- -109.88726609443934
+- -150.720646999703
+- -207.41906706153105
+- -75.78930140492815
+- -183.1141566294361
+- -145.86465327098085
+- -235.36504545335836
+- -281.0945547173969
+- -233.18214620754406
+- -207.12616237540004
+- -152.43461471702767
+- -203.04141984763248
+- -150.02455852136328
+- -187.34518426809518
+- -265.6111612741161
+- -129.04348845917977
+- -193.41142225312666
+- -90.7049859565704
+- -127.25169814282474
+- -182.64504946140917
+- -157.8249434663258
+- -217.7562185578996
+- -181.93348129015948
+- -242.71386084285342
+- -146.79164939442038
+- -146.71336252409114
+- -229.87361647392953
+- -133.77193019885993
+- -221.35140091427792
+- -155.94743088882197
+- -166.81724509985116
+- -113.09320678295467
+- -171.27027465849542
+- -159.377602672052
+- -187.21305388503686
+- -238.80090511164062
+- -253.1262646143503
+- -109.08487387042756
+- -184.49469971168386
+- -191.5132311229388
+- -70.44496776860801
+- -215.95165963817644
+- -107.85620819625484
+- -149.06971162669294
+- -119.58156072478897
+- -159.54179461640751
+- -230.98093410223288
+- -201.63392035010048
+- -90.5827797630511
+- -142.80374779196856
+- -95.45514778314696
+- -165.28086739578055
+- -88.62227912176544
+- -119.88567178023406
+- -135.31015574629714
+- -223.9174629585549
+- -146.8804645936043
+- -288.5096537730227
+- -176.28728372651457
+- -254.21306186323173
+- -57.47901626585383
+- -111.89370887483302
+- -167.58998789322217
+- -156.84442274791556
+- -178.862535446946
+- -131.98705585336998
+- -155.53653700458938
+- -114.87982704608743
+- -127.6053182111233
+- -169.29952702677664
+- -230.71280999323812
+- -110.69697751547955
+- -147.60343628352976
+- -77.0222276016208
+- -116.05610464762434
+- -59.98167452309284
+- -145.59731787917104
+- -161.10254812375408
+- -243.46339060931686
+- -60.274220954704276
+- -34.39861924673747
+- -126.93520435774637
+- -118.75874670731521
+- -73.14398889461458
+- -91.82329391402995
+- -124.65378712679401
+- -120.06852899132907
+- -38.848619248791735
+- -148.5131138691994
+- -96.11620901748239
+- -166.62503100670511
+- -146.99077907273647
+- -103.29466946283068
+- -129.278248987387
+- -40.509302977612506
+- -163.45472246452005
+- -13.143062898443247
+- -97.43882615461838
+- -86.60275662369912
+- -145.7810199124991
+- -125.2793809409674
+- -165.23145616311692
+- 132.4120951658318
+- -45.79313797853327
+- -127.40765968376884
+- -64.48246125134183
+- -70.96459335268244
+- -85.77909066650177
+- -94.10623918741744
+- -76.58296444224553
+- -4.976619258568157
+- 5.413038481518587
+- -10.133824365383301
+- -119.32397167560802
+- -25.615445843241176
+- -15.827317551960947
+- -21.449299818373746
+- 81.63003002905442
+- -23.37984925727442
+- -25.08432384280621
+- 29.63273278568562
+- -80.38488086520215
+- 33.28432641495689
+- -58.79565430878664
+- 13.956506074278591
+- -47.47275973086286
+- -0.16772532349929142
+- 21.53988916730403
+- -73.84273357578618
+- -72.00767054173988
+- -172.19982217673925
+- -1.5198048382999299
+- -116.49223499770191
+- -91.15236024297808
+- -55.5915083964266
+- -62.02646446322047
+- -131.33091040195643
+- -142.89761461974723
+- -129.5469960360492
+z:
+- -12.587038106327535
+- -75.8693731388532
+- -93.00580404520751
+- -82.28073181181468
+- -55.017515432835864
+- -78.58553297105018
+- -181.7216171468009
+- -105.8153065907965
+- -113.72834735653169
+- -123.77835231502743
+- -94.36124237121506
+- 7.999874703690282
+- -88.505402572207
+- -84.23582958218344
+- -167.35780831101766
+- -191.1077910787939
+- -112.51782966658953
+- -157.5923619634534
+- -88.92691745071829
+- -143.34048412853903
+- -84.56169786453201
+- -87.94842923788495
+- -130.01615815791274
+- -68.12015090403574
+- -161.75758261934803
+- -116.00810406593533
+- -89.82812787449495
+- -2.022554635745049
+- -11.76170352666318
+- -32.255452835749836
+- -115.1246554278457
+- -71.49232748361257
+- -124.00230967207509
+- -70.89645000454215
+- -102.95482128272164
+- -50.82159027419259
+- -33.06904048823107
+- -22.63036689793711
+- 2.9025713302973144
+- -124.74230868900925
+- -8.922991349024134
+- -53.6855755869238
+- -41.667422918658055
+- -59.574977423264315
+- -52.24133195122149
+- -14.9753707692793
+- 18.371216641533415
+- 40.448751873301134
+- -30.07601475057829
+- -8.638289462281344
+- -19.103673741593955
+- -58.989160693329964
+- 38.81395101189456
+- 64.31413648932175
+- 61.814901826391015
+- -22.73329548690856
+- 92.4764821585138
+- -30.905180607424644
+- 95.19599454483829
+- -59.41134246808385
+- -76.17332243157632
+- 49.14415703073
+- -63.74836549475062
+- -65.66980125862429
+- -46.31125437510522
+- -15.5854018545872
+- -68.1014814060287
+- -32.71914549612089
+- -82.70066988198958
+- 41.37278808561206
+- -35.29310228210178
+- -24.80334069785697
+- -2.501666953743949
+- -81.05624834186443
+- 44.689570141437194
+- -9.444026603431737
+- 28.661934934171853
+- 33.534676508527404
+- -23.002377446693522
+- -57.54964424848572
+- -36.844359921983305
+- -74.2492910493037
+- -22.215655183248508
+- -8.425280695608198
+- -81.06803504992166
+- 47.50293629619174
+- 1.2507596344205354
+- -22.147329795064653
+- 21.817219643649302
+- -2.233890964099581
+- 25.703929585905495
+- -56.973551523556274
+- 72.05132559995151
+- -88.96915425026873
+- 10.404583012909765
+- 19.217131237387072
+- -51.67869170029975
+- -86.73728854036517
+- 8.965281264407682
+- -61.39529818193962
+- -43.19282276378947
+- -49.742938375732855
+- -135.55064286454
+- -69.48539283915129
+- -34.52860770873315
+- -41.45447761465108
+- -45.31136237153822
+- -172.98720619427928
+- -91.62860054228031
+- -118.69990132513988
+- -98.59155740063761
+- -38.615966048441194
+- -59.95574983985258
+- -46.60106300721521
+- -24.44048283174632
+- -93.30967132941213
+- -139.37546334098232
+- -191.03883261368398
+- -96.34676308834342
+- -219.64922426058578
+- -54.31860599999553
+- 41.76489177130727
+- -94.79148089749532
+- -105.1248427909044
+- -179.88647293330678
+- -90.66257184925013
+- -62.56560384391366
+- 6.115980278907556
+- 10.37361018493507
+- -152.85158652125753
+- -69.1644018552841
+- -130.76626993605592
+- -92.52705628205884
+- 4.084238037716294
+- -115.05479159529355
+- -101.78629734791565
+- -111.11818707348276
+- -57.16831584420238
+- -55.389153204954084
+- -10.608819349789805
+- -70.22122456032068
+- -171.76502737028918
+- -32.273121976922596
+- -154.64579911444298
+- -58.88622322185824
+- -128.42693267456954
+- -123.58760787442688
+- -185.1613937082734
+- -123.90573654523244
+- -135.67799993329913
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
new file mode 100644
index 000000000..ef4039d3b
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -66.18292178053807
+- -115.63371289306161
+- -40.08998640911374
+- -261.31000395194786
+- -166.7909838724932
+- -139.54637670314835
+- -208.37719185679137
+- -57.717209802050384
+- -146.99926782334603
+- -94.16984989068037
+- -255.50464359809362
+- -67.9308770378757
+- -353.4014028425089
+- -168.20396034869532
+- -191.70073210680513
+- -36.41025520518381
+- -258.97388466685254
+- -239.09393621075688
+- -215.75428259623223
+- -99.96184009589246
+- -117.46963703545366
+- -16.350110272823727
+- -234.7905478492251
+- -165.3589026036757
+- 0.41946019958205855
+- -115.11125087827226
+- -147.41853651364576
+- -103.74454769846525
+- -140.90619141642424
+- -201.40554357493755
+- -341.71306618260576
+- -47.520601996709544
+- 8.723661160614721
+- -304.5231100867443
+- -12.198570874493013
+- -200.07187414553692
+- -89.68284195206441
+- -250.5605909355231
+- -276.7974106271435
+- -80.99692915874027
+- -136.66654586506084
+- -51.79816062682904
+- -85.62445575252828
+- -262.1957398217536
+- -193.5124451059287
+- -205.26576800249143
+- -138.34121902448936
+- -71.9445712933336
+- -22.608517251998556
+- -94.87504568074831
+- -127.96760156077839
+- -173.48834175964086
+- -196.745733980285
+- -159.36858356085776
+- -80.60888792369248
+- -468.1650837655404
+- -125.51666316581834
+- 10.712475760469315
+- -5.411213154927566
+- 96.82693230399087
+- -41.911224794493464
+- 22.762124542131428
+- -98.85987185081449
+- -28.459453910768794
+- 47.521581958349294
+- -248.36345727055203
+- -167.45029857022115
+- 128.28266410089716
+- 57.27033034845042
+- -58.82963578612552
+- -78.48792428054871
+- 67.26178698214157
+- -50.94569383530171
+- 56.58236082078612
+- -120.18656749761499
+- -100.11721665267609
+- -101.31410997999905
+- -55.53205860919838
+- 100.88786413431473
+- -66.82830491525722
+- 118.40778297185312
+- 68.27470664018782
+- 18.77035395820749
+- 38.56560723006115
+- 108.36119035325538
+- 2.505752009999138
+- 95.48861783590644
+- -86.62410294454978
+- 62.88822437860383
+- 8.45668376037163
+- 74.09636149418711
+- -154.22210970764675
+- -33.253355796991094
+- 247.37717511204832
+- 143.07800461797132
+- -78.2684811293491
+- 51.571400492195494
+- -132.71704558130395
+- -13.784941989330031
+- 146.01977475651552
+- 39.56660609935573
+- 113.70599705854998
+- 77.3503599700983
+- 4.444035442699823
+- 55.18774963618358
+- 42.463739509132644
+- 123.39346732558771
+- -54.62464266152292
+- 187.43447087945583
+- 44.22944044489332
+- 73.45213044828706
+- 20.761036793462168
+- -108.08520198682515
+- 176.7604941815522
+- 51.5867519537819
+- 163.86498409750413
+- 146.4850396263265
+- 203.95973954751176
+- 68.72742030684795
+- 261.21759885320284
+- -7.866632151184333
+- 163.9094272594558
+- 234.0995403976671
+- 118.97240645672969
+- 114.44267554008135
+- 133.6913309297981
+- 269.8763973140816
+- 272.9466815796491
+- 99.81503504177155
+- 296.59531963384484
+- 17.92059475315728
+- -16.90459184109093
+- 179.07991534518297
+- 354.0866295072622
+- 137.07533375520808
+- 237.85127101868108
+- 128.85925371868234
+- -20.31813870048381
+- 60.955340188663904
+- 64.94068162389156
+- -30.98586734122918
+- 316.40864325870433
+- 243.4731765974454
+- 125.7636127901831
+- 174.61246380962058
+- 72.91552370463228
+- 117.74987495056021
+- 89.06776397710752
+- -61.98483934620268
+- 81.44306324559632
+y:
+- -66.18292178053807
+- -115.63371289306161
+- -40.08998640911374
+- -261.31000395194786
+- -166.7909838724932
+- -139.54637670314835
+- -208.37719185679137
+- -57.717209802050384
+- -146.99926782334603
+- -94.16984989068037
+- -255.50464359809362
+- -67.9308770378757
+- -353.4014028425089
+- -168.20396034869532
+- -191.70073210680513
+- -36.41025520518381
+- -258.97388466685254
+- -239.09393621075688
+- -215.75428259623223
+- -99.96184009589246
+- -117.46963703545366
+- -16.350110272823727
+- -234.7905478492251
+- -165.3589026036757
+- 0.41946019958205855
+- -115.11125087827226
+- -147.41853651364576
+- -103.74454769846525
+- -140.90619141642424
+- -201.40554357493755
+- -341.71306618260576
+- -47.520601996709544
+- 8.723661160614721
+- -304.5231100867443
+- -12.198570874493013
+- -200.07187414553692
+- -89.68284195206441
+- -250.5605909355231
+- -276.7974106271435
+- -80.99692915874027
+- -136.66654586506084
+- -51.79816062682904
+- -85.62445575252828
+- -262.1957398217536
+- -193.5124451059287
+- -205.26576800249143
+- -138.34121902448936
+- -71.9445712933336
+- -22.608517251998556
+- -94.87504568074831
+- -127.96760156077839
+- -173.48834175964086
+- -196.745733980285
+- -159.36858356085776
+- -80.60888792369248
+- -468.1650837655404
+- -125.51666316581834
+- 10.712475760469315
+- -5.411213154927566
+- 96.82693230399087
+- -41.911224794493464
+- 22.762124542131428
+- -98.85987185081449
+- -28.459453910768794
+- 47.521581958349294
+- -248.36345727055203
+- -167.45029857022115
+- 128.28266410089716
+- 57.27033034845042
+- -58.82963578612552
+- -78.48792428054871
+- 67.26178698214157
+- -50.94569383530171
+- 56.58236082078612
+- -120.18656749761499
+- -100.11721665267609
+- -101.31410997999905
+- -55.53205860919838
+- 100.88786413431473
+- -66.82830491525722
+- 118.40778297185312
+- 68.27470664018782
+- 18.77035395820749
+- 38.56560723006115
+- 108.36119035325538
+- 2.505752009999138
+- 95.48861783590644
+- -86.62410294454978
+- 62.88822437860383
+- 8.45668376037163
+- 74.09636149418711
+- -154.22210970764675
+- -33.253355796991094
+- 247.37717511204832
+- 143.07800461797132
+- -78.2684811293491
+- 51.571400492195494
+- -132.71704558130395
+- -13.784941989330031
+- 146.01977475651552
+- 39.56660609935573
+- 113.70599705854998
+- 77.3503599700983
+- 4.444035442699823
+- 55.18774963618358
+- 42.463739509132644
+- 123.39346732558771
+- -54.62464266152292
+- 187.43447087945583
+- 44.22944044489332
+- 73.45213044828706
+- 20.761036793462168
+- -108.08520198682515
+- 176.7604941815522
+- 51.5867519537819
+- 163.86498409750413
+- 146.4850396263265
+- 203.95973954751176
+- 68.72742030684795
+- 261.21759885320284
+- -7.866632151184333
+- 163.9094272594558
+- 234.0995403976671
+- 118.97240645672969
+- 114.44267554008135
+- 133.6913309297981
+- 269.8763973140816
+- 272.9466815796491
+- 99.81503504177155
+- 296.59531963384484
+- 17.92059475315728
+- -16.90459184109093
+- 179.07991534518297
+- 354.0866295072622
+- 137.07533375520808
+- 237.85127101868108
+- 128.85925371868234
+- -20.31813870048381
+- 60.955340188663904
+- 64.94068162389156
+- -30.98586734122918
+- 316.40864325870433
+- 243.4731765974454
+- 125.7636127901831
+- 174.61246380962058
+- 72.91552370463228
+- 117.74987495056021
+- 89.06776397710752
+- -61.98483934620268
+- 81.44306324559632
+z:
+- 206.98447500965165
+- 107.8294028130727
+- 127.70627060298409
+- 141.06488888291858
+- -15.767558936324034
+- 104.98876408345981
+- 5.876224779897572
+- -156.24625539731528
+- -2.138097040417299
+- 127.13165841939589
+- 76.15459030446152
+- 50.62623211621471
+- 9.119504854677466
+- 161.5643353673217
+- -57.99700273904999
+- -32.27230503978612
+- -83.14731785803305
+- -15.175583056023067
+- 138.97949739911908
+- 41.44931524359774
+- -104.48621776700901
+- -184.41298252085062
+- 158.59376931867337
+- 245.7816880131291
+- 108.7612260328278
+- 58.93201120538486
+- 192.36588072885687
+- -56.5774733003704
+- 104.61593660612346
+- 43.29415695030378
+- 255.3923695434989
+- 132.63037432824035
+- 136.27528838503292
+- 135.1001577502892
+- 195.8501320161921
+- 132.10821361172376
+- 203.36045633476928
+- 167.100557680537
+- 324.5367362072095
+- 118.97279391613657
+- 70.48910814642515
+- 79.43194310400982
+- 120.72703615900545
+- 161.0617461663875
+- 187.02078690130878
+- -39.09418401746164
+- 201.29432195272426
+- 408.1755312763556
+- 334.64368728285683
+- 448.61478103938055
+- 185.06747764565623
+- 285.3909299656516
+- 304.80006984524545
+- 339.07366901303755
+- 277.1096903094583
+- 305.7648810698007
+- 312.8797610167065
+- 297.10552884618846
+- 429.46195026307305
+- 339.901727499861
+- 342.67179256842775
+- 415.8940569913967
+- 329.1228191731202
+- 260.3604320498785
+- 312.0300605531544
+- 77.40047377404535
+- 175.2247417607782
+- 290.631126753723
+- 153.06476762959315
+- 169.29995545563162
+- 418.0612051484701
+- 251.0493006899274
+- 357.5688644492346
+- 280.32211179723
+- 82.14656190493739
+- 503.0296295803606
+- 496.86752838427554
+- 429.7246679629917
+- 194.17299708811746
+- 243.71482842988723
+- 227.14613734687023
+- 227.4711869426846
+- 258.99755010599637
+- 110.97686599389304
+- 185.5664968429427
+- 420.31118153511665
+- 231.8822607074187
+- 309.81649311171697
+- 175.2254131846928
+- 179.2425988729483
+- 136.3979622202208
+- 99.79411510666263
+- 93.48973929992295
+- 109.74382336899727
+- 153.2673809862572
+- 33.70494904998594
+- 88.8129610229828
+- 122.22715562477637
+- 222.00679498214728
+- 230.69968267904997
+- 116.29246653078391
+- 246.87224523938258
+- 158.56768699833208
+- 9.116712893784213
+- 26.284223011217062
+- 175.98358193409112
+- 254.6037023245449
+- 163.13151676047016
+- 187.81788206849762
+- 64.42658973303092
+- 54.22239279380908
+- 148.48615044959578
+- -7.525251620456199
+- -17.583321211299438
+- 29.239331317104764
+- 60.17734941116717
+- -1.6246593546865153
+- 252.72611240296132
+- 76.80778332476146
+- -35.10657574865937
+- 0.15266081527046924
+- 151.53744299599742
+- 140.21158189869433
+- 308.587107767958
+- 180.10815121835486
+- 202.5833415734632
+- 268.8110941119751
+- 33.00458462815347
+- 247.14823748146384
+- 57.486615393927025
+- 306.63228759618227
+- 122.54878915217395
+- 44.552638000239895
+- 205.18443291082852
+- 270.7512434552188
+- -99.16085824948928
+- 182.44998131671272
+- 131.24849717166114
+- 135.6012176624329
+- 41.77685022596228
+- 71.26092745432041
+- 244.9924636858453
+- 170.2397777788086
+- 28.33663036933796
+- 123.17452269069157
+- 118.51957486669377
+- -19.85332562927001
+- 23.50119531752805
+- 54.07588641447143
+- -91.07200624607479
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
new file mode 100644
index 000000000..81f89ffbb
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
@@ -0,0 +1,828 @@
+x:
+- 43.8689624517487
+- -169.6007983643741
+- 30.065101369218713
+- -213.80194456204015
+- -177.86818599720698
+- -89.47729250601859
+- -237.26126603270572
+- -28.90895178468717
+- -241.97378181205764
+- 28.751410237221332
+- -279.8852663973567
+- -34.97992192624139
+- -364.0200198040815
+- -146.56388627682088
+- -235.51843671259178
+- -67.9826828007947
+- -300.9679613175251
+- -266.01731940042015
+- -160.40686421128942
+- -31.917032713536337
+- -228.17916917278487
+- -67.52265418325968
+- -279.93543505386316
+- -99.28769208154215
+- -41.73964330325352
+- -14.03180281400123
+- -164.09225196361115
+- -95.27620682960934
+- -169.03212195117646
+- -243.22225481386909
+- -438.3874977112839
+- -137.38476119639145
+- 50.83280989998028
+- -385.52775451727695
+- -115.00777420022197
+- -137.58527756048275
+- -158.91692533593877
+- -176.8491552727091
+- -322.2574538030059
+- -137.64920778885318
+- -89.03812059637349
+- -216.73854792889284
+- -123.12538039931468
+- -222.69966954734372
+- -132.56932758571173
+- -204.80667799423975
+- -206.15111386654274
+- -8.105410990393208
+- 69.05680558576898
+- -217.54000016851208
+- -105.40398832162322
+- -168.66374801089185
+- -181.10928685990706
+- -162.38012010804493
+- -157.18749108736537
+- -497.1532400025114
+- -81.47159264942029
+- 40.60934477919244
+- 72.52028937802565
+- 211.11407870514296
+- 31.925044889424697
+- -15.448604613922663
+- -108.71351956568382
+- -5.745009893067267
+- 87.6675158159406
+- -296.0279182582153
+- -233.9195781182937
+- 150.14900502415634
+- 50.047713504120075
+- -65.13712699110657
+- -63.29650156639849
+- 146.30639158700484
+- -7.420242089908876
+- 124.38604030184669
+- -225.0918022855327
+- -11.955499092061057
+- -54.97338692339286
+- 50.32031575510728
+- 112.82062511013245
+- -129.72867213328516
+- 136.89085303094504
+- 131.93641239214193
+- 32.05205211041663
+- 154.59836057722606
+- 175.67604024475781
+- -44.92837657248064
+- 81.00467628120519
+- -74.62431053956641
+- 27.537976110327044
+- 42.12871767611807
+- 44.09646445293466
+- 34.3061140292123
+- -79.67991805057657
+- 361.0179974424243
+- 206.94658977362258
+- -62.57961916655771
+- 56.35284096585629
+- -37.90639366438754
+- -34.92683123949176
+- 175.92536801389954
+- -22.202732748075075
+- 157.76726874910136
+- 156.7588267025609
+- 90.1317401095603
+- 168.47447020413233
+- 83.5602166846247
+- 165.36594566744475
+- 45.16869272257553
+- 155.02180181871185
+- 14.638975583831082
+- 94.51397248320666
+- 10.23259861337732
+- -74.05775962174776
+- 138.5976404331386
+- 35.92974930158186
+- 214.45641325560595
+- 52.395356034875604
+- 181.97934360442878
+- 158.5100111613891
+- 281.54223381715923
+- -54.62059130154339
+- 138.26745602168143
+- 360.8153239584046
+- 132.6488916217121
+- 211.33798435015814
+- 177.43241192224525
+- 303.91377149100214
+- 335.3659015411149
+- 112.4052168895307
+- 211.45162497934768
+- 26.902440056531173
+- 12.824929259779719
+- 148.78153487538145
+- 348.51675617912366
+- 80.84786703617041
+- 284.36659295579847
+- 132.89099651423604
+- 47.59218118212255
+- 123.03706687396641
+- -31.555980494697533
+- -13.830137103484992
+- 361.01370506210765
+- 271.5910348078262
+- 130.29943901622258
+- 201.50305302411329
+- 107.94428248568532
+- 148.4194735001841
+- 13.640057642855442
+- -49.82500640498612
+- 51.723330602487785
+y:
+- 43.8689624517487
+- -169.6007983643741
+- 30.065101369218713
+- -213.80194456204015
+- -177.86818599720698
+- -89.47729250601859
+- -237.26126603270572
+- -28.90895178468717
+- -241.97378181205764
+- 28.751410237221332
+- -279.8852663973567
+- -34.97992192624139
+- -364.0200198040815
+- -146.56388627682088
+- -235.51843671259178
+- -67.9826828007947
+- -300.9679613175251
+- -266.01731940042015
+- -160.40686421128942
+- -31.917032713536337
+- -228.17916917278487
+- -67.52265418325968
+- -279.93543505386316
+- -99.28769208154215
+- -41.73964330325352
+- -14.03180281400123
+- -164.09225196361115
+- -95.27620682960934
+- -169.03212195117646
+- -243.22225481386909
+- -438.3874977112839
+- -137.38476119639145
+- 50.83280989998028
+- -385.52775451727695
+- -115.00777420022197
+- -137.58527756048275
+- -158.91692533593877
+- -176.8491552727091
+- -322.2574538030059
+- -137.64920778885318
+- -89.03812059637349
+- -216.73854792889284
+- -123.12538039931468
+- -222.69966954734372
+- -132.56932758571173
+- -204.80667799423975
+- -206.15111386654274
+- -8.105410990393208
+- 69.05680558576898
+- -217.54000016851208
+- -105.40398832162322
+- -168.66374801089185
+- -181.10928685990706
+- -162.38012010804493
+- -157.18749108736537
+- -497.1532400025114
+- -81.47159264942029
+- 40.60934477919244
+- 72.52028937802565
+- 211.11407870514296
+- 31.925044889424697
+- -15.448604613922663
+- -108.71351956568382
+- -5.745009893067267
+- 87.6675158159406
+- -296.0279182582153
+- -233.9195781182937
+- 150.14900502415634
+- 50.047713504120075
+- -65.13712699110657
+- -63.29650156639849
+- 146.30639158700484
+- -7.420242089908876
+- 124.38604030184669
+- -225.0918022855327
+- -11.955499092061057
+- -54.97338692339286
+- 50.32031575510728
+- 112.82062511013245
+- -129.72867213328516
+- 136.89085303094504
+- 131.93641239214193
+- 32.05205211041663
+- 154.59836057722606
+- 175.67604024475781
+- -44.92837657248064
+- 81.00467628120519
+- -74.62431053956641
+- 27.537976110327044
+- 42.12871767611807
+- 44.09646445293466
+- 34.3061140292123
+- -79.67991805057657
+- 361.0179974424243
+- 206.94658977362258
+- -62.57961916655771
+- 56.35284096585629
+- -37.90639366438754
+- -34.92683123949176
+- 175.92536801389954
+- -22.202732748075075
+- 157.76726874910136
+- 156.7588267025609
+- 90.1317401095603
+- 168.47447020413233
+- 83.5602166846247
+- 165.36594566744475
+- 45.16869272257553
+- 155.02180181871185
+- 14.638975583831082
+- 94.51397248320666
+- 10.23259861337732
+- -74.05775962174776
+- 138.5976404331386
+- 35.92974930158186
+- 214.45641325560595
+- 52.395356034875604
+- 181.97934360442878
+- 158.5100111613891
+- 281.54223381715923
+- -54.62059130154339
+- 138.26745602168143
+- 360.8153239584046
+- 132.6488916217121
+- 211.33798435015814
+- 177.43241192224525
+- 303.91377149100214
+- 335.3659015411149
+- 112.4052168895307
+- 211.45162497934768
+- 26.902440056531173
+- 12.824929259779719
+- 148.78153487538145
+- 348.51675617912366
+- 80.84786703617041
+- 284.36659295579847
+- 132.89099651423604
+- 47.59218118212255
+- 123.03706687396641
+- -31.555980494697533
+- -13.830137103484992
+- 361.01370506210765
+- 271.5910348078262
+- 130.29943901622258
+- 201.50305302411329
+- 107.94428248568532
+- 148.4194735001841
+- 13.640057642855442
+- -49.82500640498612
+- 51.723330602487785
+z:
+- 158.02213462044517
+- 124.21389942705694
+- 133.42160238652866
+- 158.42868516085107
+- -21.605406151859455
+- 32.169365188894844
+- -22.124665667361427
+- -284.3370553173801
+- -49.996420145893985
+- 45.18025799900077
+- 70.83272531068998
+- -30.190086637370335
+- 83.32096337971734
+- 167.53181703318776
+- -56.91344159005577
+- -16.880691170276958
+- -31.641226612394295
+- -63.77736018646692
+- 155.3404382904853
+- 53.1641961123058
+- -136.70884568378537
+- -142.14313622713573
+- 54.81400336265734
+- 166.03318193784835
+- 167.03672828780103
+- 12.758532063157585
+- 262.88270654379556
+- -110.29633686990788
+- 117.26884845255185
+- -61.95240388905076
+- 226.37842481589607
+- 222.09402805357576
+- 69.76039970545094
+- 136.86308681331556
+- 137.98500997477788
+- 247.44695640637488
+- 199.8026983217359
+- 314.96927404655963
+- 337.6905947836172
+- 133.27985509039476
+- 147.8521629286081
+- 66.47742441820357
+- 147.76351959903582
+- 190.29003926246511
+- 27.071471212349145
+- -76.8549182147727
+- 286.69501455745836
+- 428.2931492491618
+- 455.61401541707056
+- 521.6731182773849
+- 109.90737231406615
+- 194.39719341953102
+- 339.82207247025315
+- 396.69340854103774
+- 329.3215784934387
+- 299.6469290240797
+- 376.28771103406586
+- 322.5336318152405
+- 525.1208758110637
+- 359.3174665937067
+- 398.69234294972256
+- 482.49576100907564
+- 415.0046037146592
+- 297.69696975229544
+- 361.95274697190786
+- 86.12408361100988
+- 248.16979285039596
+- 294.56545489386446
+- 195.02181086263064
+- 173.81987631166245
+- 485.50692106767747
+- 347.0394720125073
+- 465.59691510544513
+- 340.82681953920854
+- 147.1759691610624
+- 540.8715496577842
+- 577.5678960225167
+- 366.936611212867
+- 202.8999194336259
+- 300.499562679048
+- 297.7028917520312
+- 313.89302244023196
+- 334.60008641069373
+- 107.74468204194072
+- 235.0327715466816
+- 549.289811008155
+- 271.174000270851
+- 294.943792117891
+- 158.55887022795858
+- 160.42922276113836
+- 238.40341330861514
+- 134.30051227200818
+- 20.276647871798527
+- 79.73564021369596
+- 108.04656684276075
+- 96.98113557799664
+- -9.613618858526026
+- 231.7294599200759
+- 286.24011800776145
+- 258.9524745272242
+- 138.96205439286143
+- 230.5178459277302
+- 129.21981541108664
+- 34.94829644138465
+- -47.36503948695072
+- 258.47393124139313
+- 288.0921191562058
+- 99.35596410276497
+- 199.08849267441343
+- 5.962739108937392
+- 57.277582923395016
+- 198.9737245497959
+- -29.250459745576855
+- -35.371226355851526
+- -50.93861828903449
+- -0.5622942134144645
+- 51.2659317519989
+- 163.80067980518817
+- 100.69134696855404
+- 41.28379929101864
+- 44.394393991420785
+- 74.10606750833206
+- 210.39197419780737
+- 257.7009656039906
+- 143.52042324845172
+- 157.95281778355042
+- 371.0355274713962
+- 117.10681489657816
+- 183.5689087876269
+- 113.25094688461957
+- 321.50019921313026
+- 135.28688690325083
+- 109.65516438008626
+- 309.8506415075863
+- 297.8014922160356
+- -125.94596534196043
+- 227.3820448805529
+- 81.85741963127256
+- 168.77238219848283
+- 106.49759132079056
+- 77.40044523712592
+- 278.81113256155186
+- 186.02603672590124
+- 29.11334048494735
+- 74.6223815549481
+- 69.06428438760543
+- -26.82167216354576
+- -28.709326664936647
+- 69.74118428651663
+- -7.377070293044171
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
new file mode 100644
index 000000000..9ebb5f888
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
@@ -0,0 +1,828 @@
+x:
+- 13.39606185518066
+- -185.77131889079996
+- 35.37071152368431
+- -238.33581259116022
+- -209.485456857886
+- -55.79424251336636
+- -203.48831227572782
+- 71.17415168627254
+- -203.89976482996698
+- 25.82843127330604
+- -344.8289492809679
+- 0.7396637011390683
+- -406.3517796771794
+- -140.14958411026115
+- -253.2784846338339
+- -89.4695657638801
+- -313.6193358954695
+- -273.67964845298616
+- -179.25497919895878
+- -34.70129933746196
+- -241.90715654839505
+- -70.414531588787
+- -227.03494014908614
+- -95.726181081603
+- 18.55527337904843
+- -29.096694542505944
+- -171.32725286555382
+- -131.59983508798598
+- -157.00240735570648
+- -307.38552573316286
+- -391.40719534634144
+- -186.55425884722115
+- 80.19618382385484
+- -329.5915357772583
+- -123.96234879435323
+- -86.59040928872122
+- -176.54315397023404
+- -191.52676703031264
+- -343.8517139043967
+- -148.5392791800401
+- -113.67410843602012
+- -207.13896751744682
+- -166.37217013374718
+- -113.35662820177623
+- -171.00549088697548
+- -128.1904266174808
+- -244.75285075051545
+- 77.00339782891587
+- 135.87381293805407
+- -202.6767982233658
+- -126.30575123501822
+- -133.67849552337822
+- -125.66589466145376
+- -89.21720646974265
+- -110.08200009394923
+- -451.02020271503693
+- 2.4608108953777883
+- 88.18246273501963
+- 28.97302831035289
+- 240.13715185004833
+- -8.089703687686857
+- 39.6200269721853
+- -97.23390475854785
+- 10.70792354595319
+- 146.76906119855437
+- -320.7041669573032
+- -253.55220004603777
+- 198.50134652444424
+- 87.72819904451993
+- -69.31320776244817
+- -31.664250514915587
+- 180.21630369764446
+- 75.80680312281325
+- 113.0359055937637
+- -220.7247671092758
+- 30.49331619370011
+- -102.95254611266019
+- 91.182547452693
+- 177.0561337434178
+- -119.55169007918882
+- 126.3644722688547
+- 153.02363294963416
+- 10.700876785300418
+- 183.29979845044622
+- 182.86214231824036
+- -156.39408428738193
+- 124.5400760522099
+- -97.3165530624101
+- -75.75500454941047
+- 76.68290998800416
+- -59.53754933942954
+- 8.387185266556799
+- -14.45270100464297
+- 320.5305268382154
+- 134.58334318707833
+- -87.5137987569588
+- 28.885164898031046
+- 16.52719421062019
+- -45.035037893768106
+- 237.59895122333054
+- -70.91821757868601
+- 294.075020930127
+- 153.16129833391807
+- 100.57053722722668
+- 184.70353169165628
+- 49.408515662563396
+- 150.47800742156033
+- 83.48768583280237
+- 162.1178652240055
+- 36.77184601893627
+- 208.92863413503272
+- 100.87312831753347
+- -88.79815741139984
+- 96.32376985485261
+- -19.58202503760524
+- 198.37635894008744
+- 113.1929987433259
+- 109.42218789892819
+- 130.4791134177164
+- 392.1148842499555
+- -63.12794264557571
+- 168.82435692250974
+- 367.4348713729929
+- 163.06041388007085
+- 233.01229290232402
+- 87.09470597271827
+- 294.85281045407805
+- 310.28128101536043
+- 175.15864418463534
+- 174.3715982191862
+- 43.73595372815047
+- 0.8848266248074792
+- 138.40732060185906
+- 375.75576077195507
+- 36.69180342233255
+- 303.4734511537755
+- 97.76769493399965
+- -8.694307859357673
+- 145.51452265097544
+- -55.31044492807325
+- -25.94291946789259
+- 363.3540785961889
+- 282.26042553580754
+- 102.68242734509543
+- 192.5966790291269
+- 52.874490198664375
+- 169.05476835306834
+- -23.072692813108578
+- -39.7920324654743
+- 73.94050255334568
+y:
+- 13.39606185518066
+- -185.77131889079996
+- 35.37071152368431
+- -238.33581259116022
+- -209.485456857886
+- -55.79424251336636
+- -203.48831227572782
+- 71.17415168627254
+- -203.89976482996698
+- 25.82843127330604
+- -344.8289492809679
+- 0.7396637011390683
+- -406.3517796771794
+- -140.14958411026115
+- -253.2784846338339
+- -89.4695657638801
+- -313.6193358954695
+- -273.67964845298616
+- -179.25497919895878
+- -34.70129933746196
+- -241.90715654839505
+- -70.414531588787
+- -227.03494014908614
+- -95.726181081603
+- 18.55527337904843
+- -29.096694542505944
+- -171.32725286555382
+- -131.59983508798598
+- -157.00240735570648
+- -307.38552573316286
+- -391.40719534634144
+- -186.55425884722115
+- 80.19618382385484
+- -329.5915357772583
+- -123.96234879435323
+- -86.59040928872122
+- -176.54315397023404
+- -191.52676703031264
+- -343.8517139043967
+- -148.5392791800401
+- -113.67410843602012
+- -207.13896751744682
+- -166.37217013374718
+- -113.35662820177623
+- -171.00549088697548
+- -128.1904266174808
+- -244.75285075051545
+- 77.00339782891587
+- 135.87381293805407
+- -202.6767982233658
+- -126.30575123501822
+- -133.67849552337822
+- -125.66589466145376
+- -89.21720646974265
+- -110.08200009394923
+- -451.02020271503693
+- 2.4608108953777883
+- 88.18246273501963
+- 28.97302831035289
+- 240.13715185004833
+- -8.089703687686857
+- 39.6200269721853
+- -97.23390475854785
+- 10.70792354595319
+- 146.76906119855437
+- -320.7041669573032
+- -253.55220004603777
+- 198.50134652444424
+- 87.72819904451993
+- -69.31320776244817
+- -31.664250514915587
+- 180.21630369764446
+- 75.80680312281325
+- 113.0359055937637
+- -220.7247671092758
+- 30.49331619370011
+- -102.95254611266019
+- 91.182547452693
+- 177.0561337434178
+- -119.55169007918882
+- 126.3644722688547
+- 153.02363294963416
+- 10.700876785300418
+- 183.29979845044622
+- 182.86214231824036
+- -156.39408428738193
+- 124.5400760522099
+- -97.3165530624101
+- -75.75500454941047
+- 76.68290998800416
+- -59.53754933942954
+- 8.387185266556799
+- -14.45270100464297
+- 320.5305268382154
+- 134.58334318707833
+- -87.5137987569588
+- 28.885164898031046
+- 16.52719421062019
+- -45.035037893768106
+- 237.59895122333054
+- -70.91821757868601
+- 294.075020930127
+- 153.16129833391807
+- 100.57053722722668
+- 184.70353169165628
+- 49.408515662563396
+- 150.47800742156033
+- 83.48768583280237
+- 162.1178652240055
+- 36.77184601893627
+- 208.92863413503272
+- 100.87312831753347
+- -88.79815741139984
+- 96.32376985485261
+- -19.58202503760524
+- 198.37635894008744
+- 113.1929987433259
+- 109.42218789892819
+- 130.4791134177164
+- 392.1148842499555
+- -63.12794264557571
+- 168.82435692250974
+- 367.4348713729929
+- 163.06041388007085
+- 233.01229290232402
+- 87.09470597271827
+- 294.85281045407805
+- 310.28128101536043
+- 175.15864418463534
+- 174.3715982191862
+- 43.73595372815047
+- 0.8848266248074792
+- 138.40732060185906
+- 375.75576077195507
+- 36.69180342233255
+- 303.4734511537755
+- 97.76769493399965
+- -8.694307859357673
+- 145.51452265097544
+- -55.31044492807325
+- -25.94291946789259
+- 363.3540785961889
+- 282.26042553580754
+- 102.68242734509543
+- 192.5966790291269
+- 52.874490198664375
+- 169.05476835306834
+- -23.072692813108578
+- -39.7920324654743
+- 73.94050255334568
+z:
+- 184.13062106336886
+- 99.07732187166194
+- 92.16064002703182
+- 170.20612911811745
+- -84.43007211958228
+- 49.326375552915486
+- -58.90448082146792
+- -286.6749079132932
+- -169.5203211093247
+- 92.26845629080721
+- 90.54236668213439
+- -100.48691417371096
+- 34.39504173684831
+- 156.50553830020033
+- -48.05833218614462
+- -109.70113484604624
+- -19.190001510571605
+- -139.51884465567588
+- 65.6557609270006
+- 72.64609292231738
+- -181.26270622778387
+- -115.20104601970644
+- 63.70208679136542
+- 100.28907460903238
+- 93.81039926391475
+- -44.27202177083973
+- 272.9809883329726
+- -118.4082328800006
+- 14.567811861905994
+- -17.12309916592073
+- 199.51332283596312
+- 232.04330446108344
+- 55.3528289392478
+- 147.90455344630212
+- 173.84681195652854
+- 327.76301232162473
+- 137.19438718978904
+- 285.5109909061588
+- 354.41128257135273
+- 181.00092216807
+- 140.25184405267868
+- 90.52373293898779
+- 206.94338506653614
+- 178.30006046978937
+- 5.60386149608939
+- -23.89247478956175
+- 217.2071422488589
+- 372.99420617698996
+- 471.476215026768
+- 533.5262578721763
+- 95.53744059930627
+- 174.44050688868268
+- 372.7593642618201
+- 305.26638491173316
+- 317.6270874889623
+- 317.3883993528826
+- 415.3614385179128
+- 329.7579220583091
+- 597.4991485093107
+- 314.21773874862384
+- 422.2021190771538
+- 546.886375226279
+- 411.24799173892825
+- 381.13148134887473
+- 421.9071191701232
+- 54.28483516177912
+- 336.46480603837585
+- 320.8816221384955
+- 312.1455116382477
+- 185.07367239229572
+- 474.86758319124124
+- 360.84829553534115
+- 530.5403233062419
+- 360.0826728482063
+- 145.72401960555976
+- 550.4994357950865
+- 601.8909586524885
+- 434.9611248474422
+- 246.50077149854124
+- 348.1617378133813
+- 294.31919476742814
+- 351.81854928413077
+- 341.1966749204272
+- 149.5873858459451
+- 243.1087448411399
+- 679.9790266706096
+- 385.83746726161746
+- 356.1223224013243
+- 210.61593183364107
+- 194.0944359350073
+- 256.4763346246082
+- 132.85570845808365
+- 85.92110850935644
+- 76.88579270103827
+- 135.18079715969833
+- 115.60023399011929
+- -27.216912731239283
+- 276.33233940002117
+- 383.73501899587893
+- 271.53468064266326
+- 162.32073042244056
+- 274.8706127650842
+- 109.592443856955
+- 78.9278848517801
+- -60.56519989930266
+- 219.47369396409044
+- 276.7216524444765
+- 48.290103072556455
+- 202.89700035063984
+- 4.0557154628067735
+- 15.35719385836599
+- 234.886589735482
+- -77.35846083525735
+- -27.54113407768439
+- -0.8817734533684713
+- -44.67572207102486
+- -33.625428385975184
+- 131.79676520283965
+- 97.55883912629476
+- 101.76252079547383
+- 22.140324965741428
+- -35.06076817717431
+- 121.69090612401318
+- 376.59048718650297
+- 191.14040377065936
+- 175.21901739383418
+- 373.745620916645
+- 115.44689673508924
+- 204.04404584493037
+- 66.80907114716828
+- 322.2963889404261
+- 138.81012838733625
+- 116.96043892462627
+- 308.1997567836865
+- 392.2073428815965
+- -113.55524936225333
+- 142.64612976677094
+- 31.02434670792769
+- 160.91159393584076
+- 108.8701736903117
+- 55.02928418317193
+- 282.18275687517723
+- 164.92899096339048
+- 7.043732674359255
+- 110.52984547755112
+- 152.22833453858942
+- -77.64820084519434
+- 46.951435542123164
+- 17.079852248988303
+- 8.823259483674619
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
new file mode 100644
index 000000000..f09cc1c45
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
@@ -0,0 +1,828 @@
+x:
+- -29.375551359078376
+- -90.47443500639149
+- -91.71188550404756
+- -189.32305379289966
+- -235.83766720876812
+- -169.72239313636325
+- -149.43256088322323
+- -80.20600645446291
+- -118.55368553718105
+- 42.81215178550607
+- -265.9957636585939
+- -141.95701797617795
+- -266.46167767655487
+- -204.51014475871997
+- -189.80222697311726
+- -38.05749645329762
+- -223.70928226903334
+- -288.62220421304863
+- -220.73144672582546
+- -166.93529723196053
+- -87.8046234522134
+- -122.26709774619059
+- -208.24287361309038
+- -236.1220882073268
+- -122.64068051628453
+- -75.01958163184389
+- -204.96615380578407
+- -23.09215254192919
+- -191.92413980451423
+- -175.38877684876053
+- -280.78418583823486
+- -144.7014390367135
+- -103.03003029337698
+- -299.44372380872426
+- -102.19985354546496
+- -205.1251444690762
+- -209.604136417223
+- -271.64598338749227
+- -349.0777046931662
+- -140.45821201826254
+- -132.12210271681744
+- -89.04875206339462
+- -162.95490217138203
+- -209.54656911129615
+- -237.14361983118667
+- -266.20685193865637
+- -112.69616837548827
+- -168.68025744837794
+- -82.49433733438627
+- -115.23856334171428
+- -225.20573825262352
+- -177.25605785639775
+- -272.9606105692432
+- -209.58804425623748
+- -89.74444994088834
+- -422.6129708309914
+- -149.80385648598852
+- -76.34902279256217
+- -35.59435466166301
+- -28.379183376609024
+- -26.040840385426367
+- -23.411215764621712
+- -246.28832759393669
+- -149.42109564818247
+- 19.638873302406513
+- -247.7719621044008
+- -148.67588307949012
+- -6.819115922872016
+- -42.10929537610118
+- -86.51608561145373
+- 38.745190729243845
+- 73.44615661851579
+- -43.57206718249474
+- 59.22210318409091
+- -120.49920562371088
+- -72.20181047344533
+- -108.08988417914402
+- -75.7450387838097
+- -21.076801297481534
+- -132.65414296232717
+- 48.21738504635631
+- -12.08739295872503
+- -16.230166084301146
+- 9.739221457638676
+- 74.15950234946249
+- -43.841233195862
+- 80.17355341740009
+- -106.34374023750392
+- -21.883107257731865
+- -97.08353131548583
+- 41.03629608726767
+- -101.10630613756221
+- -83.942009736159
+- 192.5212933831941
+- 104.57687178131494
+- -35.98987118013184
+- 17.173769075202546
+- -120.23176589621691
+- -60.84712779673432
+- 1.9327942318091853
+- 29.001885322844586
+- 125.08479299949121
+- 82.45353895679158
+- -15.42840105515149
+- 27.276169375364905
+- 30.07990368176639
+- 172.91643634668338
+- -7.482340203943173
+- 157.59112514236313
+- -137.13366773892676
+- 95.53954085681873
+- 19.49350757622742
+- -57.713229800924424
+- 83.02661866355157
+- 85.08031959692558
+- 157.06008111632303
+- 109.24421484963756
+- 159.04732972517277
+- 90.46862148072736
+- 247.73417368056147
+- -32.26406535057381
+- 142.23886194195728
+- 182.27466879228888
+- 27.461550877478768
+- 99.02342386475823
+- 110.61768010646794
+- 192.14809905399466
+- 196.1612891742741
+- 65.68248160996461
+- 185.67802983123008
+- 99.19332389347727
+- 87.21381730414205
+- 150.52999560895714
+- 316.6079779476388
+- 248.3318997862404
+- 108.64439049194944
+- 69.89827365386222
+- -6.690611980882549
+- 26.166971113579585
+- 115.72582217280392
+- -6.327346694173989
+- 238.32953086103095
+- 293.5383988954415
+- 181.85731011937267
+- 66.33790674489546
+- 112.88452797859479
+- 14.829331008214357
+- 73.80993361645798
+- -58.840870314538606
+- 92.69210473123687
+y:
+- -29.375551359078376
+- -90.47443500639149
+- -91.71188550404756
+- -189.32305379289966
+- -235.83766720876812
+- -169.72239313636325
+- -149.43256088322323
+- -80.20600645446291
+- -118.55368553718105
+- 42.81215178550607
+- -265.9957636585939
+- -141.95701797617795
+- -266.46167767655487
+- -204.51014475871997
+- -189.80222697311726
+- -38.05749645329762
+- -223.70928226903334
+- -288.62220421304863
+- -220.73144672582546
+- -166.93529723196053
+- -87.8046234522134
+- -122.26709774619059
+- -208.24287361309038
+- -236.1220882073268
+- -122.64068051628453
+- -75.01958163184389
+- -204.96615380578407
+- -23.09215254192919
+- -191.92413980451423
+- -175.38877684876053
+- -280.78418583823486
+- -144.7014390367135
+- -103.03003029337698
+- -299.44372380872426
+- -102.19985354546496
+- -205.1251444690762
+- -209.604136417223
+- -271.64598338749227
+- -349.0777046931662
+- -140.45821201826254
+- -132.12210271681744
+- -89.04875206339462
+- -162.95490217138203
+- -209.54656911129615
+- -237.14361983118667
+- -266.20685193865637
+- -112.69616837548827
+- -168.68025744837794
+- -82.49433733438627
+- -115.23856334171428
+- -225.20573825262352
+- -177.25605785639775
+- -272.9606105692432
+- -209.58804425623748
+- -89.74444994088834
+- -422.6129708309914
+- -149.80385648598852
+- -76.34902279256217
+- -35.59435466166301
+- -28.379183376609024
+- -26.040840385426367
+- -23.411215764621712
+- -246.28832759393669
+- -149.42109564818247
+- 19.638873302406513
+- -247.7719621044008
+- -148.67588307949012
+- -6.819115922872016
+- -42.10929537610118
+- -86.51608561145373
+- 38.745190729243845
+- 73.44615661851579
+- -43.57206718249474
+- 59.22210318409091
+- -120.49920562371088
+- -72.20181047344533
+- -108.08988417914402
+- -75.7450387838097
+- -21.076801297481534
+- -132.65414296232717
+- 48.21738504635631
+- -12.08739295872503
+- -16.230166084301146
+- 9.739221457638676
+- 74.15950234946249
+- -43.841233195862
+- 80.17355341740009
+- -106.34374023750392
+- -21.883107257731865
+- -97.08353131548583
+- 41.03629608726767
+- -101.10630613756221
+- -83.942009736159
+- 192.5212933831941
+- 104.57687178131494
+- -35.98987118013184
+- 17.173769075202546
+- -120.23176589621691
+- -60.84712779673432
+- 1.9327942318091853
+- 29.001885322844586
+- 125.08479299949121
+- 82.45353895679158
+- -15.42840105515149
+- 27.276169375364905
+- 30.07990368176639
+- 172.91643634668338
+- -7.482340203943173
+- 157.59112514236313
+- -137.13366773892676
+- 95.53954085681873
+- 19.49350757622742
+- -57.713229800924424
+- 83.02661866355157
+- 85.08031959692558
+- 157.06008111632303
+- 109.24421484963756
+- 159.04732972517277
+- 90.46862148072736
+- 247.73417368056147
+- -32.26406535057381
+- 142.23886194195728
+- 182.27466879228888
+- 27.461550877478768
+- 99.02342386475823
+- 110.61768010646794
+- 192.14809905399466
+- 196.1612891742741
+- 65.68248160996461
+- 185.67802983123008
+- 99.19332389347727
+- 87.21381730414205
+- 150.52999560895714
+- 316.6079779476388
+- 248.3318997862404
+- 108.64439049194944
+- 69.89827365386222
+- -6.690611980882549
+- 26.166971113579585
+- 115.72582217280392
+- -6.327346694173989
+- 238.32953086103095
+- 293.5383988954415
+- 181.85731011937267
+- 66.33790674489546
+- 112.88452797859479
+- 14.829331008214357
+- 73.80993361645798
+- -58.840870314538606
+- 92.69210473123687
+z:
+- 211.2449638813512
+- 144.12643508434022
+- 70.65084772360636
+- 266.22259058351193
+- 79.58098722163406
+- 67.41137999087195
+- 53.22696167140256
+- -155.62230486888396
+- -20.637501526335246
+- 47.29218781689232
+- 137.74499579919055
+- 29.737631923863358
+- 22.265927207340653
+- 134.96071582169628
+- 12.867836720057458
+- 31.746080143319816
+- -61.25340292907621
+- 16.095025201104544
+- 146.4098832489993
+- 40.848615079904334
+- -50.14723555612424
+- -138.43221895490484
+- 153.59666936708396
+- 204.93592122102345
+- 148.309348728097
+- 132.40599809620417
+- 96.32049687526244
+- 14.391642011065146
+- 126.11541675235092
+- 98.49868770999424
+- 227.42842519198672
+- 128.45790771023644
+- 111.23880059118348
+- 137.71166622530953
+- 202.20346380741577
+- 74.30635938312481
+- 211.12787550053028
+- 179.57731430777284
+- 232.0547769473891
+- 106.31587572443996
+- 56.91595541471525
+- 164.6658069280793
+- 105.45001283843736
+- 121.9916460272607
+- 166.56172537885004
+- 43.101383902483924
+- 177.9179920479384
+- 350.4344815921115
+- 273.82271685178284
+- 345.753927164644
+- 205.74782564547337
+- 95.8509054019219
+- 349.4294217304583
+- 292.26509190007056
+- 202.08072722544105
+- 279.18674353150703
+- 307.05731255330056
+- 256.0348143653611
+- 352.8725202764028
+- 342.3067330714769
+- 329.85645000425404
+- 331.2448078046214
+- 261.8592784241704
+- 223.12260579613678
+- 229.7406559910389
+- 106.71386980499686
+- 225.70066674342786
+- 232.960637316935
+- 125.3723581420407
+- 190.07607611067053
+- 384.80580145715317
+- 211.87101947223573
+- 388.5322145790263
+- 251.75754271449767
+- 163.80291231609192
+- 403.4985442367527
+- 407.9914837009331
+- 348.4985939500316
+- 217.94056440429313
+- 186.13163981629907
+- 214.8689491665111
+- 187.73133261536157
+- 152.4888749587075
+- 129.9939226790654
+- 185.17672711886533
+- 420.48693538484144
+- 210.68106693570417
+- 216.2355938232026
+- 151.18380118094063
+- 177.5903345689017
+- 118.42270264251937
+- 134.75396155303804
+- 119.2231629624467
+- 120.5135786586908
+- 99.0081517922155
+- 44.340956839597816
+- 88.5756716853682
+- 146.37757087986753
+- 191.7854607520014
+- 116.84831300929238
+- 102.1535340161195
+- 183.49767166335988
+- 110.26972529822831
+- 31.855645025076026
+- 104.47232402972293
+- 105.16661554418253
+- 186.7668231442927
+- 107.25810511264469
+- 149.91517473659937
+- 67.54777348159766
+- 112.98835500793635
+- 160.24636088290956
+- -9.135489008559233
+- 76.2158660273898
+- 75.39618399235141
+- 115.81860406030827
+- -94.71479356756392
+- 180.49611085352274
+- 124.42826449811837
+- -52.797664491975596
+- 77.4224115843028
+- 99.21542756265629
+- 104.7404776249072
+- 163.9225564943106
+- 128.76984457451476
+- 164.02881762648596
+- 222.10664278936633
+- 54.19271402906178
+- 206.07507379901463
+- 113.56828585758434
+- 297.4088031293355
+- 138.71506972040604
+- 11.133786666699164
+- 245.97664524914433
+- 254.27210332312245
+- -14.460600520764679
+- 104.53854381236779
+- 82.05872014286024
+- 42.73723160438324
+- 29.0544071318862
+- 36.90402358428645
+- 161.02262155535433
+- 120.86832719606659
+- -16.647004317176723
+- 134.56200411183218
+- 80.3362373679378
+- -70.14863660353376
+- 38.133373393847464
+- 133.55911624440668
+- -75.3184987249469
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/package.xml
new file mode 100644
index 000000000..d4d3d59cf
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_hand_over_action</name>
+  <version>1.0.0</version>
+  <description>An action for handing objects over</description>
+
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>genmsg</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <export></export>
+</package>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
new file mode 100644
index 000000000..3c74c4951
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
@@ -0,0 +1,10 @@
+# goal
+string posture_type
+bool obstacle
+---
+# result
+bool success
+---
+# feedback
+string current_state
+string message
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
new file mode 100644
index 000000000..7affc7937
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="mdr_hand_over_action" type="hand_over_action" name="hand_over_server" output="screen">
+        <param name="move_arm_server" value="move_arm_server" />
+        <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
+        <param name="init_config_name" value="neutral" />
+        <param name="hand_over_dmp" value="$(find mdr_hand_over_action)/config/trajectory_weights/grasp.yaml" />
+        <param name="dmp_tau" value="30" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
new file mode 100755
index 000000000..2dbc032cd
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+
+from mdr_hand_over_action.msg import HandOverAction
+from mdr_hand_over_action.action_states import HandOverSM
+
+class HandOverServer(object):
+    '''A server exposing an action for handing objects over.
+
+    The server expects the following parameters to be made available on the ROS parameter server:
+    '''
+    def __init__(self):
+        gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mas_hsr_gripper_controller')
+        move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
+        init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
+        hand_over_dmp = rospy.get_param('~hand_over_dmp', 'hand_over_dmp')
+        dmp_tau = rospy.get_param('~dmp_tau', 'dmp_tau')
+
+        rospy.loginfo('[hand_over] Initialising state machine')
+        self.action_sm = HandOverSM(gripper_controller_pkg_name=gripper_controller_pkg_name,
+                                      move_arm_server=move_arm_server,
+                                      init_config_name=init_config_name,
+                                      hand_over_dmp=hand_over_dmp,
+                                      dmp_tau=dmp_tau)
+        rospy.loginfo('[hand_over] State machine initialised')
+
+        self.action_server = actionlib.SimpleActionServer('hand_over_server',
+                                                          HandOverAction,
+                                                          self.execute, False)
+        self.action_server.start()
+        rospy.loginfo('hand_over action server ready')
+
+    def execute(self, goal):
+        rospy.loginfo('[hand_over] Received an action request')
+        self.action_sm.goal = goal
+        self.action_sm.result = None
+        self.action_sm.execution_requested = True
+        while not self.action_sm.result:
+            rospy.sleep(0.05)
+        self.action_server.set_succeeded(self.action_sm.result)
+
+if __name__ == '__main__':
+    rospy.init_node('hand_over_server')
+    hand_over_server = HandOverServer()
+    try:
+        hand_over_server.action_sm.run()
+        while hand_over_server.action_sm.is_running and not rospy.is_shutdown():
+            rospy.spin()
+    except (KeyboardInterrupt, SystemExit):
+        print('{0} interrupted; exiting...'.format(hand_over_server.action_sm.name))
+        hand_over_server.action_sm.stop()
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
new file mode 100755
index 000000000..043791aef
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
@@ -0,0 +1,47 @@
+#! /usr/bin/env python
+from __future__ import print_function
+import argparse
+import rospy
+import actionlib
+import numpy as np
+
+from mdr_hand_over_action.msg import HandOverAction, HandOverGoal
+
+if __name__ == '__main__':
+    posture_contexts = ['standing', 'sitting', 'lying_down']
+    obstacle_contexts = [0, 1]                                 # 0: no obstacle, 1: obstacle
+    
+    parser = argparse.ArgumentParser(description='help text')
+    parser.add_argument("--context", "-c", help="list of context parameters. E.g.: ['sitting', 0]") 
+    args = parser.parse_args()
+
+    if not args.context:
+        print('No context parameters provided!')
+        print('Example inputs: ')
+        print('- python hand_over_action_client_test -c "[\'standing\', 0]"')
+        print('- python hand_over_action_client_test -c "[\'sitting\', 1]"')
+    elif len(args.context) != 2:
+        print('Expected two context parameters: posture and obstacle!')
+    elif args.context[0] not in posture_contexts:
+        print('Invalid posture context!')
+    elif args.context[1] not in obstacle_contexts:
+        print('Invalid obstacle context!')
+    else:
+        posture_context = args.context[0]
+        obstacle_context = args.context[1]
+
+        rospy.init_node('hand_over_action_client_test')
+
+        client = actionlib.SimpleActionClient('/hand_over_server', HandOverAction)
+        client.wait_for_server()
+
+        goal = HandOverGoal()
+
+        goal.posture_type = args.context[0]
+        goal.obstacle = args.context[1]
+
+        timeout = 15.0
+        rospy.loginfo('Sending action lib goal to hand_over_server')
+        client.send_goal(goal)
+        client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
+        print(client.get_result())
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/__init__.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
new file mode 100644
index 000000000..0ec007ff5
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -0,0 +1,189 @@
+#!/usr/bin/python
+import pickle
+import numpy as np
+
+import rospy
+import tf
+import actionlib
+from geometry_msgs.msg import PoseStamped
+
+from pyftsm.ftsm import FTSMTransitions
+from mas_execution.action_sm_base import ActionSMBase
+from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
+from mdr_hand_over_action.msg import HandOverGoal, HandOverResult
+
+from mdr_move_arm_action.dmp import DMPExecutor
+
+from importlib import import_module
+
+class HandOverSM(ActionSMBase):
+    def __init__(self, timeout=120.0,
+                 gripper_controller_pkg_name='mas_hsr_gripper_controller',
+                 move_arm_server='move_arm_server',
+                 init_config_name = 'neutral',
+                 hand_over_position_policy_parameters_file = '$(find mdr_hand_over_action)/config/learned_policy_params/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl',
+                 hand_over_dmp = '$(find mdr_hand_over_action)/config/trajectory_weights/grasp.yaml',
+                 dmp_tau = 30.,
+                 max_recovery_attempts=1):
+        super(HandOverSM, self).__init__(
+            'HandOver', [], max_recovery_attempts)
+        self.timeout = timeout
+
+        gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name)
+        GripperControllerClass = getattr(import_module(gripper_controller_module_name), 'GripperController')
+        self.gripper = GripperControllerClass()
+
+        self.move_arm_server = move_arm_server
+
+        self.init_config_name = init_config_name
+
+        self.hand_over_dmp = hand_over_dmp
+        self.dmp_tau = dmp_tau
+        # self.hand_over_position_policy_parameters_file = hand_over_position_policy_parameters_file
+        # self.hand_over_position_policy_parameters_file = 'contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl'
+        self.hand_over_position_policy_parameters_file = 'contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl'
+        self.hand_over_position_policy_parameters = self.load_policy_params_from_file(hand_over_position_policy_parameters_file)
+
+        self.tf_listener = tf.TransformListener()
+
+    def init(self):
+        try:
+            self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction)
+            rospy.loginfo('[hand_over] Waiting for %s server', self.move_arm_server)
+            self.move_arm_client.wait_for_server()
+        except Exception as exc:
+            rospy.logerr('[hand_over] %s', str(exc))
+            return FTSMTransitions.INIT_FAILED
+
+        return FTSMTransitions.INITIALISED
+
+    def running(self):
+        posture_type = self.goal.posture_type
+        obstacle = self.goal.obstacle
+
+        # > Pick context-dependent hand-over position:
+        policy_parameter_a = self.learned_position_policy_parameters[0][0]
+        policy_parameter_A = self.learned_position_policy_parameters[0][1]
+
+        if posture_type == 'standing':
+            context_vector = np.array([0.0, 0.0, 0.4])
+        elif posture_type == 'sitting':
+            context_vector = np.array([0.5, 0.0, 0.0])
+        elif posture_type == 'lying_down':
+            context_vector = np.array([0.0, 0.7, 0.0])
+        
+        # Sample upper-level policy (with no exploration) for hand-over position;
+        # by calculating the mean of the linear-Gaussian model, given context vector s:
+        hand_over_position = np.squeeze(policy_parameter_a + context_vector.dot(policy_parameter_A))
+
+        hand_over_pose = PoseStamped()
+        hand_over_pose.header.frame_id = 'base_link'
+        hand_over_pose.header.stamp = rospy.Time.now()
+
+        hand_over_pose.pose.position.x = hand_over_position[0]
+        hand_over_pose.pose.position.y = hand_over_position[1]
+        hand_over_pose.pose.position.z = hand_over_position[2]
+
+        hand_over_pose.pose.orientation.x = 0.000
+        hand_over_pose.pose.orientation.y = 0.000
+        hand_over_pose.pose.orientation.z = 0.000
+        hand_over_pose.pose.orientation.w = 1.000
+
+        pose_base_link = self.tf_listener.transformPose('base_link', hand_over_pose)
+
+        ## TODO:
+        # > Pick context_dependent hand-over trajectory shape:
+        if not obstacle:
+            trajectory_weights_filename = 'grasp.yaml'
+        else:
+            ## Initial learned trajectory:
+            # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml'
+            
+            ## Smoothed trajectories:
+            trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2900iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2100iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_1600iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_800iters.yaml'
+
+            ## Smoothed trajectories, with corrected end_points:
+            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml'
+
+            ## Underhand trajectories:
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_7700iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_6900iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_6100iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_5300iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_4500iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_3700iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_2900iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_2100iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_1600iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_800iters.yaml'
+
+
+        self.hand_over_dmp = '$(find mdr_hand_over_action)/config/trajectory_weights/'+trajectory_weights_filename
+
+        ## TODO: determine whether aligning base is necessary for hand_over action here:
+        ## ...
+
+        # Initialize by grasping a provided object:
+        rospy.loginfo('[hand_over] Closing the gripper')
+        self.gripper.close() 
+
+        # Move to chosen hand_over position, along appropriate trajectory:
+        rospy.loginfo('[hand_over] Handing object over...')
+        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
+
+        # Wait for person to hold object:
+        rospy.loginfo('[hand_over] Waiting before releasing object...')
+        rospy.sleep(5.)
+
+        # Release the object:
+        rospy.loginfo('[hand_over] Opening the gripper...')
+        self.gripper.open()
+
+        # Return to a neutral arm position:
+        rospy.loginfo('[hand_over] Moving back to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
+
+        # For now, assume unconditional success:
+        self.result = self.set_result(True)
+        return FTSMTransitions.DONE
+
+    def __move_arm(self, goal_type, goal):
+        '''Sends a request to the 'move_arm' action server and waits for the
+        results of the action execution.
+        Keyword arguments:
+        goal_type -- 'MoveArmGoal.NAMED_TARGET' or 'MoveArmGoal.END_EFFECTOR_POSE'
+        goal -- A string if 'goal_type' is 'MoveArmGoal.NAMED_TARGET';
+                a 'geometry_msgs/PoseStamped' if 'goal_type' is 'MoveArmGoal.END_EFFECTOR_POSE'
+        '''
+        move_arm_goal = MoveArmGoal()
+        move_arm_goal.goal_type = goal_type
+        if goal_type == MoveArmGoal.NAMED_TARGET:
+            move_arm_goal.named_target = goal
+        elif goal_type == MoveArmGoal.END_EFFECTOR_POSE:
+            move_arm_goal.end_effector_pose = goal
+            move_arm_goal.dmp_name = self.hand_over_dmp
+            move_arm_goal.dmp_tau = self.dmp_tau
+        self.move_arm_client.send_goal(move_arm_goal)
+        self.move_arm_client.wait_for_result()
+        result = self.move_arm_client.get_result()
+        return result
+
+    def recovering(self):
+        ## TODO: include recovery behaviours?
+        rospy.sleep(5.)
+        return FTSMTransitions.DONE_RECOVERING
+
+    def set_result(self, success):
+        result = HandOverResult()
+        result.success = success
+        return result
+
+    def load_policy_params_from_file(self, filename):
+        with open(filename, 'rb') as f:
+            return pickle.load(f)
\ No newline at end of file
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/setup.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/setup.py
new file mode 100644
index 000000000..4b26a1820
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_hand_over_action'],
+    package_dir={'mdr_hand_over_action': 'ros/src/mdr_hand_over_action'}
+)
+
+setup(**d)

From 9c21e4dfedb740ed84993b52cd5d309a44f3c8c3 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 19 Jul 2019 22:16:05 +0200
Subject: [PATCH 002/264] Added a demo for context-aware item hand over

---
 .../CMakeLists.txt                            | 27 +++++++++
 .../config/context_aware_hand_over_sm.yaml    | 58 +++++++++++++++++++
 .../package.xml                               | 27 +++++++++
 .../ros/launch/hand_over.launch               | 21 +++++++
 .../__init__.py                               |  0
 .../scenario_states/__init__.py               |  0
 .../scenario_states/detect_person.py          | 43 ++++++++++++++
 .../scenario_states/hand_over.py              | 45 ++++++++++++++
 .../scenario_states/identify_posture.py       | 29 ++++++++++
 .../mdr_demo_context_aware_hand_over/setup.py | 11 ++++
 10 files changed, 261 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/CMakeLists.txt
new file mode 100644
index 000000000..32ac555d1
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_demo_context_aware_hand_over)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  mdr_move_arm_action
+  mdr_move_base_action
+  mdr_find_people
+  mdr_hand_over_action
+  mas_execution_manager
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mdr_move_arm_action
+    mdr_move_base_action
+    mdr_find_people
+    mdr_hand_over_action
+    mas_execution_manager
+)
+
+catkin_python_setup()
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
new file mode 100644
index 000000000..b70aff59e
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -0,0 +1,58 @@
+sm_id: mdr_demo_context_aware_hand_over
+states: [DETECT_PERSON, IDENTIFY_POSTURE, HAND_OVER]
+outcomes: [DONE, FAILED]
+state_descriptions:
+    - state:
+        name: DETECT_PERSON
+        state_module_name: mdr_demo_context_aware_hand_over.scenario_states.detect_person
+        state_class_name: DetectPerson
+        transitions:
+            - transition:
+                name: succeeded
+                state: IDENTIFY_POSTURE
+            - transition:
+                name: failed
+                state: DETECT_PERSON
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: IDENTIFY_POSTURE
+        state_module_name: mdr_demo_context_aware_hand_over.scenario_states.identify_posture
+        state_class_name: IdentifyPosture
+        transitions:
+            - transition:
+                name: succeeded
+                state: HAND_OVER
+        arguments:
+            - argument:
+                name: posture_height_width_ratio_ranges
+                value:
+                    standing: [1.5, 3.0]
+                    seated: [0.6, 1.5]
+                    lying: [0.0, 0.6]
+    - state:
+        name: HAND_OVER
+        state_module_name: mdr_demo_context_aware_hand_over.scenario_states.hand_over
+        state_class_name: HandOver
+        transitions:
+            - transition:
+                name: succeeded
+                state: DONE
+            - transition:
+                name: failed
+                state: HAND_OVER
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: obstacle_present
+                value: False
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/package.xml
new file mode 100644
index 000000000..8ae3e6ecb
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_demo_context_aware_hand_over</name>
+  <version>1.0.0</version>
+  <description>State machine and launch files for a scenario in which a robot detects a person and hands an item over</description>
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <author email="ahmed.abdelrahman@smail.inf.h-brs.de">Ahmed Faisal Abdelrahman</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>mdr_move_arm_action</build_depend>
+  <build_depend>mdr_move_base_action</build_depend>
+  <build_depend>mdr_find_people</build_depend>
+  <build_depend>mdr_hand_over_action</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>mdr_move_arm_action</run_depend>
+  <run_depend>mdr_move_base_action</run_depend>
+  <run_depend>mdr_find_people</run_depend>
+  <run_depend>mdr_hand_over_action</run_depend>
+  <run_depend>mas_execution_manager</run_depend>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
new file mode 100644
index 000000000..28ea9b089
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+
+    <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch">
+        <arg name="data_path" value="$(find mdr_demo_context_aware_hand_over)/config/" />
+        <arg name="problem_path" value="$(find mdr_demo_context_aware_hand_over)/config/problem.pddl" />
+        <arg name="strl_file_path" value="$(find mdr_demo_context_aware_hand_over)/config/plan.strl" />
+    </include>
+
+    <!-- Actions -->
+    <include file="$(find mdr_move_arm_action)/ros/launch/move_arm.launch" />
+    <include file="$(find mdr_move_base_action)/ros/launch/move_base_action.launch" />
+    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
+    <include file="$(find mdr_hand_over_action)/ros/launch/hand_over.launch" />
+
+    <!-- State machine -->
+    <node pkg="mas_execution_manager" type="state_machine_creator" name="context_aware_hand_over" output="screen">
+        <param name="sm_config_file" type="str" value="$(find mdr_demo_context_aware_hand_over)/config/context_aware_hand_over_sm.yaml" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
new file mode 100644
index 000000000..860d2aed2
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
@@ -0,0 +1,43 @@
+import rospy
+import actionlib
+
+from mdr_find_people.msg import FindPeopleAction, FindPeopleGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class DetectPerson(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'detect_person',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'],
+                                   output_keys=['person_list'])
+        self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
+        self.action_server = kwargs.get('action_server', 'find_people_server')
+        self.timeout = kwargs.get('timeout', 120.)
+
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+        self.client = actionlib.SimpleActionClient(self.action_server, FindPeopleAction)
+        self.client.wait_for_server(rospy.Duration(10.))
+
+    def execute(self, userdata):
+        goal = FindPeopleGoal()
+
+        # calling the actionlib server and waiting for the execution to end
+        rospy.loginfo('[detect_person] Sending action lib goal to {0}'.format(self.action_server))
+        self.say('Detecting people')
+        self.client.send_goal(goal)
+        self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        result = self.client.get_result()
+
+        if result:
+            rospy.loginfo('[detect_person] Found {0} people'.format(len(result.person_list.persons)))
+            userdata.person_list = result.person_list
+            return 'succeeded'
+        else:
+            rospy.logerr('Could not detect a person')
+            self.say('Could not detect a person')
+            if self.retry_count == self.number_of_retries:
+                self.say('Aborting operation')
+                return 'failed_after_retrying'
+            self.retry_count += 1
+            return 'failed'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
new file mode 100644
index 000000000..4b6c050b2
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -0,0 +1,45 @@
+import rospy
+import actionlib
+
+from mdr_hand_over_action.msg import HandOverAction, HandOverGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class HandOver(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'hand_over',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'],
+                                   input_keys=['posture'])
+        self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
+        self.action_server = kwargs.get('action_server', 'hand_over')
+        self.obstacle_present = kwargs.get('obstacle_present', False)
+        self.timeout = kwargs.get('timeout', 120.)
+
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+        self.client = actionlib.SimpleActionClient(self.action_server, HandOverAction)
+        self.client.wait_for_server(rospy.Duration(10.))
+
+    def execute(self, userdata):
+        goal = HandOverGoal()
+        goal.posture_type = userdata.posture
+        goal.obstacle = self.obstacle_present
+
+        # calling the actionlib server and waiting for the execution to end
+        rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))
+        self.say('Handing item over to a {0} person'.format(userdata.posture))
+        self.client.send_goal(goal)
+        self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        result = self.client.get_result()
+
+        if result and result.success:
+            rospy.loginfo('Successfully handed object over')
+            return 'succeeded'
+        else:
+            rospy.logerr('Could not hand object over')
+            self.say('Could not hand object over')
+            if self.retry_count == self.number_of_retries:
+                self.say('Aborting operation')
+                return 'failed_after_retrying'
+            self.retry_count += 1
+            return 'failed'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
new file mode 100644
index 000000000..c5d4b6c5f
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
@@ -0,0 +1,29 @@
+import rospy
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class IdentifyPosture(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'identify_posture',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded'],
+                                   input_keys=['person_list'],
+                                   output_keys=['posture'])
+        self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
+        self.posture_height_width_ratio_ranges = kwargs.get('posture_height_width_ratio_ranges', {})
+
+    def execute(self, userdata):
+        self.say('I will only hand over the item to the first detected person')
+
+        first_person_image_bb_width = float(userdata.person_list.persons[0].rgb_image.width)
+        first_person_image_bb_height = float(userdata.person_list.persons[0].rgb_image.height)
+        bb_height_width_ratio = first_person_image_bb_height / first_person_image_bb_width
+        rospy.loginfo('[identify_posture] Height width bb ratio: {0}'.format(bb_height_width_ratio))
+
+        person_posture = None
+        for posture, ratio_ranges in self.posture_height_width_ratio_ranges.items():
+            if ratio_ranges[0] <= bb_height_width_ratio <= ratio_ranges[1]:
+                person_posture = posture
+
+        rospy.loginfo('[identify_posture] Found a {0} person'.format(person_posture))
+        userdata.posture = person_posture
+        return 'succeeded'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/setup.py
new file mode 100644
index 000000000..7a48e7785
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_demo_context_aware_hand_over'],
+    package_dir={'mdr_demo_context_aware_hand_over': 'ros/src/mdr_demo_context_aware_hand_over'}
+)
+
+setup(**d)

From f023cb043841a5c571e5bc3e4adea8affa8c6670 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 19 Jul 2019 22:21:27 +0200
Subject: [PATCH 003/264] [demo_context_aware_hand_over/states/hand_over] Fixed
 incorrect action server name

---
 .../scenario_states/hand_over.py                                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 4b6c050b2..58974a7c4 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -11,7 +11,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    outcomes=['succeeded', 'failed', 'failed_after_retrying'],
                                    input_keys=['posture'])
         self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
-        self.action_server = kwargs.get('action_server', 'hand_over')
+        self.action_server = kwargs.get('action_server', 'hand_over_server')
         self.obstacle_present = kwargs.get('obstacle_present', False)
         self.timeout = kwargs.get('timeout', 120.)
 

From 0d012ac23c1bfdbdffacf0cd3eac3b479447ce96 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 19 Jul 2019 22:52:29 +0200
Subject: [PATCH 004/264] [demo_context_aware_hand_over/sm] Updated the bb
 ratios for the different contexts

---
 .../config/context_aware_hand_over_sm.yaml                    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
index b70aff59e..b11929d4b 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -32,8 +32,8 @@ state_descriptions:
             - argument:
                 name: posture_height_width_ratio_ranges
                 value:
-                    standing: [1.5, 3.0]
-                    seated: [0.6, 1.5]
+                    standing: [2.0, 5.0]
+                    seated: [0.6, 2.0]
                     lying: [0.0, 0.6]
     - state:
         name: HAND_OVER

From b753fccced88f43be18c5dbd67ca0b73eeb3cf6d Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 09:28:24 +0200
Subject: [PATCH 005/264] [hand_over_action] Correct dmp weight files (RPY
 vector sizes)

---
 ...d_obstacle_avoiding_weights_1600iters.yaml | 825 ++++++++++--------
 ...d_obstacle_avoiding_weights_2100iters.yaml | 825 ++++++++++--------
 ...d_obstacle_avoiding_weights_2900iters.yaml | 825 ++++++++++--------
 ...ed_obstacle_avoiding_weights_800iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_1600iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_2100iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_2900iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_3700iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_4500iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_5300iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_6100iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_6900iters.yaml | 825 ++++++++++--------
 ...corrected_underhand_weights_7700iters.yaml | 825 ++++++++++--------
 ..._corrected_underhand_weights_800iters.yaml | 825 ++++++++++--------
 ...d_obstacle_avoiding_weights_1600iters.yaml | 825 ++++++++++--------
 ...d_obstacle_avoiding_weights_2100iters.yaml | 825 ++++++++++--------
 ...d_obstacle_avoiding_weights_2900iters.yaml | 825 ++++++++++--------
 ...ed_obstacle_avoiding_weights_800iters.yaml | 825 ++++++++++--------
 18 files changed, 8100 insertions(+), 6750 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
index 366175f9c..ad3774c36 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
@@ -451,378 +451,453 @@ z:
 - -149.04938390927575
 - 84.38242545119284
 - 80.72949701618471
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
index 4ebb766b6..034340a77 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
@@ -451,378 +451,453 @@ z:
 - -105.89449755489385
 - 33.73473466239024
 - 136.2264101780908
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
index ab15b0753..f3e5232ce 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
@@ -451,378 +451,453 @@ z:
 - -63.11945703033657
 - 48.081395646440086
 - 157.13565809180034
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
index 6343958e6..231edc66c 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
@@ -451,378 +451,453 @@ z:
 - -101.88672783775839
 - 83.0232257931865
 - 43.383037925239414
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
index 04bcac2c3..91e5c9c17 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
@@ -451,378 +451,453 @@ z:
 - -169.1920506442319
 - -108.23662676360165
 - -207.56861909360228
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
index 6929a11c6..19a5cff5a 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
@@ -451,378 +451,453 @@ z:
 - -144.308647591676
 - -170.29484389514315
 - -162.37157500544316
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
index eba346c3d..d56bdec49 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
@@ -451,378 +451,453 @@ z:
 - -103.85651781817371
 - -131.53030224442517
 - -122.21710160107816
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
index 8627242ee..9eadff47c 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
@@ -451,378 +451,453 @@ z:
 - -151.858761996727
 - -161.10172004637832
 - -135.60897240328893
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
index c12337b06..18aeb4a9f 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
@@ -451,378 +451,453 @@ z:
 - -137.19949307427692
 - -164.01851373080234
 - -202.79022329301824
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
index 1b1eaa6fa..4e840d95d 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
@@ -451,378 +451,453 @@ z:
 - -144.99990784662083
 - -164.950361248946
 - -208.17585911641996
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
index 2b6dc75d7..37bf470ba 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
@@ -451,378 +451,453 @@ z:
 - -183.92027475378654
 - -177.78294904663525
 - -194.0016286625082
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
index ac3b7c76d..74e3c4c62 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
@@ -451,378 +451,453 @@ z:
 - -173.18252574263116
 - -163.59185250182108
 - -184.34441485108246
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
index c6ea837cc..3b75d6638 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
@@ -451,378 +451,453 @@ z:
 - -164.52979947684372
 - -161.41818051546772
 - -158.09310696825614
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
index c203bf9e5..b00633da1 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
@@ -451,378 +451,453 @@ z:
 - -185.1613937082734
 - -123.90573654523244
 - -135.67799993329913
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
index ef4039d3b..619510115 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
@@ -451,378 +451,453 @@ z:
 - 23.50119531752805
 - 54.07588641447143
 - -91.07200624607479
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
index 81f89ffbb..ebb2aaa7b 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
@@ -451,378 +451,453 @@ z:
 - -28.709326664936647
 - 69.74118428651663
 - -7.377070293044171
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
index 9ebb5f888..48d01f439 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
@@ -451,378 +451,453 @@ z:
 - 46.951435542123164
 - 17.079852248988303
 - 8.823259483674619
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
index f09cc1c45..11b7bae7a 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
@@ -451,378 +451,453 @@ z:
 - 38.133373393847464
 - 133.55911624440668
 - -75.3184987249469
-pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
-yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
-  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
-  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
-  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
-  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
-  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
-  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
-  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
-  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
-  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
-  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
-  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
-  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
-  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
-  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
-  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
-  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
-  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
-  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
-  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
-  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
-  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
-  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
-  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
-  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
-  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
-  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
-  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
-  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
-  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
-  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
-  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
-  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
-  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
-  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
-  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
-  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
-  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
-  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
-  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
-  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
-  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
-  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
-  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
-  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
-  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
-  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
-  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
-  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
-  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
-  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
-  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
-  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
-  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
-  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
-  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
-  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
-  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
-  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
-  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
-  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
-  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
-  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
-  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
-  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
-  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
-  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
-  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
-  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
-  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
-  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
-  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
-  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
-  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
-  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
-  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
-  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
-  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
-  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
-  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
-  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
-  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
-  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
-  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
-  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
-  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
-  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
-  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
-  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
-  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
-  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
-  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
-  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
-  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
-  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
-  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
-  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
-  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
-  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
-  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
-  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
-  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
-  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
-  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
-  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
-  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
-  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
-  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
-  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
-  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
-  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
-  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
-  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
-  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
-  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
-  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
-  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
-  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
-  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
-  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
-  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
-  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
-  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
-  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
-  -420.89393277454684]
+pitch: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+roll: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]
+yaw: [-156.65718192265612,
+ -156.82512813476856,
+ -157.03476785128979,
+ -157.2807477120766,
+ -157.55369295460645,
+ -157.84461759860156,
+ -158.14778763059294,
+ -158.459737944698,
+ -158.77686003690096,
+ -159.09532479965736,
+ -159.41325989374954,
+ -159.73163715474024,
+ -160.0521375845057,
+ -160.3745530176387,
+ -160.69700732848457,
+ -161.0185104942475,
+ -161.3402653767862,
+ -161.66397674111627,
+ -161.98953153448417,
+ -162.315201866925,
+ -162.64003295077077,
+ -162.96511555690367,
+ -163.29205326830424,
+ -163.62078022025358,
+ -163.9496971026068,
+ -164.27788257902984,
+ -164.60632279261657,
+ -164.93652376742696,
+ -165.26846111422094,
+ -165.60065680035294,
+ -165.93222401150882,
+ -166.26405206032558,
+ -166.5975532366255,
+ -166.9327393757321,
+ -167.2682466574162,
+ -167.60322357479959,
+ -167.93847000443222,
+ -168.27530835178302,
+ -168.61378183900877,
+ -168.95263403217712,
+ -169.29104925446183,
+ -169.62974494472823,
+ -169.9699574813401,
+ -170.31175703012775,
+ -170.6539879618333,
+ -170.99587071205337,
+ -171.33804689251744,
+ -171.68167070253227,
+ -172.02683518428148,
+ -172.37247918025702,
+ -172.71785930237908,
+ -173.06354756695924,
+ -173.4106198177865,
+ -173.75918826314913,
+ -174.10828013599635,
+ -174.45718809095524,
+ -174.8064204116356,
+ -175.1569783712803,
+ -175.50898997238295,
+ -175.86156501042154,
+ -176.21403187168687,
+ -176.56684061134436,
+ -176.9209216656706,
+ -177.276415779213,
+ -177.63250973601328,
+ -177.9885671847553,
+ -178.34498510912093,
+ -178.7026267789968,
+ -179.0616429301755,
+ -179.42129201479344,
+ -179.7809723347144,
+ -180.14103262348885,
+ -180.50227258176474,
+ -180.86485046896803,
+ -181.2280913368986,
+ -181.59142740879287,
+ -181.95516366594228,
+ -182.3200397542152,
+ -182.6862192544363,
+ -183.0530889992954,
+ -183.42011429540196,
+ -183.78756055865932,
+ -184.15611080378343,
+ -184.525931978699,
+ -184.8964681246396,
+ -185.2672167028437,
+ -185.638407452449,
+ -186.01067008275334,
+ -186.3841731854121,
+ -186.75841368027824,
+ -187.132920178221,
+ -187.50789034493104,
+ -187.88390380611247,
+ -188.26112928818085,
+ -188.6391124973969,
+ -189.01741212654568,
+ -189.39619709895,
+ -189.7760000696095,
+ -190.15698858912273,
+ -190.53875329031354,
+ -190.92088183004392,
+ -191.30351746122224,
+ -191.68714886802226,
+ -192.07194129758645,
+ -192.4575266759198,
+ -192.8435204676572,
+ -193.23004308121793,
+ -193.617542113636,
+ -194.0061795490326,
+ -194.39562519327265,
+ -194.78552113473754,
+ -195.17596753027587,
+ -195.56737365493905,
+ -195.95989742408074,
+ -196.35324332333818,
+ -196.74707886293638,
+ -197.14148632095296,
+ -197.53683929553736,
+ -197.933290967728,
+ -198.33057750888972,
+ -198.7283906402854,
+ -199.12679692660544,
+ -199.52613681329163,
+ -199.92655820874447,
+ -200.32782617456402,
+ -200.72965543146975,
+ -201.13209880120448,
+ -201.53546597968077,
+ -201.93989917925003,
+ -202.34518974707686,
+ -202.751074198292,
+ -203.1575933993827,
+ -203.56502857939287,
+ -203.97351593447695,
+ -204.38287067560304,
+ -204.7928499203278,
+ -205.20348419671345,
+ -205.6150284301484,
+ -206.0276125727257,
+ -206.44107345232243,
+ -206.85518761577225,
+ -207.26997671022124,
+ -207.68567140275596,
+ -208.10239525551458,
+ -208.5200046331361,
+ -208.93829436247796,
+ -209.35727851912242,
+ -209.77716544140384,
+ -210.19807222793216,
+ -210.61987285855759]

From 1998d8ba365e889fd88f554bc56d3047281f07ac Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 09:29:22 +0200
Subject: [PATCH 006/264] [hand_over_action] Change policy param pickle files
 to python 2 format

---
 .../config/learned_policy_parameters/160_py2.pkl | Bin 0 -> 6050 bytes
 .../config/learned_policy_parameters/800_py2.pkl | Bin 0 -> 6017 bytes
 .../config/learned_policy_parameters/80_py2.pkl  | Bin 0 -> 6042 bytes
 3 files changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/160_py2.pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/160_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/160_py2.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..87ba1ee44febdace1eaa678f12b91a4ff6559954
GIT binary patch
literal 6050
zcmX|F2U`?buq6o!5+o{uiXftdRRmWtObZ5*gi%BZl7t0Op^;#Akerd6K_rcmBuHky
zug(v9@AUfKA|IaHeM8*}r>fduk@u?NW$D|Hyppo~ke3xN%8PQ!%5vV?W|igVmArag
zURIG;ZmXL8nPvOx*`K$nd7N3DYLS<fSMn@B@3n3ID-GDHmXFQN%*=9CtH;*-|3yxD
z(W{H<fqK<iui7-I2leWqfH-shv#eKb|8L7@<!?*5((bVZR}@@P5679snK!8RdetEy
z?$3VLRmX&cgxLS#&%b}-%*s`#+@?ay$EUa^7T?7be9#9YI?D(QV&KsyS=K$1MO&|c
z5d6@oM+zx{vtD&6S6y=p%?kgO-K~%Ux$9LAO(086preHj+Lo2vQU=k9Nh>U3LUwfM
z!)DxxO4Lh-E(z0EmL~jqBzt0duKiA77}XfinM<3DqlPc*WmNb4kbK92%qO8yOz1#k
z*2~NI1p^1On3hhPtFp-D7&wo`JE8c737vXO6Vy|$9xqqD{u9(&6ZC{$J^7!Yr~U}~
zoPwfF>aL!bg`5{Ubjr#nEMw=SjOn6acrG*8_mys4lD^<lDrMTxxuW@+p7J&lOP{B#
zyz!A)Vhnhr{bR7~6MwW!zzj@@GOm*aX~D4dUFnfQo$8|rcUrIdmaBfb4eA*hC$8`R
zwdt=3c~-9m{1LLRUOo3mNQ0}HnJrq*%R1_?m8FUGRraWWuZ7t#U`{5agZArk&w(KZ
zjj0~;1B1k<xhwl<!cIjl3?FUvSmL0K*h#jrScZM<aZt#|eJ9g&c`$fM{l&inWSUSm
z6aUGU4a;XRT)lJJ6W_duWLb|MOFh1E0=mvhhqOw&e9@-2V_xQ@Q=0B+?<S<aH2=6>
zgDXXj!S|(wFd;iZCd*N+O`pl8T|3Z@UXDtoHbv?~&`InZ9xqD?XwxPwCt*YeKVwWA
zY{|N`rARHdIKq+)9?aBf((%cfY+<Mhv)Z)L8<-11hZs2sK)V<-#85zdFc)iB7t?KR
zbwhF06*T2&^MA=S_GJ65m^iYr_mMX9DDH}%zUY^lb7<foC_60fdRRVZf5YBoPH&V(
zj?jNJ<qTEbLL4j&kFbg!>}oGe*COp;a4dkKf)6{Wm?ms#FGghk1-h+dpM$Wc|Hv%9
zln}bffGl6asxNV%bffhU-%Ru5XxS)|`P;HWTqb)K>0x3Z441LXx*IV>GMgNLVQF+C
z#wI1gpe$p8cX57z8th9CYPB2pVXVAZHek^1{L@yxNgE~86fQsAWG?|D_`!iijHio}
zrd0-lA<Pd(XAL*Jx#s9|Tsy7(Dg&<_ji)%s$PH%X{5nH;3>y(4vgbp1`F;f&ZfaYU
zp!JrMhiqzR%qL$zSR6rA=@9w!i44%~uv!%4DVtcPqtcz%rR#$j^R>ZZ{Bv}n&W_ky
zdIF@o6l3)B@((gVn+%9ab9$qf|0heSCA6?W%WH2N?cu(5)FX_)eM&G8jsdK`F3TY9
zhDpY6KS)mFq|R_!<g4^b^<x+c%*|!ub7m?G{?gCuOv<(k2Qqx)`BPbyUSg_u6us2o
znsjo77%>@dF54fTz;NxNjD7M%4Ix@Sp_zU^jcskxL}nVAtev&x)z~I8Rbxv3kS_-B
zIiV)uMlxUY$391+tt`jG@Bstz8?7?w7tJ|yB+f1|hE(05YH27F`Yo_`8;0Xnn9n*a
z-|jM)JfdyMmAf(YF@-TOhCT25Fj#%6%CpK&gy9Gpd2XXaC9!mRlwQK9FW+<U^Mtfw
z+mj2ijwNsU$WK||=Ge%<(r1k8Zov>zT=4PyYaM5Iy!OVJY+Y7p0DP4iY+ZJVq?tYN
z6BB8%Tz51z?eRlwQC1sSql^9c2L=i#4Y{<%Lv#{Hkga-UoLo4?QHA~IU@^!zq3;Xo
zy!sfLh!gQ6r(A~6Py8>H%vD&GX!s|pU&Km(Ay4pqa$I`yuo6Xam;aF;PdzIsj8!nh
zWd~hPD7kc9%a&;!zEI%5rP)0~dfpM94!&P_){)>Zcap`7bi3Z<7ImNBWlBR<1TP^Y
z*bVs<7|tDxVaKG?UKc`GFGCL;j$z4%_(!&rzwEgkB3wTsW4SWnZmDFGmpakFo2{Ny
z&~t}Y9Ey<XK&ALjl{cn?!W0Y?qPi>+jS<Re<=iW&lW9dk=NlPLw2+M`<(;3an`}z!
zUFB_1CBx3>AERK@2SywDrp76i&e9Q#_Q3m!tY89D)W%OtakR%+SY$>p_;9c*3;2ra
zQ0a+PFn-yG@jV%PSGg^HQvE=-WtT@9WyY@DH8WiC%(B3ae8o3k#jVOrMrA%4-*s3w
z=bm*Q86G4F>-Qly-=BuzJci3K^}x;#h8JY}$$#u%qApIy;-`F<CE53)>aH?LchkOY
z?hN{(vuMA2!3~Cc7`i9FFe|mksp$rc6<(lJV`r$oS{Xh18grK}^2KkNm(h2a!|YX#
z8hJruiav%$l{M5-n_nnYJ;TUVz9yu&$uvXF?hel$<lGf4w1=fje<)B!vnh6P5`%S4
zwsJKa<#}B^q>+<bg`ZBwpAe;rQFc-=pb?RJP~i^U=W<}U<(SE1W@RfA221REoC`dr
z@x<(%B1*p*sfF`8zK6;f`cHCDCO;Fs?69RhU$9R~#Fjm$OK$*XtEAc{9R_<JOocte
z79l2Id6alLb7y+|<|D_rM2(u}>95tVu>Xp=96zMJ@-y1(c!49BIUr-ltqGmH<u{Y}
zSTv`&Yb68=h;cJ6rm9<*?3)?<9lSqt+E|E~l3(z!koZ81%i>|}+-}*ZL|?M3a3EcL
zz)EcJ8s*4FBQ0q{a}alCLhFsju9)8QNNKqKE<=6?P$eJHVey#(J41|y_$-;bf0Rmn
z<%W&(^t)kA!OgD6b8lX!D*dIaj0MF$);W;DVpeF=S@=SQOYKQozLf+rme0|ye0`2S
zX_3|fZo7FEU;Gb91L-0{I&uOYRTaYUkaggWYkF`IDMrVz_<rI!AxuxmRn5<_%_&wP
zGH8W8baCO)P^<=*dU3lG#~dBcpXHhPcBNX{PtZhckin-{95BMmIin}<2t_e!D*r->
z_76l}BCb6B{%&X{!Aq&lW#QX-kdx7Wg`qzV<2w#s<T+rX5R=;To)7UAm<yv;0%!+T
zS+UZ{!AW#o(<&`{9(q?ytewbXyBxeeBqNp5T0yxSEU;E;T}Dyqg<a32CLhhb4JOT*
zBW2)zF^dQX=}g4}`d-2ibqQabW%FDq=kFiIa3a2^t6Hfv_{q`ZZ5bC1xsc%7BGuQN
zQFC2ts#y4x^LexMIeoB}4q`C|;72?*-VnP|u6A2&;j6Y_G(w-6OS{34V`m;f3Txv*
z@6c|2_f<YmPdd)f-77bz?ipZT-5CIb_|9A0Jm(c1n!+oe!Q?;mgl)8HPB{BMgUNXt
zyNpz6J<C+OMGY)I;H!aT*^tSTY7((l+wZ#vdB?#}oN(M5lTP<w@OxuU1=JUl-V<2U
zE;tqan;K;Dj_l{8e~#t4LF|X(*GDUDk3|g0k2sbA`+90DzRb$6fO*vo!v$!M@+4$y
zQGbB+wou3<d)K2-3~w;Pv}z**UN+388<#2LF5PvXo9s!qw<5+k{CFc9N28QCtP;C}
zoMkIXIT&3Wbq2;q%E1ps*r`Ngy7EbynaX#QeY2FeCGMW6f0M1a$hQMeSc5NSE12`Y
ze^^%2vXxI5NB;@*=(3b+Xt6n}cgs{hTO}2^97$6!^$z{TGKrrl3U<oEW#MB<lENfk
z!-9Aa)eoZ-Y)da;^JSY;%5|y6V5m;I{w6y`J4p>8bs`-F2a?W6CslliHQ&l>dN~>C
z#b<nz#VA=|*e7f8BSW4nMp)VhMVXN_3>y6`SxPq(#ss4Sh6h+U?uGgw!cR7rq|vT`
zvn?=I>V%FI+BSrt$AV!6vt+rqZ?NU6$F#<M9T>k&$1zzxVqsz~gD3Hgt${&WB4kV>
zVd;;7A)b|Q?MXKn$?pDI)0R)CnYL79J6U$!?^!S}_2wxQWj`)KAr;Oh2UUGYQDj2T
z#v$fonj&9y7g(z`$x5bj>u!lGsc3krF!l6HBPN+5hBFmuvk${kMoOBZ1k;1&iVLXO
zapt~Rp5AAksALC|lU4#TAE`W)W%eq&*zvodcwti-BUKpLi<q#JBkhhda^jrks3ZuM
z2eN$qlIBo}nq3Mi=r|>^cqLpq{IBq$+u2N`;~4Ub;GFN6WX764hOQf=D5l(AS82fB
zEzUFg#IgW2ma>+~yBxy3K3r3LQ<*SZSb5EdQ2dh^xP?s@Pwr#Hj-|BSbXSOb4x{cP
zy5G{=(O6~5Yk7f&1ah744btio8){DKVD21dOw3r^)_i<kCENdyHWo5h9Npcd%kMNr
z*rL|gWuHp>b&6~DvSltT@hR_?9xj_HFONpTz&4nj&3MXlTJ6}~E7*8_mGz>SY*Alv
z1+`l3xNri-(<iXRf)%yY*>@HrEAO>YCsus)XtC2~ZE+h`&W9r@Dck2s>2Z$DVioEv
zlQ(jiYZi!~yJUJ=%QtOCr!U$+<Y9<+xp*7xe(^En1U8dD)2`*GQ+?$A%nWp%qVj3e
zy-=+^!i+yxcJWfJ*JaJkg>-y7up;QKB|{`cT6D2eXUz$Qqv#2gmQQwcfBZhrv{3DR
z00x(e6R3MG_1w-x4&Ro!2Z=24w{=p-VuK}1A)8!<>9;`!o>nDbOD9v(b^^^0WRrQ-
zbV!CV^7oN5>?usKEMRu9_cy+qv%V;{JIjsQcxMyagu2917x{5e@w$GK*<>KlM`7Y0
zyidRFd;gb`h~C>Q_gbXyigL9?z6DWfbGH<BWg|@EU0A=Tuuz&s^Sc8WN>G9#14AC5
zNw3JTJ~5LO>>z!-DdP#Ru)(aMNR6yTko%iu?Xlv24o$Sprv8bdQrK#LDy{j6LYMA9
zL#3=TQm{immkAZ7GzQJG)C9|ltfwnvAydNP{FDqv8y{-|PG=}in4raf-A~O>?CIMz
zi62g-D`99Uc^`z?$4WZCb}W=!!9<D@mvaPzNzy_=C-~8TWp%$-qH>yj_OjGowu)n5
zqFM&6U8E^S@hpfdR?r-)NRK!3=Zth@{>uU_0hkF#U9y7VRCL~Fo%m3ZYN_{;$#=4u
zs(h9udVFJPe46r6Y9g?ZcRoYGP`C^qFY?V)EDuP3f!Wneg&oxd(=}G4H$!1KE=l!i
z)Rv_y5wRaIo{G&UN+DX0CCjhy$I3G{cs){G6!J(pe~hVtWQElSN{p;UA7@0ZCM(WW
zA7$}#cA~-ta_s`!?WO2=C7PD5=G2*#SmiOV+ag=BxiLy4=5D7^?fcP`0>-%W8X*!d
z-pgmykIVQy1#9Ib(HYj0ca+E%c{x14F;aPTmY)<xPRY=9<*%x&H}7OVRJj;i#LP2-
z=~GN6UmY<Qz(&j{rc;k)_vTw?Eu{OU!#lF1m_~i-ggS?hZ-V)7kJ;8IOV)#kXU#5>
z^;fZdmL)9e%2-o&yyVK_+blB0_|!rB+OWj1m+w|gT+ydJFi_-rNV!&sDSNtlgQF6{
z+GH+<DqXWwFq7mOdM1N`YH~S9o37o)46A-im-47o4EVv26>Fa@gZ3djdLjQI`YJIV
z%5O=$q=DIC3EgBUx?Hua-m=C5lbZ*Fq?sbGDOx8mpDVkW9vC9N_ctT4*;W>bOJGdJ
z(t8;t0lzl85+H*gNDOK6rRXor`;yRP{XslaV7CMBF9Qi4nKC22J}~6IWzjr^v2+VQ
zpFETvLn|1MC~4Sqvc}r8GfHuCyjB^b_b=ughIvm&iZorK!Hz|F%4lG?;*O?dhQXvv
zg()W-ZTw%_hiZ*i?-?0!kfA{3W5HS3k7AT(gcV6`>IGYcAvlL>*Ft9SHOA=|%-*`g
zL^^te_v<W@2^%a#9A!;~QJZ(Z=Es;`139S&qmk~UOFx+oJ0c^_Y`4f0J!I91VkdSh
z)ZS03YV<z0$)X<>q8FP~y;M6fMf9ReJpqHQ7kO<bEtKt+8FtTCIm<Sk;H(1^Y2956
z&=Qyryvmrdpd#5&Ooayop`WRXbxFrFY@YLHO2BW_L_W+CBTF;&y!PobP_G7+tHJ+$
zR}IlVS^lM0FZ}z-vQEAD=Yyp$zpB2MsT<Nt(J_6Kjo)5ayfj3k<)$V1Z&Nz12AtBV
zq1v98^lDhS8vfs&m$g0r)~i?kZ_lfL_VnYPr({Rw*&_Clrw4T8y3w@L_#MD6)tT4|
dOH8i9vQCZAHoc}-ua~Pga?915^{Ou9e*t1%^(O!T

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..58211955fe700b92708c53581a22c5e63b58fc2b
GIT binary patch
literal 6017
zcmb_g2Uk=}vn2@%5+o{uiXfnbs|bo1ro}`OWE4??AP5&kg&~O9L2^cNPLf7JlyJ$g
zR_!15-agmweZSzrT2OsXcXf4zy{nJWBKK9r%aXUDxy7Y<p)V_5loviNEq(gdHmfu*
zxA;|Ad1*y%xvjzM&n(+l&;GnMn8%yd8Z2_Na*Lnk<(Apzz0w3*gXJT0Gc&UsgViHz
z{(tebyztdI!@fF$wccP;Z`iLl90-gz=dWd*!S+8YpOwEY;Yzzl7F>}ZX*d{f7H?i}
zu-6+L0^|SayJB!mOiYaXAO8INC*G{w;FQx?VEHJ3YoyU!*3l+&(u7f6l8isND#Owu
zCLN~F2FNBRq#Cn2!=VBi;;c8glp9=g3d{=rJ%U>S?Q+)}JT$v3HM<TMIB1&7G|esY
zOU7kOzWT|o?u<L;!elQ+#&qwnSP@o%UNp!awxl~q7O;_vab1YaU}+zIp<SkQcd!(L
zm9lcQ$e#RzA+$w`(H)Ik(<rO?uGlK1p?fky!{<<crv#Ha!x7C&Prc!2xxwqdR(fkz
z9@86+|JTYBf2@q5mH%Hq1|MzflX`=1xxp`|-f)WUkMI3INBL_e1n3Qce@y7DGo1co
zg3;B?%ofdOWEHj8$b#{?e3czc%h!Tz7%@i&w&R<0x<7?677eK$@*M+|QGHu>(TJ^z
z92h^^>aoZ{D`h8!>}M%<vBN<DANL+l)8)eGC3WX62Ff%=Sx@>qTh=U}!FcJ`Nl#38
zQIcPJbX)2%!3pRLkamWzO}=Q;+c3}c=#a)c+PiV7E6F>m*Tkhl$B?_yOffM#Q6|e#
zrA?p7#sZCQLk~wKQj;Qeq3EFO9FZW4iD=a({kn%?8TgD*ZLlG$(wZVQS{4|wC<FU5
zb((d2vLYK8ti-G~ZR9HEf|(m84g#67rVKIWQ{JD06|BnMO>K33QDr0=pK9}e%QSXm
z^R4W0WMl6mt>!nV6+e8@C)KA>&p~i@c>I;{JkFlL&IL|ykOq#>cR1w~UENGMMCu=6
z8Qs{{UhG|twu8|zAI1tkY^P%yv7x;fmiZUxvXWg6!k_#tv-na>(M|g0*Lf`aQVx<X
zv>f1@Y3>{&YlSj@Q<f-~%8o^PxELu$$mj*#)mTb0n;eKCX>g*9by|c0`Gs+w#rZy}
zu`AuE(N5fp(eff$gHb#44_ld#R$8VxTz<I8P9lc!odb(lPZuZ6tLPA8xF1Z;nq2?p
zvZK#YZMU{dm}Sr!PH>Qs6M|OxeTw1{tVM;&jt|8PcPmhTO;b^fmg`O)vaanhpL}J1
zQ52=u4$)5@OFz>N%Z0(7M3rAmRHpN)bbb(1o;LWI@Eo0}wWI7U-GS0of>Gvq`3LD|
zO!{R{OL~)+|0heSp=e=&<}z;^?cuJr)kBQEeL|r>0{vJnE6t$Xg*_R&c|SRglR6?8
zk+0GtRgYlIH#e8@&zY$(`b!^=Gbx)c9LUhs=TBr=dMMMqBj}+ASEPe0#DvKNbJ_gx
z7{<%zWb~6KswtxR6PlR!)7aD|jc2Bz(b`#CUIpSK|4!*2@WsF#CsZe1P3DWfxaVlF
zm0t-ken3Crt3@XLVmN0G9q1HOXyq-smVw$sp9OYq!g$mQ^H~RF;x>!PBgR%*WjmHR
zrdR``*zvv#qt&O%T&wIP7!RR=`!+bdrYs#ECFe2X%l91oI3_LF^yEUUV$qv9@<SG=
z9BUa^{ERW(br?g7@;{y_({XltnKwpd<AP!U@RcyXals{;VfMfejHktM-Qm=<M-Q+;
zTWw^8Dfazu7-^u?=P(uz&_OwhXw@TQvLI6&RoH(H5u=Pz^nF2}R~<nk<s^K6S}ud=
zqx>(F%vD$wYw{0Ny@->(0`B1Z_^5Q}V(A9W{q?tef8zO?##n{0T(;2pn3hZD<!qVO
z;R_AEC{6BB(*2I&$&kAR0ge>@awk~KNSEt1s;K?^E>r5WqId`y#&+nZpa^O(g&&a)
zdtE5SIvKq0a0H7!l)q;?`OA*m0g9`qWHd*{-7Qr%aj66KJlXPbh3;F7;$W0a2dSc4
zmEM>R4p-<eKviip8lu!mb^4Xm%Cu7Gcq2ne7P59jz4LQ*lXYpit=<N|X4#qiV--ex
zV6u@3L%e$JEbSp^3%aXh3FDZeH-2D>qaD`5Pcnkhhl6dh<5yILNq3yW*aaWf_hj5{
zbyIq!>b`8sHn%j%j9s~FW`y$0vLK^=#e}bNt284w&d1=J4y)$WS?iJEL6ES1AA0Tm
zNf^&ys1#H8?fhVTLA0Oz+YWp5#mPAQkZ-amyIyqNCHaOf#<$g-MPC?zw%cdjV7!CD
zJMtT|Qgf7^uE%J>Sz0xEitekCk;7$}JAaNZ{*ieZc}H@dy~I%i4`@o!$1<q0f*N}B
z3yrE{8M(^W#1uD~W~tfT;@$(CyQG!&kaX$~1j$G?%?`Q8Vx5zX94$t<Ugr*I;N(`}
zr<1YAlv2ecTPf(*fXJ|4QA5}1r!Zc3%;Yw+vXKd+CAK|I2OZIHV)j-ctzVDU%6S#v
z!ekVE$2lmKA4y(z*wCIY*xyUShCQcCPatM1rOGB9MtdJjg+IdvMNGc(DE4xuW@h~Q
zL&x|e4VvbeuT`(G`-)tS@6z`AGg|F<fJ2zsC!<HLDLQ$}KP2zb7*27QW}d5C#>}{w
zu5KpTH?jEJd46)*XsGNZzu;z}2|*Z>p9i&lyJYP(dXr^|1L@?0Nh=IorXAU6U?lg@
z6im$|w4NAj%ieo#DfL(0Wyn8)bje4wTYP50&QL~uLYB<kJxr&*a>Lpg=G~Cy;3n6j
zIoGbxmHyIM%7$VWs~pHc5q@Jj3t#APsX5Nbw-7)^^Eld;ug}pd&C-%jwd<Gg#ebjF
z6E32p{b}IC$^sZ4u!q}nO%Ev~#OPQS-;O<}h`q<es;1}I<P@t=8L+|*I=S$07?wjy
zyr}Nj5l6=}0o*gsu0*5#M9su{8F&)ufMFia8Qr;uXo^X5`4?KWyD$1Y<<}?Q-ww;9
z@KS1WFo2HzoQ%Fmmi`z_?>Kmo>wxhBOlr@&KO|IOE}UKoWE|LK#YqDP$I*FN>$L2-
z>20=~>q*41Z4Sx~$na}vsi0jB7Fa2<E~Tl=!p>(>orfl#29xIGNa?>@#3sT)I#RKK
z-j^`mIFB#RvVOXR^LGzpC<))v4O*)-@X68RZ7CNHx)9)7qYY)8QGG?KE7|yz^Ldl>
zI(@K~cFJN3#P<ZOy`k(%yV`8Afv?(vktls?4&w&nQ#<oOLRc#|dWSaa+pqGt`@N$q
z-JNoC`kn>$)tv<}fNwm-wbNcPVJSTFDNO#&OxQ$=mV~qKGf2*x*k+|lO8}{KgC6*K
zpRf9pWlbiJ8}3nVrB8R=gT3QnEJ{4;jY+3FF#5gW<wEL;2=9sPX&0Od|D^_*yd}F&
z(?7>?-2isO@cW~crsF3D<$FBafL%R3mQZTtmrq`G!%#ljZg^6}FR9x{cv~nS$=>!T
z5aSyRlU8k{-^+$<x^{s!ZZlnXsbojGyp@>Z@%@dg9loL7uuJR;c9xBMYJW`8jZ-i^
zRQo>^V(T>;($yzkBPOt&?3<<D7Q1_*?oGCGk%@he*@OSgR+#g@dr+3svehSyq3;;F
zby?~%nr#m2-7?i@t9$t_htd?L-l4BZCh;RhVXHJk7Csi=QzZFnHpBy{dT>KwQ+g;i
zU9dT!u1FOI!gSJkk?0iTB-Iq@6KT)ickh&R(8UMX^DUJz%ZW%YKJza2^M)+2?31<n
zkwH&3BW&%1Z<rA@j2iqbT1poQW1Q6i<9#d~^+H`R#ZNZprNJ(rvn?=M;)M1T#x|6t
z$A)1EvqZVKZ?NI2$Fzoh?HIes#M!fa$i~E6299HbR|BIoN6DxL!qOKDV*)$hn&WOT
z5#9aSJ5J{@X{IfeY$nUL`yC7NQctd;DZBBBicmP499;P!Maj6H7l+u7X-dB8&azi)
zl%-5{{dTb|8c_d4k$U>10h6SNp-d&M_TgB}NJ&#Fgc&qfluyr&k^5%3d#`zt$_^nW
zEd^mdT0P)>vklwW@;j@%ur3YJ2AFsk;U)8_v^mP~vC~?j5+K+f$geBswS-F2;*wv%
z#3`1akrC4FAIXDmW|Kz8Fz6S>Io~iz#+p8Y&Z~qdQf`l{)MMv5=b3!s*npZ!*vsTz
z2&JwMmz8fS3A34<*L*0=KaT$ESa<QH9uu}KrS+P-qTGEDwI9*-mf?=UGAXb51?m%t
zb-q^#tBY7?9ig4vImDWnvAC(__`HE=|6N+y$V582yGf_tNt&=hudm83o%Z_#*X(4=
zTzJwGo-I8>)>B>{j)u{Uhu)E~l;@1vk=v12E4##gQTA+6R}qOCt#@2F2Ghx7SY*SB
z8v5)Tn~|mW+Nc9dK6*6UX|pz{hMn`lXj;m<`lNI_$7Qh#b(YDiIpmrJ%FkUgJ*{Oz
zo6+Hmwhy@&<XO($M4MkiEHQzX$scK#^U~=)Vt-}^I!@5}jOk98c0Iz3KUa40P%T$v
z#m$9qd^4yb_^l;NBvhJpaZ+o|3C6?VcZ23nc1(Z#bB44~<-HF^mx^PkeJ*uWw?_=$
zl)3v!Z1FdBQp;w8ElU9}xr+2#Fa1v{6S1L_DQP{1ru(u^Ufnw&Lm0kz=oIf1dvR=F
zwy<*%-^|%x6xjt(Q7g~9$7@1uQi+Rv-><x`TqB$G2l*%x|G-`5ZSVWPR1$h_vfXQz
z-bi(+SSEt$w7Khwx3Xc<cqdlxC^ky7XnMB~gNZ6QIw<r$8udzs^hufQU<a7vjTw)5
zgf+5;l4@CrBK9}Q$|L1}8jXz2y8f{;(AX+}I<4ukVoJB8{<SQ#Qn1B5mvIC3(pWUV
zq&h^FWHnt8h4vB;=A~q?+W1&Aa5_UdVVn{FeK$2j*)zARlRlhCSK(+bejkk4M=BjN
zSSXIfc#4XDdI$seq?v|}^P>UV>OQX|b&~h&Us7|yDxQUjDjBeLk;Yi%nIB)I&=jYn
z+nfA3Bkh_0qCj&XW+G6VtT2>{j=Ss=A1JAkIv<&QC+n%|vn(>>8%h$=)JLg~!b0wu
z428i689G|%o2e}KNngI%rA);e)i~)IOVX2}SdNQQbrLnD=_)Gj1IALZ{#X^D<w&yp
zj(DV=@q*VaRfVAs)tMut27(oq@2gl@i8;!OT25BZl^^Bj=j<fK3*^dKUbh!x5>yN$
zUB#(0DRJr%kJ~I8aXGOn8gn<(==R+hS^-o184VCA|GbmOsvncFI|?i11koAxlebj#
zi`=K&zad(^3g9P&;S(}=Mg3Ko_2!+-hpBUMg=C&#OrIc~e09WJATMGj**oz_wy(W)
z)=Iih+P$NTix|`=PN;SG_$Gu8cgVIrS+W{TdDiS4QGXem0c>GWTgslY{Uuix-DHy~
zrY8>C*M>!wy?nDGamAeUK!2g@0d=_mQ}#^tdPfz?-efM8E?u!yn7QW~b}EB~y63W=
zF<rTd8Fu}aF6B2;(eDRiR-Apd4A_Tq>xH}r=zWc`Fn&woA&q2*MRXCNm~vIJeBBxg
zBsUKhNfS+8QQA#lK1a4QJupc5&P7&Yldb%uTntkx7T?PV0l3U8GEfFS5E#<rOW|Lb
z_a&go>iq;#V3z~WFa3!gnKC0iJ}~CIWz#%`(R2$wpFEHr%P5!*sWhxRS!3ndDOHr5
zpmoOR`HP&xGVcyek;d~3*pVBaG7=P__*pTTWiTmI;p&*9jsHvgFzw>ib4rFCWH3m5
z%ny*=8?5q-@It9cJ!`92f^(>HEg*xhuui{V_WCUn>Bu3TucJ`LZLkn^m^~FnY~J~r
zA0fR4aZ)!%qTLCXeli_?NQRwx-6BqO6II8HoOoNI_kPe-BX_ALi+NOl9<0;#QsqR7
z=s~C97>u@F#I>!oFkZLJuzjY|SvHvj0S+Y6+S}-7BrqRzi8W(ENAf~36%iPWK2jI^
zlJ;j<KkZLSz(1&tevl<5wq}Mi+NaAPy&<^V5c1!zrlHy=%fIx7v;TgwtTmkb^TE=W
zA57oN)KzJr>6pI8i{DOog4D;L`I;s1Z(Z6i1)k6u!Ze-d^@i|rL&Se|UeI)2)EgrI
zqw~@qoj%m*08@4dk!}~?b+T51rTcD<vZDKJ9e%}=RL^g_QJTifdc&1+!_}N}!?ikt
HF7$r^flIw(

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..c1c95f79051aad386b46e362afdf03a8455ae130
GIT binary patch
literal 6042
zcmYLNiCc}`*KQ(8gGNnANi-luGMq<+CZ$87K`Et#dQu|ON%NfMd7|MoC=G9ehU;4Q
zANGCs^ZI?ikL%*z&slr#wbyj7y$xo$Z^~bnd<f1hF3k&mUH+=9@L6f;vkz8TrFpr<
zZ{C)bmgkmPsV0A=tlm8T`>SdiZ&It8<!0p;KhMj3YnAs#16Hc}V^b3olN{CJ@j?Fo
z>RDOgn~Um!I`yDlwX9bU>D9yj@uvK1UZ-09XUXSfA4)jW`mq^j<cF(A;!WaB>s1@Q
zYU>~WcfISXU1DNl-2d>;zyHLWl&SVPjRoe9eK{vkHf0G*73LV$p$(lyUu5~RZ0gW|
zQ<~4qL?w(m^=JXLaL}ucWvWw7fl0x?`a2g;DHpx!swrizDRr#CR$KNZmzB}Tho@g*
z1K)IRFreQdI6@|M=cP@WWXI|qzRA9Hp7V;6E*tde95IMpd<%Bu#_<J~m{p_~O|t(;
zx#EUS4^+!Os$}V5xJ<~FOv}D);mgC)Bfl`AQ{6P>-1VwQnd<pp<-9cIj_cJE|5fhf
zU*(FZT%{OZNGqnpWl<-)Vw6Ey#Vj^tOed`;PPwArl**?1N>eFTF3XTEh1xZmNv%}N
zyiOMH2Pa`0YqGBU>X|7!sJ<vh9a^OhJDHcBKhR+=<Kj{IaZ2WNs<)=yDZT1bruyd8
ztEcIe_}>3Bp`WJZ8NKTNSIgcy_3U3Q4NfK|R%kva>!`(677Q=tyX<00z87TEoU<}6
z?f4;`F3(_yMMJ8q%wd2S)pukMjrdcZ1H)%4J(f6VCAODs{3^vBb~z~E=iU=(x?C7M
zrS9Stf0-hb&7>FEvSI!lhO4(vxnshUNS5{JHrHc<0_Z#=?b1RYe$&L;upqP2A&qyn
z-^Qh`B+o;y!IeV0pnK9xn3$a?lVzyV#HX{dNTu7*!%>OUq)1&bI*1*@5@ab6t(wsC
zeGJRMSBz?dEm@b=6sf@$M_7`9Lzy~FJAPS{Eeuv*MiU#kf!RQ`i;)9=w23i84Ee-|
za<GPV*^ky{*B4cUqw$$0|68W8E88DrpCd~fZ)r7+;;NYUL7!BgMLh?B*`e{*L-Qy-
zf!)g#Z;%F#(0455G)>)193=IRu!?T%Xg};<i?oKpE+2++er%^<8nLDQFf0qN&}AWe
z9E3i7Av5?^Oz15AvU~}vKEwgig_gtoWr{n;$VQ<oM9T_ssqC7ihl+tPOhzy3Zp0GF
zOtL?Qq`{sTo74ycvW#(_#o+;}u_xWA(N5fp(Xt}hfI&O+ycIL2m6~Y=mw9K|O~f$f
zI53NKceK~6iViV^`oid-;rjR2?7Tg+-P*2VhE8iZ$w5X=5TpF}X+k$_L<GyOH{s=b
z<*2`@Em4e?TlTK9sqL|leEm>S1kpR&$fr-FpJ9j9!a#S~#4-bw;k+)LpTwA_4SprO
zL?>#kiM^!TU%E;#$~Z6kB>nVBzwB#4Z}jy0VlFj=W@c!9>t(5Z+|#ytgs~4#3Hrm(
zkJY!O8N^-Km$B#v$!Qeo2%|^7vtCp^h9Te7RK~w%ro!MSeLT*jY&&uwLpNSNl~w5>
zrg=xuLkq4+2WN;8lL@A>{pkq|*DlKF7k5+>qWKG&823}y)`Z40)6jU(L7QI1>M+57
zf9fCdhk?8Hs7}0*%pdyVUZTNDmJ?w3gns!4Ei&mFLz!80pi_*&6}M?xI%*$%X4s8}
z!NURzSx02z4wK0>#!6acCzdg$Fb77l>vazXi!T+q7THNK97O~7ZLobuEFG>TmoVbP
z-#M5+E-l!0=R~Yy$%`>EFN<6p8yQ&oiZR_S7=nxPKc9Q6qjdXQFO168WrYsFcd5qK
zWyeUm*%k8`PmANcW2tG6A7YETTFM$jZ0-dNR8Z=3=!=KwAdX<Q>X9*7l%E`x+k6cY
zgNzaSyrRvk+|Wpzgt=#BGKfCne@n@1xka&t=TY@4PWlSCgU=HW>CVMU6xCgRA#+dN
z-%%NhAg0S7bUvZx(s?agrgZp5g|A4HON4Y+5}pdWS8&FT;Jgcq#k6!e-Q*IrUn?`E
zJ}ZKUkYVfue+dZV3dT@3>9ElS6V}P#16wyNc@xiN+xy9`^I^jE(=wVP<1Xe(HtSLc
z>Upx&6AHR-(~E-<G8Lc{-LCM$RA8ur{sL5$Mxr4?Ii;L^L)w^96m-0op(Hceh*Bzj
zot(+KEq9a;f$x}hM!#4EBi=At%7hxPymOHDAhZSCQ)C6>_(^Nb<0nVE%!OZM1cNsR
zJF<xHs0xwpI0a*uy_w&Wad(tx>6NMnvMoE@(kRo`Wlou4ihGtB{>XPs_$ba5CNd%m
zG5Ddwx+&MJb<J>PkvRA<_~yq`Fr33sDSke%_J!dUtNr8)YwXh&r{XX#KV(VvJZZYC
z@&jG;Z>tNFzVHm%?p$z&;VuU6%5Tg_jR!4VkI{k))N1rJ%~vBM$KGQ0(nbF8k1WVY
zCHZ#dDn|`GpfN=sOQ*^jYG}=GRH}|?<Rsq{Q=Darsb+nfdk;`{MN91=>C_(%kdbVv
z9dw_`IxAZ_nvZflFCNy4lXJPRPR5=PrIOtLq@Z6bMCu`hD|DTG2E#48Ol~tHTbVGJ
zW5@MufSXn)W^NZ!`^`u#o!9XrL`Knff`d|-Px7?JmiB$o=6({kY$z^0{+OwdD$8^j
zY`pO^^f|T&G5N-|*wcY4GvYTN*~KSmrD=ijTJ;8dZ^-4CleTwX(Q3^D9L4kj8TB|w
zXzwNeki17@DB>c`JXg1jnQ$^q-AuA?V)D21{N%LJVA)T8#m#~f0x%}Oj%fRK$;LbM
zCd&#3(#a2#78tljJ+je2Pwt~BkSmkWdSb96`yaWb)L*a6kbnGXlFw*2`^to!CPsZi
zmdxHeMx(xQ#>P3u-H>MBCMS=ao7ZVdKj|!GL$QZ-4rHJRzcH1CZ#1~noS^4hSU^Ve
zINFi#FVQQ_(vr_*H?QKG-vOy-xrmVVXa0{W3SfAMA^dSl4=QAd(J?K49DhlO{U@wd
zO)s%c5sP3MuwV<;$%)58uo_h2$>ol_+1Z^t!#(q?OElU~)KsjOfv4fN80O&|(4Bjf
zsu(qsf1^fw2O=*KzdQBuPDm!fYpKb>06Gp)7=7VP{V^CTIe3+8i}3<XYTvs*C6r?}
zlveSlAJ}EZNdpHb(0NVkv~0NP9rm)DNvvZ#9K1a&!|$Y}oO;=sVXfp~DOF_@c0QNt
zJT&n%m^39vO8>ngHW9Yck%~q1zJ?*{623Xe=GhX;?;XQX5`LtsTB|hh#m@CZDJKp(
zvcR`Ss&6S#eO;<6*!Yz3dz19qe>y1b#A5WvTmm-U6FX6_HY;r5yEb7YLZ6yLzrpa#
z+SH#Vtd$#8qV3?FH+kIsz6Voxx6G8bXM%lqVFC=`2TyVHtY=I}3XgmmlP?$v+i1~(
zaOPtM$$1+)%v5PPLn_^(1%5r?PyNZVA(JQ6`^2rZ>7GlVS3C?wi5^~<w7(03?|V}k
zpstAJJ&`@_qJ81N)F6|$W$#(~*Er4_z+MP`f40!p_=Q24i)S0Kr>DgdN-ccz$*ay7
z%12w2J0X^(?f}c%VgX6^j%$G!-eZ`wYAOAmmSoe7%hYj);kw5~cBRWp5o0{&-pj_Z
zDCIr7#I8UG*}AVBiYbaZ4dWx_(5FKDd54B{<%=|t%6F1|vXl?SF7Bv%pRG8`#DOR5
z!GC2dnDx7NL{`(Xl`j}W-*I&7vXpCRwmhbH&Q!iy+|PGBnx^1qCHjhF67wkv{*;Ev
z;^*S~3Q4}24e<b~9!4qHmL9^U%a$jV>r#b*5S?^hVReeJmuf=VMB4KY+&?WHH1T2f
zd@FAm<*Z1rzG6atMad%5K3U5j8FXhe!qz@8%7jJ3pp~B`bLk>sj59l6cz{I@Pt*kx
zezCkH4c7UTHp6I%J=#;~+hC?18-^9ku*!XSk1Z!XrnK7Ej<IM4&c690HYTPrZ~_y&
z8W^NGLPoVBEPb&sB(U?XIpGW=tGge2$EiF9&6K$!+sU%ya@UN!)RU`FmA&{xg{5#N
zIk4hWiX!8BUL0aSrz!GXcY(cHqpV~qx9$|nl8XAL3aO`08Zb$U7|K+n)g}~687XOs
z62u6aEy|~5$H;v%+`ZQ{Ny!dkO<D=SLZtFgmU&m%!5`lXiYGRuAyS2rcaeSkc_wXk
zGJO237N{%`Y!77l`Xw!(k~F{MmospR<yUx^wEKngpxfD`(J>7AMo{JlCdpV+yrAA-
zi6Z6pI7vNrZ&A+Z&5JFJCG2H#FY~fE@adZ3lS;yDX6LmKO!ZHo{}whK-MNktf6S%z
zri(({eFU|i(e;7uj=?G^ulW_~6IttgZm_H_VUu};c5>$sb7I;oT8r@omDPSuTG_~i
z+qpPPr|&7Mutlq{%N~vP`y}V=X3K17(o>!-Jxn%JULT8u!Gwq2m9dnU^qSk9aBRH2
z%6?Jytx#7UjvB3ZTs#irspD8;!-^W(><62Xm5<t}11sKoG+S#@TU>^n^N~nuD$_Je
zw?kYOyHE$2ypcn$StNexnCX5{CNzl-AGCeS#URgeF&b^Y39+mRyiCrgUCT?S`B?ii
zGthC8#-~qrL$vD=ru{gxlZR@#E^E$?EXUCS<$)i}nIgf`tc#P{gA_0vLwA5Qf3arx
z<DYY+g(|NDFgTVUN9{}A&&TNXIp*5zgCw^2+d8Rbv%!|7fR|i_^jk0ePb(6!rIVl1
zdK^s;WRtwQe^`bveC6nA-YNFu*ueb3?iKtnWq(m*eTECQ^340ZCe$XCILh21#q;`2
zvPpk{w?g6{xW~Bd{ditULQgc?y=LhRSFRSzL?DeedrRT1Y?w6OiS@e*8>JaERUW`#
zq7oPx5c~j*dPRoxNtx_m2N>gx8BchG4YG#9$B4BE*8V10d#w1KMI(K)sehuVRJO{G
zMr(SaFr?d2|4vq!Dfq)UmvI&QX-t}BsSc7ASx;A3h4vGV<fUXV+xS^ia4JKw$2dLy
z`(A2>V#C<3PWp5*T?s{V@y9^SJXX>%jm6?{jHf8^&yHf?zBE(OaXuQbt?u(oQcm%n
zy(~4CE#jG&sFHz$j?x&bxaY?gDQJpQq}z-9IW6s(|5kx!e@us=Hd(<?Dmw15Pkg9I
zmDG95WTkATDqs0XH7E@w32Dk_sgA&6?zs#FgJCk{QRtJYm>-b7e3Pr03U5^7q-(55
zPlm#DT#~9&s3}cXBH}(_EEStilmfK4CCl%y$I5eF@Vcd{F!+&j&W+T-VujTQO02BK
zcrc?@lNE=G&+_YQc9Oyi<k|&Zx0hlPlo)!tilWmgamr&Jw^_F0a$=Q8%toit?0YfP
z0>=1rT0tcKbvKV$KPF>$6|9x9h)%PgysbpO%6-QD8zPlAXZWNrd{PFlE9Wb+-dD;(
zh;lKmkjyiTsgtCW?{=8==S9pY`zIgE&dm=FT1xjxyH{j!5uN(f9<{cg-v{yIF4@*Q
zOV$I4XG|`#>aSw^3|m;#ma?a8f6bXi(QGos_|#T=ZCGO3%MS|@SB&=XRp@kBxmJLm
zHVpN8J0+OC$!si5x@N9m`o2@h=?o_7zT+YKbS)aw?E1|e%c4@z?+Zg#oK3b2*aUOy
z#k_~;eTT6Sz9sRH2C~Bvx>%tYa#gZ=>mU|MZmvv{CaS!qXg7g{9NEcq#USzBE6l_u
zEBQrS3}Y&mKFSCS@LQ8`e;N41!jLB43eRJ~hXqa6A0&_hyKH%W=}&ac<O5-kHw-x+
z*fjsdXu27{PaaN>r5B7xl{9SHAH>@8(@Iftg4P+M=R7%wY2F>2B8`{mFt;do83_ne
zT+o=zG?<j1p~`VPOTX7PA=<^O=d=vl%3y%<Isc68MKQ}WLJOrP^@5ec6r4qsQvn%#
zjd}VFGq-M&NJozHd>w@{Zi&T+W9+FgVp-{9>PC7EpinnPB3)Q6ePt^2s0=&sy2U!t
z&8j+HWY60Qtv63ojojlhS&X9s^k9>wmnwTwL=QUE<1koxvabC}3*mLk1Uu&{9Auk8
zaK@HIT6+im^aK_Ht}<uLXh>cteunu6qL0+YzNGy*HqZKz67UbIBOhjok*%3}PJ6lx
z(5r!EYS4clR)e)C%kz5m!oN?Jwd%#c50*ZBR{bbHZ%7MO$J9+;{B}bVq&^1CH_ci9
zHl_Wl|4E%1qOEyJuZEVXVgFt8vbN?Gy&C?XHLw2vnjY^}*>Jyzex0ogS$yCuEwQRA
h=Ci|$3~0|H%Q`heTl1P;y<Vo?$SG5A)~UMS{{@a$>N@}c

literal 0
HcmV?d00001


From 642c73850c3336747e1ac620ab47c13ee5264811 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 09:30:27 +0200
Subject: [PATCH 007/264] [hand_over_action] Fix action params: config file
 paths

---
 .../ros/launch/hand_over.launch               |  5 ++-
 .../ros/scripts/hand_over_action              |  8 +++-
 .../src/mdr_hand_over_action/action_states.py | 37 ++++++++++++-------
 3 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index 7affc7937..cf0fc390a 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -4,7 +4,10 @@
         <param name="move_arm_server" value="move_arm_server" />
         <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
         <param name="init_config_name" value="neutral" />
-        <param name="hand_over_dmp" value="$(find mdr_hand_over_action)/config/trajectory_weights/grasp.yaml" />
+        <param name="hand_over_dmp" value="grasp.yaml" />
+        <param name="hand_over_position_policy_parameters_file" value="800.pkl" />
+        <param name="hand_over_policy_config_dir" value="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
+        <param name="hand_over_dmp_weights_dir" value="$(find mdr_hand_over_action)/config/trajectory_weights/" />
         <param name="dmp_tau" value="30" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
index 2dbc032cd..81c0c88d5 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
@@ -14,13 +14,19 @@ class HandOverServer(object):
         gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mas_hsr_gripper_controller')
         move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
         init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
-        hand_over_dmp = rospy.get_param('~hand_over_dmp', 'hand_over_dmp')
+        hand_over_dmp = rospy.get_param('~hand_over_dmp', '')
+        pos_policy_params_file = rospy.get_param('~hand_over_position_policy_parameters_file', '')
+        dmp_weights_dir = rospy.get_param('~hand_over_dmp_weights_dir', '')
+        policy_config_dir = rospy.get_param('~hand_over_policy_config_dir', '')
         dmp_tau = rospy.get_param('~dmp_tau', 'dmp_tau')
 
         rospy.loginfo('[hand_over] Initialising state machine')
         self.action_sm = HandOverSM(gripper_controller_pkg_name=gripper_controller_pkg_name,
                                       move_arm_server=move_arm_server,
                                       init_config_name=init_config_name,
+                                      hand_over_policy_config_dir=policy_config_dir,
+                                      hand_over_dmp_weights_dir=dmp_weights_dir,
+                                      hand_over_position_policy_parameters_file=pos_policy_params_file,
                                       hand_over_dmp=hand_over_dmp,
                                       dmp_tau=dmp_tau)
         rospy.loginfo('[hand_over] State machine initialised')
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 0ec007ff5..bf1928a7f 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -1,4 +1,5 @@
 #!/usr/bin/python
+from os.path import join
 import pickle
 import numpy as np
 
@@ -21,8 +22,10 @@ def __init__(self, timeout=120.0,
                  gripper_controller_pkg_name='mas_hsr_gripper_controller',
                  move_arm_server='move_arm_server',
                  init_config_name = 'neutral',
-                 hand_over_position_policy_parameters_file = '$(find mdr_hand_over_action)/config/learned_policy_params/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl',
-                 hand_over_dmp = '$(find mdr_hand_over_action)/config/trajectory_weights/grasp.yaml',
+                 hand_over_policy_config_dir='',
+                 hand_over_position_policy_parameters_file = '800_py2.pkl',
+                 hand_over_dmp_weights_dir='',
+                 hand_over_dmp = 'grasp.yaml',
                  dmp_tau = 30.,
                  max_recovery_attempts=1):
         super(HandOverSM, self).__init__(
@@ -36,13 +39,15 @@ def __init__(self, timeout=120.0,
         self.move_arm_server = move_arm_server
 
         self.init_config_name = init_config_name
+        self.hand_over_policy_config_dir = hand_over_policy_config_dir
+        self.hand_over_dmp_weights_dir = hand_over_dmp_weights_dir 
 
         self.hand_over_dmp = hand_over_dmp
         self.dmp_tau = dmp_tau
         # self.hand_over_position_policy_parameters_file = hand_over_position_policy_parameters_file
         # self.hand_over_position_policy_parameters_file = 'contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl'
-        self.hand_over_position_policy_parameters_file = 'contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl'
-        self.hand_over_position_policy_parameters = self.load_policy_params_from_file(hand_over_position_policy_parameters_file)
+        self.hand_over_position_policy_parameters_file = join(self.hand_over_policy_config_dir, '160_py2.pkl')
+        self.hand_over_position_policy_parameters = self.load_policy_params_from_file(self.hand_over_position_policy_parameters_file)
 
         self.tf_listener = tf.TransformListener()
 
@@ -62,14 +67,14 @@ def running(self):
         obstacle = self.goal.obstacle
 
         # > Pick context-dependent hand-over position:
-        policy_parameter_a = self.learned_position_policy_parameters[0][0]
-        policy_parameter_A = self.learned_position_policy_parameters[0][1]
+        policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
+        policy_parameter_A = self.hand_over_position_policy_parameters[0][1]
 
         if posture_type == 'standing':
             context_vector = np.array([0.0, 0.0, 0.4])
-        elif posture_type == 'sitting':
+        elif posture_type == 'seated':
             context_vector = np.array([0.5, 0.0, 0.0])
-        elif posture_type == 'lying_down':
+        elif posture_type == 'lying':
             context_vector = np.array([0.0, 0.7, 0.0])
         
         # Sample upper-level policy (with no exploration) for hand-over position;
@@ -100,14 +105,14 @@ def running(self):
             # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml'
             
             ## Smoothed trajectories:
-            trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2900iters.yaml'
+            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2900iters.yaml'
             # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2100iters.yaml'
             # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_1600iters.yaml'
             # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_800iters.yaml'
 
             ## Smoothed trajectories, with corrected end_points:
             # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml'
+            trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml'
             # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml'
             # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml'
 
@@ -124,14 +129,18 @@ def running(self):
             # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_800iters.yaml'
 
 
-        self.hand_over_dmp = '$(find mdr_hand_over_action)/config/trajectory_weights/'+trajectory_weights_filename
+        self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename)
 
         ## TODO: determine whether aligning base is necessary for hand_over action here:
         ## ...
 
+        # # Go to pregrasp low position:
+        # rospy.loginfo('[hand_over] Moving back to pregrasp low position...')
+        # self.__move_arm(MoveArmGoal.NAMED_TARGET, 'pregrasp_low')
+
         # Initialize by grasping a provided object:
-        rospy.loginfo('[hand_over] Closing the gripper')
-        self.gripper.close() 
+        # rospy.loginfo('[hand_over] Closing the gripper')
+        # self.gripper.close() 
 
         # Move to chosen hand_over position, along appropriate trajectory:
         rospy.loginfo('[hand_over] Handing object over...')
@@ -186,4 +195,4 @@ def set_result(self, success):
 
     def load_policy_params_from_file(self, filename):
         with open(filename, 'rb') as f:
-            return pickle.load(f)
\ No newline at end of file
+            return pickle.load(f)

From 6fd4fc320731a1036f322a6b237bb985b093d69e Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 09:45:57 +0200
Subject: [PATCH 008/264] [hand_over_action] Change argument parsing in client
 test file

---
 .../ros/scripts/hand_over_action_client_test  | 22 +++++++++++--------
 1 file changed, 13 insertions(+), 9 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
index 043791aef..fd8394728 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
@@ -4,31 +4,34 @@ import argparse
 import rospy
 import actionlib
 import numpy as np
+import ast
 
 from mdr_hand_over_action.msg import HandOverAction, HandOverGoal
 
 if __name__ == '__main__':
-    posture_contexts = ['standing', 'sitting', 'lying_down']
+    posture_contexts = ['standing', 'seated', 'lying']
     obstacle_contexts = [0, 1]                                 # 0: no obstacle, 1: obstacle
     
     parser = argparse.ArgumentParser(description='help text')
     parser.add_argument("--context", "-c", help="list of context parameters. E.g.: ['sitting', 0]") 
     args = parser.parse_args()
 
-    if not args.context:
+    context_params = ast.literal_eval(args.context)
+
+    if not context_params:
         print('No context parameters provided!')
         print('Example inputs: ')
         print('- python hand_over_action_client_test -c "[\'standing\', 0]"')
         print('- python hand_over_action_client_test -c "[\'sitting\', 1]"')
-    elif len(args.context) != 2:
+    elif len(context_params) != 2:
         print('Expected two context parameters: posture and obstacle!')
-    elif args.context[0] not in posture_contexts:
+    elif context_params[0] not in posture_contexts:
         print('Invalid posture context!')
-    elif args.context[1] not in obstacle_contexts:
+    elif context_params[1] not in obstacle_contexts:
         print('Invalid obstacle context!')
     else:
-        posture_context = args.context[0]
-        obstacle_context = args.context[1]
+        posture_context = context_params[0]
+        obstacle_context = bool(context_params[1])
 
         rospy.init_node('hand_over_action_client_test')
 
@@ -37,8 +40,9 @@ if __name__ == '__main__':
 
         goal = HandOverGoal()
 
-        goal.posture_type = args.context[0]
-        goal.obstacle = args.context[1]
+        rospy.loginfo('Setting goal for a %s person, with %s obstacle in-between...' % (posture_context, 'an' if obstacle_context else 'no'))
+        goal.posture_type = posture_context
+        goal.obstacle = obstacle_context
 
         timeout = 15.0
         rospy.loginfo('Sending action lib goal to hand_over_server')

From bd583bf04fad0c7f4526161e14b1f139e07d359b Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 12:02:57 +0200
Subject: [PATCH 009/264] [hand_over_action] Add force sensing for object
 reception (testing)

---
 .../src/mdr_hand_over_action/action_states.py | 64 +++++++++++++++++--
 1 file changed, 58 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index bf1928a7f..0b639a209 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -2,11 +2,14 @@
 from os.path import join
 import pickle
 import numpy as np
+from collections import deque
+from scipy.stats import norm
 
 import rospy
 import tf
 import actionlib
 from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import WrenchStamped
 
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
@@ -20,6 +23,7 @@
 class HandOverSM(ActionSMBase):
     def __init__(self, timeout=120.0,
                  gripper_controller_pkg_name='mas_hsr_gripper_controller',
+                 force_sensor_topic='/hsrb/wrist_wrench/raw',
                  move_arm_server='move_arm_server',
                  init_config_name = 'neutral',
                  hand_over_policy_config_dir='',
@@ -50,6 +54,11 @@ def __init__(self, timeout=120.0,
         self.hand_over_position_policy_parameters = self.load_policy_params_from_file(self.hand_over_position_policy_parameters_file)
 
         self.tf_listener = tf.TransformListener()
+        self.force_sensor_topic = force_sensor_topic
+
+        self.force_measurements_window_size = 5000
+        self.force_measurements_x = deque([], maxlen=self.force_measurements_window_size)
+        self.force_detection_threshold = 5.
 
     def init(self):
         try:
@@ -138,7 +147,7 @@ def running(self):
         # rospy.loginfo('[hand_over] Moving back to pregrasp low position...')
         # self.__move_arm(MoveArmGoal.NAMED_TARGET, 'pregrasp_low')
 
-        # Initialize by grasping a provided object:
+        # # Initialize by grasping a provided object:
         # rospy.loginfo('[hand_over] Closing the gripper')
         # self.gripper.close() 
 
@@ -146,13 +155,28 @@ def running(self):
         rospy.loginfo('[hand_over] Handing object over...')
         self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
 
-        # Wait for person to hold object:
-        rospy.loginfo('[hand_over] Waiting before releasing object...')
-        rospy.sleep(5.)
+        # Naive object release strategy:
+        # # --------------------------------------------------------------
+        # # Wait for person to hold object:
+        # rospy.loginfo('[hand_over] Waiting before releasing object...')
+        # rospy.sleep(5.)
+
+        # # Release the object:
+        # rospy.loginfo('[hand_over] Opening the gripper...')
+        # self.gripper.open()
+        # #  --------------------------------------------------------------
+
+        # Force sensing object release strategy:
+        # --------------------------------------------------------------
+        # Wait for person to pull object:
+        rospy.loginfo('[hand_over] Waiting for object to be received...')
+        object_reception_detected = self.detect_object_reception()
 
         # Release the object:
-        rospy.loginfo('[hand_over] Opening the gripper...')
-        self.gripper.open()
+        if object_reception_detected:
+            rospy.loginfo('[hand_over] Opening the gripper to release object...')
+            self.gripper.open()
+        #  --------------------------------------------------------------
 
         # Return to a neutral arm position:
         rospy.loginfo('[hand_over] Moving back to neutral position...')
@@ -196,3 +220,31 @@ def set_result(self, success):
     def load_policy_params_from_file(self, filename):
         with open(filename, 'rb') as f:
             return pickle.load(f)
+
+    def detect_object_reception(self):
+        self.force_sensor_sub = rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
+        
+        mu_0 = -10
+        mu_1 = -20
+        std = 1
+        cumsum = 0
+
+        pdf_0 = norm(mu_0, std)
+        pdf_1 = norm(mu_1, std)
+
+        while cumsum >= self.force_detection_threshold:
+            for sensor_reading in self.force_measurements_x:
+                cumsum += np.log(pdf_1.pdf(sensor_reading) / pdf_0.pdf(sensor_reading))
+
+            print()
+            print('[hand_over DEBUG] Force Measurements:')
+            print(self.force_measurements_x)
+            print('[hand_over DEBUG] Current cumsum:', cumsum)
+                
+        rospy.loginfo('[hand_over] Object reception detected!')
+        return True
+        
+        # rospy.spin()
+        
+    def force_sensor_cb(self, force_sensor_msg):
+        self.force_measurements_x.append(force_sensor_msg.wrench.force.x)
\ No newline at end of file

From f607f435dcebeed75ad68c77f4dd26ddc7ee2cb9 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 15:55:31 +0200
Subject: [PATCH 010/264] [hand_over_action] Implement change detection for
 object reception

---
 .../src/mdr_hand_over_action/action_states.py | 68 +++++++++++--------
 1 file changed, 41 insertions(+), 27 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 0b639a209..f34460469 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -2,7 +2,6 @@
 from os.path import join
 import pickle
 import numpy as np
-from collections import deque
 from scipy.stats import norm
 
 import rospy
@@ -56,9 +55,12 @@ def __init__(self, timeout=120.0,
         self.tf_listener = tf.TransformListener()
         self.force_sensor_topic = force_sensor_topic
 
-        self.force_measurements_window_size = 5000
-        self.force_measurements_x = deque([], maxlen=self.force_measurements_window_size)
+        self.latest_force_measurement_x = 0.
+        self.latest_force_measurement_z = 0.
+        self.cumsum = 0
+        self.cumsum_z = 0
         self.force_detection_threshold = 5.
+        self.object_reception_detected = False
 
     def init(self):
         try:
@@ -170,20 +172,31 @@ def running(self):
         # --------------------------------------------------------------
         # Wait for person to pull object:
         rospy.loginfo('[hand_over] Waiting for object to be received...')
-        object_reception_detected = self.detect_object_reception()
-
-        # Release the object:
-        if object_reception_detected:
+        rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
+        self.object_reception_detected = False
+        self.cumsum = 0.
+        self.cumsum_z = 0.
+
+        start_time = rospy.get_time()
+        while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected:
+            self.detect_object_reception()
+            rospy.sleep(0.1)
+
+        # If a pull is detected, release the object:
+        if self.object_reception_detected:        
             rospy.loginfo('[hand_over] Opening the gripper to release object...')
             self.gripper.open()
-        #  --------------------------------------------------------------
+        else:
+            rospy.loginfo('[hand_over] Keeping object since no reception was detected')
 
         # Return to a neutral arm position:
         rospy.loginfo('[hand_over] Moving back to neutral position...')
         self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
 
-        # For now, assume unconditional success:
-        self.result = self.set_result(True)
+        if self.object_reception_detected:
+            self.result = self.set_result(True)
+        else:
+            self.result = self.set_result(False)
         return FTSMTransitions.DONE
 
     def __move_arm(self, goal_type, goal):
@@ -222,29 +235,30 @@ def load_policy_params_from_file(self, filename):
             return pickle.load(f)
 
     def detect_object_reception(self):
-        self.force_sensor_sub = rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
-        
         mu_0 = -10
         mu_1 = -20
         std = 1
-        cumsum = 0
 
         pdf_0 = norm(mu_0, std)
         pdf_1 = norm(mu_1, std)
 
-        while cumsum >= self.force_detection_threshold:
-            for sensor_reading in self.force_measurements_x:
-                cumsum += np.log(pdf_1.pdf(sensor_reading) / pdf_0.pdf(sensor_reading))
-
-            print()
-            print('[hand_over DEBUG] Force Measurements:')
-            print(self.force_measurements_x)
-            print('[hand_over DEBUG] Current cumsum:', cumsum)
-                
-        rospy.loginfo('[hand_over] Object reception detected!')
-        return True
-        
-        # rospy.spin()
+        self.cumsum += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
+        self.cumsum_z += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
+        if self.cumsum > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
+            rospy.loginfo('[hand_over] Object reception detected!')
+            self.object_reception_detected = True
         
     def force_sensor_cb(self, force_sensor_msg):
-        self.force_measurements_x.append(force_sensor_msg.wrench.force.x)
\ No newline at end of file
+        self.latest_force_measurement_x = force_sensor_msg.wrench.force.x
+        self.latest_force_measurement_z = force_sensor_msg.wrench.force.z
+
+        print('[hand_over DEBUG] Force Measurements:')
+        # print(self.force_measurements_x)
+        print('[hand_over DEBUG] Current cumsum:', self.cumsum)
+        print('[hand_over DEBUG] Current cumsum in z:', self.cumsum_z)
+        print("*********************")
+        print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
+        print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
+        print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
+        print("\n\n")
+

From 6cf8b05c6a58678e3de431dde93f77fad3869654 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 17:49:14 +0200
Subject: [PATCH 011/264] [demo_context_aware_hand_over] Add Alex's fixes to
 scenario files

---
 .../mdr_find_people/ros/launch/find_people.launch      |  2 +-
 .../ros/scripts/find_people_client_test                | 10 ++++++++--
 .../ros/src/mdr_find_people/action_states.py           |  2 +-
 .../ros/src/mdr_find_people/find_people.py             |  3 ++-
 .../ros/scripts/perceive_plane_client_test             |  2 +-
 .../ros/launch/hand_over.launch                        |  2 +-
 6 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
index 8b30a8ee5..03befefc6 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="pointcloud_topic" default="/mdr_perception/rectified_points"/>
+    <arg name="pointcloud_topic" default="/hsrb/head_rgbd_sensor/depth_registered/rectified_points"/>
 
     <node pkg="mdr_find_people" type="find_people_server" name="find_people_server" output="screen" >
         <param name="pointcloud_topic" type="string" value="$(arg pointcloud_topic)" />
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
index ffa05ad40..a6b7dcf47 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
@@ -19,8 +19,14 @@ if __name__ == '__main__':
         print('Sending goal...')
         client.send_goal(goal)
         print('Waiting for result...')
-        client.wait_for_result(rospy.Duration.from_sec(20))
-        rospy.loginfo(client.get_result())
+        client.wait_for_result(rospy.Duration.from_sec(120))
+
+        result = client.get_result()
+        if result is None:
+            rospy.logerr('No people found')
+        rospy.loginfo('Found {0} people'.format(len(result.person_list.persons)))
+        for person in result.person_list.persons:
+            rospy.loginfo('Image_size: width -- {0}, height -- {1}'.format(person.rgb_image.width, person.rgb_image.height))
     except Exception as e:
         rospy.logerr('Failed to execute find people action: {}'.format(repr(e)))
 
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index b9759a7bc..a3f63b0d3 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -48,7 +48,7 @@ def __init__(self):
                              output_keys=['find_people_result', 'error_message'])
 
         self._listener = tf.TransformListener()
-        self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
+        self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/hsrb/head_rgbd_sensor/depth_registered/rectified_points')
 
 
     def execute(self, userdata):
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index ee4485a64..d84e96154 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -9,7 +9,8 @@
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz, draw_labeled_boxes
+                                      crop_cloud_to_xyz
+from mas_perception_libs.visualization import draw_labeled_boxes
 from ssd_keras_ros import SSDKerasObjectDetector
 
 
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client_test
index ce4ea032e..ee04e5f61 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client_test
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client_test
@@ -6,7 +6,7 @@ from mdr_perceive_plane_action.msg import PerceivePlaneAction, PerceivePlaneGoal
 
 if __name__ == '__main__':
     rospy.init_node('perceive_plane_client_tester')
-    client = actionlib.SimpleActionClient('perceive_plane_server', PerceivePlaneAction)
+    client = actionlib.SimpleActionClient('/mdr_actions/perceive_plane_server', PerceivePlaneAction)
     client.wait_for_server()
     goal = PerceivePlaneGoal()
     goal.plane_config = 'table'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
index 28ea9b089..26ca4a495 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
@@ -9,7 +9,7 @@
     </include>
 
     <!-- Actions -->
-    <include file="$(find mdr_move_arm_action)/ros/launch/move_arm.launch" />
+    <include file="$(find mas_hsr_move_arm_action)/ros/launch/move_arm.launch" />
     <include file="$(find mdr_move_base_action)/ros/launch/move_base_action.launch" />
     <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
     <include file="$(find mdr_hand_over_action)/ros/launch/hand_over.launch" />

From 3ab4eecc5569518f0b09b6ae741cebf8bffd77b5 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 22:24:09 +0200
Subject: [PATCH 012/264] [hand_over_action] Extend object reception to check
 both dims: x and z

---
 .../src/mdr_hand_over_action/action_states.py | 66 ++++++++++---------
 1 file changed, 35 insertions(+), 31 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index f34460469..51a69ed24 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -57,7 +57,7 @@ def __init__(self, timeout=120.0,
 
         self.latest_force_measurement_x = 0.
         self.latest_force_measurement_z = 0.
-        self.cumsum = 0
+        self.cumsum_x = 0
         self.cumsum_z = 0
         self.force_detection_threshold = 5.
         self.object_reception_detected = False
@@ -76,29 +76,34 @@ def init(self):
     def running(self):
         posture_type = self.goal.posture_type
         obstacle = self.goal.obstacle
-
-        # > Pick context-dependent hand-over position:
-        policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
-        policy_parameter_A = self.hand_over_position_policy_parameters[0][1]
-
-        if posture_type == 'standing':
-            context_vector = np.array([0.0, 0.0, 0.4])
-        elif posture_type == 'seated':
-            context_vector = np.array([0.5, 0.0, 0.0])
-        elif posture_type == 'lying':
-            context_vector = np.array([0.0, 0.7, 0.0])
-        
-        # Sample upper-level policy (with no exploration) for hand-over position;
-        # by calculating the mean of the linear-Gaussian model, given context vector s:
-        hand_over_position = np.squeeze(policy_parameter_a + context_vector.dot(policy_parameter_A))
+        context_aware = self.goal.context_aware
 
         hand_over_pose = PoseStamped()
         hand_over_pose.header.frame_id = 'base_link'
         hand_over_pose.header.stamp = rospy.Time.now()
+        if context_aware:
+            # > Pick context-dependent hand-over position:
+            policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
+            policy_parameter_A = self.hand_over_position_policy_parameters[0][1]
+
+            if posture_type == 'standing':
+                context_vector = np.array([0.0, 0.0, 0.4])
+            elif posture_type == 'seated':
+                context_vector = np.array([0.5, 0.0, 0.0])
+            elif posture_type == 'lying':
+                context_vector = np.array([0.0, 0.7, 0.0])
+        
+            # Sample upper-level policy (with no exploration) for hand-over position;
+            # by calculating the mean of the linear-Gaussian model, given context vector s:
+            hand_over_position = np.squeeze(policy_parameter_a + context_vector.dot(policy_parameter_A))
 
-        hand_over_pose.pose.position.x = hand_over_position[0]
-        hand_over_pose.pose.position.y = hand_over_position[1]
-        hand_over_pose.pose.position.z = hand_over_position[2]
+            hand_over_pose.pose.position.x = hand_over_position[0]
+            hand_over_pose.pose.position.y = hand_over_position[1]
+            hand_over_pose.pose.position.z = hand_over_position[2]
+        else:
+            hand_over_pose.pose.position.x = 0.5
+            hand_over_pose.pose.position.y = 0.078
+            hand_over_pose.pose.position.z = 0.8
 
         hand_over_pose.pose.orientation.x = 0.000
         hand_over_pose.pose.orientation.y = 0.000
@@ -174,7 +179,7 @@ def running(self):
         rospy.loginfo('[hand_over] Waiting for object to be received...')
         rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
         self.object_reception_detected = False
-        self.cumsum = 0.
+        self.cumsum_x = 0.
         self.cumsum_z = 0.
 
         start_time = rospy.get_time()
@@ -242,9 +247,9 @@ def detect_object_reception(self):
         pdf_0 = norm(mu_0, std)
         pdf_1 = norm(mu_1, std)
 
-        self.cumsum += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
+        self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
         self.cumsum_z += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
-        if self.cumsum > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
+        if self.cumsum_x > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
         
@@ -252,13 +257,12 @@ def force_sensor_cb(self, force_sensor_msg):
         self.latest_force_measurement_x = force_sensor_msg.wrench.force.x
         self.latest_force_measurement_z = force_sensor_msg.wrench.force.z
 
-        print('[hand_over DEBUG] Force Measurements:')
-        # print(self.force_measurements_x)
-        print('[hand_over DEBUG] Current cumsum:', self.cumsum)
-        print('[hand_over DEBUG] Current cumsum in z:', self.cumsum_z)
-        print("*********************")
-        print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
-        print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
-        print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
-        print("\n\n")
+        # print('[hand_over DEBUG] Force Measurements:')
+        # print('[hand_over DEBUG] Current cumsum in x:', self.cumsum_x)
+        # print('[hand_over DEBUG] Current cumsum in z:', self.cumsum_z)
+        # print("*********************")
+        # print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
+        # print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
+        # print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
+        # print("\n\n")
 

From 9cc54574a60c6675344427d39066890fd1f79a04 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 20 Jul 2019 22:26:38 +0200
Subject: [PATCH 013/264] [hand_over_action] Add choice for context-awareness
 in action & scenario

---
 .../mdr_hand_over_action/ros/action/HandOver.action        | 1 +
 .../config/context_aware_hand_over_sm.yaml                 | 7 +++++--
 .../scenario_states/hand_over.py                           | 2 ++
 3 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
index 3c74c4951..3db6f3316 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
@@ -1,6 +1,7 @@
 # goal
 string posture_type
 bool obstacle
+bool context_aware
 ---
 # result
 bool success
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
index b11929d4b..b831ebefd 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -32,8 +32,8 @@ state_descriptions:
             - argument:
                 name: posture_height_width_ratio_ranges
                 value:
-                    standing: [2.0, 5.0]
-                    seated: [0.6, 2.0]
+                    standing: [1.65, 5.0]
+                    seated: [0.6, 1.65]
                     lying: [0.0, 0.6]
     - state:
         name: HAND_OVER
@@ -56,3 +56,6 @@ state_descriptions:
             - argument:
                 name: obstacle_present
                 value: False
+            - argument:
+                name: context_aware
+                value: False
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 58974a7c4..4853b6383 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -14,6 +14,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.action_server = kwargs.get('action_server', 'hand_over_server')
         self.obstacle_present = kwargs.get('obstacle_present', False)
         self.timeout = kwargs.get('timeout', 120.)
+        self.context_aware = kwargs.get('context_aware', True)
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.retry_count = 0
@@ -24,6 +25,7 @@ def execute(self, userdata):
         goal = HandOverGoal()
         goal.posture_type = userdata.posture
         goal.obstacle = self.obstacle_present
+        goal.context_aware = self.context_aware
 
         # calling the actionlib server and waiting for the execution to end
         rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))

From f6ac96d4a8878ac1a9277857fc6fde39e6f0394d Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Mon, 22 Jul 2019 16:42:52 +0200
Subject: [PATCH 014/264] [hand_over_action] Remove explicit vocalization of
 detected posture

---
 .../ros/src/mdr_hand_over_action/action_states.py          | 7 ++++---
 .../scenario_states/hand_over.py                           | 3 ++-
 .../scenario_states/identify_posture.py                    | 2 +-
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 51a69ed24..4a90071dc 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -101,6 +101,7 @@ def running(self):
             hand_over_pose.pose.position.y = hand_over_position[1]
             hand_over_pose.pose.position.z = hand_over_position[2]
         else:
+            # Pick context-independent hand-over position:
             hand_over_pose.pose.position.x = 0.5
             hand_over_pose.pose.position.y = 0.078
             hand_over_pose.pose.position.z = 0.8
@@ -112,7 +113,6 @@ def running(self):
 
         pose_base_link = self.tf_listener.transformPose('base_link', hand_over_pose)
 
-        ## TODO:
         # > Pick context_dependent hand-over trajectory shape:
         if not obstacle:
             trajectory_weights_filename = 'grasp.yaml'
@@ -193,6 +193,7 @@ def running(self):
             self.gripper.open()
         else:
             rospy.loginfo('[hand_over] Keeping object since no reception was detected')
+        # --------------------------------------------------------------        
 
         # Return to a neutral arm position:
         rospy.loginfo('[hand_over] Moving back to neutral position...')
@@ -249,6 +250,7 @@ def detect_object_reception(self):
 
         self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
         self.cumsum_z += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
+        
         if self.cumsum_x > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
@@ -264,5 +266,4 @@ def force_sensor_cb(self, force_sensor_msg):
         # print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
         # print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
         # print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
-        # print("\n\n")
-
+        # print("\n\n")
\ No newline at end of file
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 4853b6383..035979cb9 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -29,7 +29,8 @@ def execute(self, userdata):
 
         # calling the actionlib server and waiting for the execution to end
         rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))
-        self.say('Handing item over to a {0} person'.format(userdata.posture))
+	    # Disabled temporarily for hand-over experiments:
+        # self.say('Handing item over to a {0} person'.format(userdata.posture))
         self.client.send_goal(goal)
         self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
         result = self.client.get_result()
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
index c5d4b6c5f..d3596e7c6 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
@@ -12,7 +12,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.posture_height_width_ratio_ranges = kwargs.get('posture_height_width_ratio_ranges', {})
 
     def execute(self, userdata):
-        self.say('I will only hand over the item to the first detected person')
+        self.say('I will hand over the object to the first detected person')
 
         first_person_image_bb_width = float(userdata.person_list.persons[0].rgb_image.width)
         first_person_image_bb_height = float(userdata.person_list.persons[0].rgb_image.height)

From 4b2e4a566fae613bc6ebcba3f5fef988db5aeeb0 Mon Sep 17 00:00:00 2001
From: Lucy <robotics@mail.inf.h-brs.de>
Date: Fri, 11 Oct 2019 12:32:44 +0200
Subject: [PATCH 015/264] [hand_over_action] Minor modifications (debugging
 outputs, fixes, parameter tweaking)

---
 .../ros/src/mdr_hand_over_action/action_states.py   | 13 ++++++++-----
 .../config/context_aware_hand_over_sm.yaml          |  2 +-
 .../ros/launch/hand_over.launch                     |  2 --
 .../scenario_states/detect_person.py                |  2 +-
 .../scenario_states/hand_over.py                    |  2 +-
 5 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 4a90071dc..8c0bdae7f 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -59,7 +59,7 @@ def __init__(self, timeout=120.0,
         self.latest_force_measurement_z = 0.
         self.cumsum_x = 0
         self.cumsum_z = 0
-        self.force_detection_threshold = 5.
+        self.force_detection_threshold = 1.
         self.object_reception_detected = False
 
     def init(self):
@@ -176,6 +176,7 @@ def running(self):
         # Force sensing object release strategy:
         # --------------------------------------------------------------
         # Wait for person to pull object:
+        rospy.sleep(1.)
         rospy.loginfo('[hand_over] Waiting for object to be received...')
         rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
         self.object_reception_detected = False
@@ -241,7 +242,7 @@ def load_policy_params_from_file(self, filename):
             return pickle.load(f)
 
     def detect_object_reception(self):
-        mu_0 = -10
+        mu_0 = -2
         mu_1 = -20
         std = 1
 
@@ -250,8 +251,10 @@ def detect_object_reception(self):
 
         self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
         self.cumsum_z += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
-        
-        if self.cumsum_x > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
+
+        print(self.cumsum_x)
+        if self.cumsum_x > self.force_detection_threshold:
+        # if self.cumsum_x > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
         
@@ -266,4 +269,4 @@ def force_sensor_cb(self, force_sensor_msg):
         # print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
         # print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
         # print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
-        # print("\n\n")
\ No newline at end of file
+        # print("\n\n")
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
index b831ebefd..2d1ccec59 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -58,4 +58,4 @@ state_descriptions:
                 value: False
             - argument:
                 name: context_aware
-                value: False
+                value: True
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
index 26ca4a495..ee3e3b5ac 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
@@ -9,8 +9,6 @@
     </include>
 
     <!-- Actions -->
-    <include file="$(find mas_hsr_move_arm_action)/ros/launch/move_arm.launch" />
-    <include file="$(find mdr_move_base_action)/ros/launch/move_base_action.launch" />
     <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
     <include file="$(find mdr_hand_over_action)/ros/launch/hand_over.launch" />
 
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
index 860d2aed2..558e418b4 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/detect_person.py
@@ -29,7 +29,7 @@ def execute(self, userdata):
         self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
         result = self.client.get_result()
 
-        if result:
+        if result and result.person_list.persons:
             rospy.loginfo('[detect_person] Found {0} people'.format(len(result.person_list.persons)))
             userdata.person_list = result.person_list
             return 'succeeded'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 035979cb9..525c2c57b 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -30,7 +30,7 @@ def execute(self, userdata):
         # calling the actionlib server and waiting for the execution to end
         rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))
 	    # Disabled temporarily for hand-over experiments:
-        # self.say('Handing item over to a {0} person'.format(userdata.posture))
+        self.say('Handing item over to a {0} person'.format(userdata.posture))
         self.client.send_goal(goal)
         self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
         result = self.client.get_result()

From 1a99ec9fbb177d58e5913f755ed6b398df2af1b8 Mon Sep 17 00:00:00 2001
From: Lucy <robotics@mail.inf.h-brs.de>
Date: Thu, 12 Mar 2020 16:06:06 +0100
Subject: [PATCH 016/264] [context_aware_hand_over/identify_posture] Adapted
 the Person msg use to the new msg format

---
 .../scenario_states/identify_posture.py                       | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
index d3596e7c6..f98b50ef6 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
@@ -14,8 +14,8 @@ def __init__(self, save_sm_state=False, **kwargs):
     def execute(self, userdata):
         self.say('I will hand over the object to the first detected person')
 
-        first_person_image_bb_width = float(userdata.person_list.persons[0].rgb_image.width)
-        first_person_image_bb_height = float(userdata.person_list.persons[0].rgb_image.height)
+        first_person_image_bb_width = float(userdata.person_list.persons[0].body_image.width)
+        first_person_image_bb_height = float(userdata.person_list.persons[0].body_image.height)
         bb_height_width_ratio = first_person_image_bb_height / first_person_image_bb_width
         rospy.loginfo('[identify_posture] Height width bb ratio: {0}'.format(bb_height_width_ratio))
 

From e75cd6e1138e95ccce3690fec30679c6f1123cb4 Mon Sep 17 00:00:00 2001
From: Lucy <robotics@mail.inf.h-brs.de>
Date: Thu, 12 Mar 2020 16:41:18 +0100
Subject: [PATCH 017/264] [context_aware_hand_over/identify_posture] The object
 is now handed over to the closest person

---
 .../scenario_states/identify_posture.py       | 24 ++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
index f98b50ef6..d8466725b 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
@@ -1,3 +1,5 @@
+import numpy as np
+
 import rospy
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
@@ -12,10 +14,11 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.posture_height_width_ratio_ranges = kwargs.get('posture_height_width_ratio_ranges', {})
 
     def execute(self, userdata):
-        self.say('I will hand over the object to the first detected person')
+        self.say('I will hand over the object to the closest person')
 
-        first_person_image_bb_width = float(userdata.person_list.persons[0].body_image.width)
-        first_person_image_bb_height = float(userdata.person_list.persons[0].body_image.height)
+        closest_person_idx = find_closest_person(userdata.person_list.persons)
+        first_person_image_bb_width = float(userdata.person_list.persons[closest_person_idx].body_image.width)
+        first_person_image_bb_height = float(userdata.person_list.persons[closest_person_idx].body_image.height)
         bb_height_width_ratio = first_person_image_bb_height / first_person_image_bb_width
         rospy.loginfo('[identify_posture] Height width bb ratio: {0}'.format(bb_height_width_ratio))
 
@@ -27,3 +30,18 @@ def execute(self, userdata):
         rospy.loginfo('[identify_posture] Found a {0} person'.format(person_posture))
         userdata.posture = person_posture
         return 'succeeded'
+
+
+def find_closest_person(people):
+    '''Returns the index of the person closest to the robot.
+
+    Keyword arguments:
+    people: Sequence[mas_perception_msgs.msg.Person] -- list of people in the current scene
+
+    '''
+    distances = np.zeros(len(people))
+    for i, person in enumerate(people):
+        distances[i] = np.linalg.norm([person.pose.pose.position.x,
+                                       person.pose.pose.position.y,
+                                       person.pose.pose.position.z])
+    return np.argmin(distances)

From 4fba100dc9187a282035b41f56ae6af0203fa688 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 12 Mar 2020 16:52:56 +0100
Subject: [PATCH 018/264] [hand-over-action] Remove unnecessary learned policy
 parameter files

---
 .../learned_policy_parameters/800_py2.pkl     | Bin 6017 -> 0 bytes
 .../learned_policy_parameters/80_py2.pkl      | Bin 6042 -> 0 bytes
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl | Bin 4176 -> 0 bytes
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl | Bin 4176 -> 0 bytes
 ....7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl | Bin 4176 -> 0 bytes
 ...=> learned_position_policy_parameters.pkl} | Bin
 ..._corrected_obstacle_avoiding_weights.yaml} |   0
 ...d_obstacle_avoiding_weights_1600iters.yaml | 903 ------------------
 ...d_obstacle_avoiding_weights_2100iters.yaml | 903 ------------------
 ...ed_obstacle_avoiding_weights_800iters.yaml | 903 ------------------
 ...smoothed_corrected_underhand_weights.yaml} |   0
 ...corrected_underhand_weights_1600iters.yaml | 903 ------------------
 ...corrected_underhand_weights_2100iters.yaml | 903 ------------------
 ...corrected_underhand_weights_2900iters.yaml | 903 ------------------
 ...corrected_underhand_weights_3700iters.yaml | 903 ------------------
 ...corrected_underhand_weights_4500iters.yaml | 903 ------------------
 ...corrected_underhand_weights_5300iters.yaml | 903 ------------------
 ...corrected_underhand_weights_6100iters.yaml | 903 ------------------
 ...corrected_underhand_weights_6900iters.yaml | 903 ------------------
 ..._corrected_underhand_weights_800iters.yaml | 903 ------------------
 ...d_smoothed_obstacle_avoiding_weights.yaml} |   0
 ...d_obstacle_avoiding_weights_1600iters.yaml | 903 ------------------
 ...d_obstacle_avoiding_weights_2100iters.yaml | 903 ------------------
 ...ed_obstacle_avoiding_weights_800iters.yaml | 903 ------------------
 24 files changed, 13545 deletions(-)
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
 rename mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/{160_py2.pkl => learned_position_policy_parameters.pkl} (100%)
 rename mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/{learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml => learned_smoothed_corrected_obstacle_avoiding_weights.yaml} (100%)
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
 rename mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/{learned_smoothed_corrected_underhand_weights_7700iters.yaml => learned_smoothed_corrected_underhand_weights.yaml} (100%)
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
 rename mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/{learned_smoothed_obstacle_avoiding_weights_2900iters.yaml => learned_smoothed_obstacle_avoiding_weights.yaml} (100%)
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
 delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/800_py2.pkl
deleted file mode 100644
index 58211955fe700b92708c53581a22c5e63b58fc2b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 6017
zcmb_g2Uk=}vn2@%5+o{uiXfnbs|bo1ro}`OWE4??AP5&kg&~O9L2^cNPLf7JlyJ$g
zR_!15-agmweZSzrT2OsXcXf4zy{nJWBKK9r%aXUDxy7Y<p)V_5loviNEq(gdHmfu*
zxA;|Ad1*y%xvjzM&n(+l&;GnMn8%yd8Z2_Na*Lnk<(Apzz0w3*gXJT0Gc&UsgViHz
z{(tebyztdI!@fF$wccP;Z`iLl90-gz=dWd*!S+8YpOwEY;Yzzl7F>}ZX*d{f7H?i}
zu-6+L0^|SayJB!mOiYaXAO8INC*G{w;FQx?VEHJ3YoyU!*3l+&(u7f6l8isND#Owu
zCLN~F2FNBRq#Cn2!=VBi;;c8glp9=g3d{=rJ%U>S?Q+)}JT$v3HM<TMIB1&7G|esY
zOU7kOzWT|o?u<L;!elQ+#&qwnSP@o%UNp!awxl~q7O;_vab1YaU}+zIp<SkQcd!(L
zm9lcQ$e#RzA+$w`(H)Ik(<rO?uGlK1p?fky!{<<crv#Ha!x7C&Prc!2xxwqdR(fkz
z9@86+|JTYBf2@q5mH%Hq1|MzflX`=1xxp`|-f)WUkMI3INBL_e1n3Qce@y7DGo1co
zg3;B?%ofdOWEHj8$b#{?e3czc%h!Tz7%@i&w&R<0x<7?677eK$@*M+|QGHu>(TJ^z
z92h^^>aoZ{D`h8!>}M%<vBN<DANL+l)8)eGC3WX62Ff%=Sx@>qTh=U}!FcJ`Nl#38
zQIcPJbX)2%!3pRLkamWzO}=Q;+c3}c=#a)c+PiV7E6F>m*Tkhl$B?_yOffM#Q6|e#
zrA?p7#sZCQLk~wKQj;Qeq3EFO9FZW4iD=a({kn%?8TgD*ZLlG$(wZVQS{4|wC<FU5
zb((d2vLYK8ti-G~ZR9HEf|(m84g#67rVKIWQ{JD06|BnMO>K33QDr0=pK9}e%QSXm
z^R4W0WMl6mt>!nV6+e8@C)KA>&p~i@c>I;{JkFlL&IL|ykOq#>cR1w~UENGMMCu=6
z8Qs{{UhG|twu8|zAI1tkY^P%yv7x;fmiZUxvXWg6!k_#tv-na>(M|g0*Lf`aQVx<X
zv>f1@Y3>{&YlSj@Q<f-~%8o^PxELu$$mj*#)mTb0n;eKCX>g*9by|c0`Gs+w#rZy}
zu`AuE(N5fp(eff$gHb#44_ld#R$8VxTz<I8P9lc!odb(lPZuZ6tLPA8xF1Z;nq2?p
zvZK#YZMU{dm}Sr!PH>Qs6M|OxeTw1{tVM;&jt|8PcPmhTO;b^fmg`O)vaanhpL}J1
zQ52=u4$)5@OFz>N%Z0(7M3rAmRHpN)bbb(1o;LWI@Eo0}wWI7U-GS0of>Gvq`3LD|
zO!{R{OL~)+|0heSp=e=&<}z;^?cuJr)kBQEeL|r>0{vJnE6t$Xg*_R&c|SRglR6?8
zk+0GtRgYlIH#e8@&zY$(`b!^=Gbx)c9LUhs=TBr=dMMMqBj}+ASEPe0#DvKNbJ_gx
z7{<%zWb~6KswtxR6PlR!)7aD|jc2Bz(b`#CUIpSK|4!*2@WsF#CsZe1P3DWfxaVlF
zm0t-ken3Crt3@XLVmN0G9q1HOXyq-smVw$sp9OYq!g$mQ^H~RF;x>!PBgR%*WjmHR
zrdR``*zvv#qt&O%T&wIP7!RR=`!+bdrYs#ECFe2X%l91oI3_LF^yEUUV$qv9@<SG=
z9BUa^{ERW(br?g7@;{y_({XltnKwpd<AP!U@RcyXals{;VfMfejHktM-Qm=<M-Q+;
zTWw^8Dfazu7-^u?=P(uz&_OwhXw@TQvLI6&RoH(H5u=Pz^nF2}R~<nk<s^K6S}ud=
zqx>(F%vD$wYw{0Ny@->(0`B1Z_^5Q}V(A9W{q?tef8zO?##n{0T(;2pn3hZD<!qVO
z;R_AEC{6BB(*2I&$&kAR0ge>@awk~KNSEt1s;K?^E>r5WqId`y#&+nZpa^O(g&&a)
zdtE5SIvKq0a0H7!l)q;?`OA*m0g9`qWHd*{-7Qr%aj66KJlXPbh3;F7;$W0a2dSc4
zmEM>R4p-<eKviip8lu!mb^4Xm%Cu7Gcq2ne7P59jz4LQ*lXYpit=<N|X4#qiV--ex
zV6u@3L%e$JEbSp^3%aXh3FDZeH-2D>qaD`5Pcnkhhl6dh<5yILNq3yW*aaWf_hj5{
zbyIq!>b`8sHn%j%j9s~FW`y$0vLK^=#e}bNt284w&d1=J4y)$WS?iJEL6ES1AA0Tm
zNf^&ys1#H8?fhVTLA0Oz+YWp5#mPAQkZ-amyIyqNCHaOf#<$g-MPC?zw%cdjV7!CD
zJMtT|Qgf7^uE%J>Sz0xEitekCk;7$}JAaNZ{*ieZc}H@dy~I%i4`@o!$1<q0f*N}B
z3yrE{8M(^W#1uD~W~tfT;@$(CyQG!&kaX$~1j$G?%?`Q8Vx5zX94$t<Ugr*I;N(`}
zr<1YAlv2ecTPf(*fXJ|4QA5}1r!Zc3%;Yw+vXKd+CAK|I2OZIHV)j-ctzVDU%6S#v
z!ekVE$2lmKA4y(z*wCIY*xyUShCQcCPatM1rOGB9MtdJjg+IdvMNGc(DE4xuW@h~Q
zL&x|e4VvbeuT`(G`-)tS@6z`AGg|F<fJ2zsC!<HLDLQ$}KP2zb7*27QW}d5C#>}{w
zu5KpTH?jEJd46)*XsGNZzu;z}2|*Z>p9i&lyJYP(dXr^|1L@?0Nh=IorXAU6U?lg@
z6im$|w4NAj%ieo#DfL(0Wyn8)bje4wTYP50&QL~uLYB<kJxr&*a>Lpg=G~Cy;3n6j
zIoGbxmHyIM%7$VWs~pHc5q@Jj3t#APsX5Nbw-7)^^Eld;ug}pd&C-%jwd<Gg#ebjF
z6E32p{b}IC$^sZ4u!q}nO%Ev~#OPQS-;O<}h`q<es;1}I<P@t=8L+|*I=S$07?wjy
zyr}Nj5l6=}0o*gsu0*5#M9su{8F&)ufMFia8Qr;uXo^X5`4?KWyD$1Y<<}?Q-ww;9
z@KS1WFo2HzoQ%Fmmi`z_?>Kmo>wxhBOlr@&KO|IOE}UKoWE|LK#YqDP$I*FN>$L2-
z>20=~>q*41Z4Sx~$na}vsi0jB7Fa2<E~Tl=!p>(>orfl#29xIGNa?>@#3sT)I#RKK
z-j^`mIFB#RvVOXR^LGzpC<))v4O*)-@X68RZ7CNHx)9)7qYY)8QGG?KE7|yz^Ldl>
zI(@K~cFJN3#P<ZOy`k(%yV`8Afv?(vktls?4&w&nQ#<oOLRc#|dWSaa+pqGt`@N$q
z-JNoC`kn>$)tv<}fNwm-wbNcPVJSTFDNO#&OxQ$=mV~qKGf2*x*k+|lO8}{KgC6*K
zpRf9pWlbiJ8}3nVrB8R=gT3QnEJ{4;jY+3FF#5gW<wEL;2=9sPX&0Od|D^_*yd}F&
z(?7>?-2isO@cW~crsF3D<$FBafL%R3mQZTtmrq`G!%#ljZg^6}FR9x{cv~nS$=>!T
z5aSyRlU8k{-^+$<x^{s!ZZlnXsbojGyp@>Z@%@dg9loL7uuJR;c9xBMYJW`8jZ-i^
zRQo>^V(T>;($yzkBPOt&?3<<D7Q1_*?oGCGk%@he*@OSgR+#g@dr+3svehSyq3;;F
zby?~%nr#m2-7?i@t9$t_htd?L-l4BZCh;RhVXHJk7Csi=QzZFnHpBy{dT>KwQ+g;i
zU9dT!u1FOI!gSJkk?0iTB-Iq@6KT)ickh&R(8UMX^DUJz%ZW%YKJza2^M)+2?31<n
zkwH&3BW&%1Z<rA@j2iqbT1poQW1Q6i<9#d~^+H`R#ZNZprNJ(rvn?=M;)M1T#x|6t
z$A)1EvqZVKZ?NI2$Fzoh?HIes#M!fa$i~E6299HbR|BIoN6DxL!qOKDV*)$hn&WOT
z5#9aSJ5J{@X{IfeY$nUL`yC7NQctd;DZBBBicmP499;P!Maj6H7l+u7X-dB8&azi)
zl%-5{{dTb|8c_d4k$U>10h6SNp-d&M_TgB}NJ&#Fgc&qfluyr&k^5%3d#`zt$_^nW
zEd^mdT0P)>vklwW@;j@%ur3YJ2AFsk;U)8_v^mP~vC~?j5+K+f$geBswS-F2;*wv%
z#3`1akrC4FAIXDmW|Kz8Fz6S>Io~iz#+p8Y&Z~qdQf`l{)MMv5=b3!s*npZ!*vsTz
z2&JwMmz8fS3A34<*L*0=KaT$ESa<QH9uu}KrS+P-qTGEDwI9*-mf?=UGAXb51?m%t
zb-q^#tBY7?9ig4vImDWnvAC(__`HE=|6N+y$V582yGf_tNt&=hudm83o%Z_#*X(4=
zTzJwGo-I8>)>B>{j)u{Uhu)E~l;@1vk=v12E4##gQTA+6R}qOCt#@2F2Ghx7SY*SB
z8v5)Tn~|mW+Nc9dK6*6UX|pz{hMn`lXj;m<`lNI_$7Qh#b(YDiIpmrJ%FkUgJ*{Oz
zo6+Hmwhy@&<XO($M4MkiEHQzX$scK#^U~=)Vt-}^I!@5}jOk98c0Iz3KUa40P%T$v
z#m$9qd^4yb_^l;NBvhJpaZ+o|3C6?VcZ23nc1(Z#bB44~<-HF^mx^PkeJ*uWw?_=$
zl)3v!Z1FdBQp;w8ElU9}xr+2#Fa1v{6S1L_DQP{1ru(u^Ufnw&Lm0kz=oIf1dvR=F
zwy<*%-^|%x6xjt(Q7g~9$7@1uQi+Rv-><x`TqB$G2l*%x|G-`5ZSVWPR1$h_vfXQz
z-bi(+SSEt$w7Khwx3Xc<cqdlxC^ky7XnMB~gNZ6QIw<r$8udzs^hufQU<a7vjTw)5
zgf+5;l4@CrBK9}Q$|L1}8jXz2y8f{;(AX+}I<4ukVoJB8{<SQ#Qn1B5mvIC3(pWUV
zq&h^FWHnt8h4vB;=A~q?+W1&Aa5_UdVVn{FeK$2j*)zARlRlhCSK(+bejkk4M=BjN
zSSXIfc#4XDdI$seq?v|}^P>UV>OQX|b&~h&Us7|yDxQUjDjBeLk;Yi%nIB)I&=jYn
z+nfA3Bkh_0qCj&XW+G6VtT2>{j=Ss=A1JAkIv<&QC+n%|vn(>>8%h$=)JLg~!b0wu
z428i689G|%o2e}KNngI%rA);e)i~)IOVX2}SdNQQbrLnD=_)Gj1IALZ{#X^D<w&yp
zj(DV=@q*VaRfVAs)tMut27(oq@2gl@i8;!OT25BZl^^Bj=j<fK3*^dKUbh!x5>yN$
zUB#(0DRJr%kJ~I8aXGOn8gn<(==R+hS^-o184VCA|GbmOsvncFI|?i11koAxlebj#
zi`=K&zad(^3g9P&;S(}=Mg3Ko_2!+-hpBUMg=C&#OrIc~e09WJATMGj**oz_wy(W)
z)=Iih+P$NTix|`=PN;SG_$Gu8cgVIrS+W{TdDiS4QGXem0c>GWTgslY{Uuix-DHy~
zrY8>C*M>!wy?nDGamAeUK!2g@0d=_mQ}#^tdPfz?-efM8E?u!yn7QW~b}EB~y63W=
zF<rTd8Fu}aF6B2;(eDRiR-Apd4A_Tq>xH}r=zWc`Fn&woA&q2*MRXCNm~vIJeBBxg
zBsUKhNfS+8QQA#lK1a4QJupc5&P7&Yldb%uTntkx7T?PV0l3U8GEfFS5E#<rOW|Lb
z_a&go>iq;#V3z~WFa3!gnKC0iJ}~CIWz#%`(R2$wpFEHr%P5!*sWhxRS!3ndDOHr5
zpmoOR`HP&xGVcyek;d~3*pVBaG7=P__*pTTWiTmI;p&*9jsHvgFzw>ib4rFCWH3m5
z%ny*=8?5q-@It9cJ!`92f^(>HEg*xhuui{V_WCUn>Bu3TucJ`LZLkn^m^~FnY~J~r
zA0fR4aZ)!%qTLCXeli_?NQRwx-6BqO6II8HoOoNI_kPe-BX_ALi+NOl9<0;#QsqR7
z=s~C97>u@F#I>!oFkZLJuzjY|SvHvj0S+Y6+S}-7BrqRzi8W(ENAf~36%iPWK2jI^
zlJ;j<KkZLSz(1&tevl<5wq}Mi+NaAPy&<^V5c1!zrlHy=%fIx7v;TgwtTmkb^TE=W
zA57oN)KzJr>6pI8i{DOog4D;L`I;s1Z(Z6i1)k6u!Ze-d^@i|rL&Se|UeI)2)EgrI
zqw~@qoj%m*08@4dk!}~?b+T51rTcD<vZDKJ9e%}=RL^g_QJTifdc&1+!_}N}!?ikt
HF7$r^flIw(

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/80_py2.pkl
deleted file mode 100644
index c1c95f79051aad386b46e362afdf03a8455ae130..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 6042
zcmYLNiCc}`*KQ(8gGNnANi-luGMq<+CZ$87K`Et#dQu|ON%NfMd7|MoC=G9ehU;4Q
zANGCs^ZI?ikL%*z&slr#wbyj7y$xo$Z^~bnd<f1hF3k&mUH+=9@L6f;vkz8TrFpr<
zZ{C)bmgkmPsV0A=tlm8T`>SdiZ&It8<!0p;KhMj3YnAs#16Hc}V^b3olN{CJ@j?Fo
z>RDOgn~Um!I`yDlwX9bU>D9yj@uvK1UZ-09XUXSfA4)jW`mq^j<cF(A;!WaB>s1@Q
zYU>~WcfISXU1DNl-2d>;zyHLWl&SVPjRoe9eK{vkHf0G*73LV$p$(lyUu5~RZ0gW|
zQ<~4qL?w(m^=JXLaL}ucWvWw7fl0x?`a2g;DHpx!swrizDRr#CR$KNZmzB}Tho@g*
z1K)IRFreQdI6@|M=cP@WWXI|qzRA9Hp7V;6E*tde95IMpd<%Bu#_<J~m{p_~O|t(;
zx#EUS4^+!Os$}V5xJ<~FOv}D);mgC)Bfl`AQ{6P>-1VwQnd<pp<-9cIj_cJE|5fhf
zU*(FZT%{OZNGqnpWl<-)Vw6Ey#Vj^tOed`;PPwArl**?1N>eFTF3XTEh1xZmNv%}N
zyiOMH2Pa`0YqGBU>X|7!sJ<vh9a^OhJDHcBKhR+=<Kj{IaZ2WNs<)=yDZT1bruyd8
ztEcIe_}>3Bp`WJZ8NKTNSIgcy_3U3Q4NfK|R%kva>!`(677Q=tyX<00z87TEoU<}6
z?f4;`F3(_yMMJ8q%wd2S)pukMjrdcZ1H)%4J(f6VCAODs{3^vBb~z~E=iU=(x?C7M
zrS9Stf0-hb&7>FEvSI!lhO4(vxnshUNS5{JHrHc<0_Z#=?b1RYe$&L;upqP2A&qyn
z-^Qh`B+o;y!IeV0pnK9xn3$a?lVzyV#HX{dNTu7*!%>OUq)1&bI*1*@5@ab6t(wsC
zeGJRMSBz?dEm@b=6sf@$M_7`9Lzy~FJAPS{Eeuv*MiU#kf!RQ`i;)9=w23i84Ee-|
za<GPV*^ky{*B4cUqw$$0|68W8E88DrpCd~fZ)r7+;;NYUL7!BgMLh?B*`e{*L-Qy-
zf!)g#Z;%F#(0455G)>)193=IRu!?T%Xg};<i?oKpE+2++er%^<8nLDQFf0qN&}AWe
z9E3i7Av5?^Oz15AvU~}vKEwgig_gtoWr{n;$VQ<oM9T_ssqC7ihl+tPOhzy3Zp0GF
zOtL?Qq`{sTo74ycvW#(_#o+;}u_xWA(N5fp(Xt}hfI&O+ycIL2m6~Y=mw9K|O~f$f
zI53NKceK~6iViV^`oid-;rjR2?7Tg+-P*2VhE8iZ$w5X=5TpF}X+k$_L<GyOH{s=b
z<*2`@Em4e?TlTK9sqL|leEm>S1kpR&$fr-FpJ9j9!a#S~#4-bw;k+)LpTwA_4SprO
zL?>#kiM^!TU%E;#$~Z6kB>nVBzwB#4Z}jy0VlFj=W@c!9>t(5Z+|#ytgs~4#3Hrm(
zkJY!O8N^-Km$B#v$!Qeo2%|^7vtCp^h9Te7RK~w%ro!MSeLT*jY&&uwLpNSNl~w5>
zrg=xuLkq4+2WN;8lL@A>{pkq|*DlKF7k5+>qWKG&823}y)`Z40)6jU(L7QI1>M+57
zf9fCdhk?8Hs7}0*%pdyVUZTNDmJ?w3gns!4Ei&mFLz!80pi_*&6}M?xI%*$%X4s8}
z!NURzSx02z4wK0>#!6acCzdg$Fb77l>vazXi!T+q7THNK97O~7ZLobuEFG>TmoVbP
z-#M5+E-l!0=R~Yy$%`>EFN<6p8yQ&oiZR_S7=nxPKc9Q6qjdXQFO168WrYsFcd5qK
zWyeUm*%k8`PmANcW2tG6A7YETTFM$jZ0-dNR8Z=3=!=KwAdX<Q>X9*7l%E`x+k6cY
zgNzaSyrRvk+|Wpzgt=#BGKfCne@n@1xka&t=TY@4PWlSCgU=HW>CVMU6xCgRA#+dN
z-%%NhAg0S7bUvZx(s?agrgZp5g|A4HON4Y+5}pdWS8&FT;Jgcq#k6!e-Q*IrUn?`E
zJ}ZKUkYVfue+dZV3dT@3>9ElS6V}P#16wyNc@xiN+xy9`^I^jE(=wVP<1Xe(HtSLc
z>Upx&6AHR-(~E-<G8Lc{-LCM$RA8ur{sL5$Mxr4?Ii;L^L)w^96m-0op(Hceh*Bzj
zot(+KEq9a;f$x}hM!#4EBi=At%7hxPymOHDAhZSCQ)C6>_(^Nb<0nVE%!OZM1cNsR
zJF<xHs0xwpI0a*uy_w&Wad(tx>6NMnvMoE@(kRo`Wlou4ihGtB{>XPs_$ba5CNd%m
zG5Ddwx+&MJb<J>PkvRA<_~yq`Fr33sDSke%_J!dUtNr8)YwXh&r{XX#KV(VvJZZYC
z@&jG;Z>tNFzVHm%?p$z&;VuU6%5Tg_jR!4VkI{k))N1rJ%~vBM$KGQ0(nbF8k1WVY
zCHZ#dDn|`GpfN=sOQ*^jYG}=GRH}|?<Rsq{Q=Darsb+nfdk;`{MN91=>C_(%kdbVv
z9dw_`IxAZ_nvZflFCNy4lXJPRPR5=PrIOtLq@Z6bMCu`hD|DTG2E#48Ol~tHTbVGJ
zW5@MufSXn)W^NZ!`^`u#o!9XrL`Knff`d|-Px7?JmiB$o=6({kY$z^0{+OwdD$8^j
zY`pO^^f|T&G5N-|*wcY4GvYTN*~KSmrD=ijTJ;8dZ^-4CleTwX(Q3^D9L4kj8TB|w
zXzwNeki17@DB>c`JXg1jnQ$^q-AuA?V)D21{N%LJVA)T8#m#~f0x%}Oj%fRK$;LbM
zCd&#3(#a2#78tljJ+je2Pwt~BkSmkWdSb96`yaWb)L*a6kbnGXlFw*2`^to!CPsZi
zmdxHeMx(xQ#>P3u-H>MBCMS=ao7ZVdKj|!GL$QZ-4rHJRzcH1CZ#1~noS^4hSU^Ve
zINFi#FVQQ_(vr_*H?QKG-vOy-xrmVVXa0{W3SfAMA^dSl4=QAd(J?K49DhlO{U@wd
zO)s%c5sP3MuwV<;$%)58uo_h2$>ol_+1Z^t!#(q?OElU~)KsjOfv4fN80O&|(4Bjf
zsu(qsf1^fw2O=*KzdQBuPDm!fYpKb>06Gp)7=7VP{V^CTIe3+8i}3<XYTvs*C6r?}
zlveSlAJ}EZNdpHb(0NVkv~0NP9rm)DNvvZ#9K1a&!|$Y}oO;=sVXfp~DOF_@c0QNt
zJT&n%m^39vO8>ngHW9Yck%~q1zJ?*{623Xe=GhX;?;XQX5`LtsTB|hh#m@CZDJKp(
zvcR`Ss&6S#eO;<6*!Yz3dz19qe>y1b#A5WvTmm-U6FX6_HY;r5yEb7YLZ6yLzrpa#
z+SH#Vtd$#8qV3?FH+kIsz6Voxx6G8bXM%lqVFC=`2TyVHtY=I}3XgmmlP?$v+i1~(
zaOPtM$$1+)%v5PPLn_^(1%5r?PyNZVA(JQ6`^2rZ>7GlVS3C?wi5^~<w7(03?|V}k
zpstAJJ&`@_qJ81N)F6|$W$#(~*Er4_z+MP`f40!p_=Q24i)S0Kr>DgdN-ccz$*ay7
z%12w2J0X^(?f}c%VgX6^j%$G!-eZ`wYAOAmmSoe7%hYj);kw5~cBRWp5o0{&-pj_Z
zDCIr7#I8UG*}AVBiYbaZ4dWx_(5FKDd54B{<%=|t%6F1|vXl?SF7Bv%pRG8`#DOR5
z!GC2dnDx7NL{`(Xl`j}W-*I&7vXpCRwmhbH&Q!iy+|PGBnx^1qCHjhF67wkv{*;Ev
z;^*S~3Q4}24e<b~9!4qHmL9^U%a$jV>r#b*5S?^hVReeJmuf=VMB4KY+&?WHH1T2f
zd@FAm<*Z1rzG6atMad%5K3U5j8FXhe!qz@8%7jJ3pp~B`bLk>sj59l6cz{I@Pt*kx
zezCkH4c7UTHp6I%J=#;~+hC?18-^9ku*!XSk1Z!XrnK7Ej<IM4&c690HYTPrZ~_y&
z8W^NGLPoVBEPb&sB(U?XIpGW=tGge2$EiF9&6K$!+sU%ya@UN!)RU`FmA&{xg{5#N
zIk4hWiX!8BUL0aSrz!GXcY(cHqpV~qx9$|nl8XAL3aO`08Zb$U7|K+n)g}~687XOs
z62u6aEy|~5$H;v%+`ZQ{Ny!dkO<D=SLZtFgmU&m%!5`lXiYGRuAyS2rcaeSkc_wXk
zGJO237N{%`Y!77l`Xw!(k~F{MmospR<yUx^wEKngpxfD`(J>7AMo{JlCdpV+yrAA-
zi6Z6pI7vNrZ&A+Z&5JFJCG2H#FY~fE@adZ3lS;yDX6LmKO!ZHo{}whK-MNktf6S%z
zri(({eFU|i(e;7uj=?G^ulW_~6IttgZm_H_VUu};c5>$sb7I;oT8r@omDPSuTG_~i
z+qpPPr|&7Mutlq{%N~vP`y}V=X3K17(o>!-Jxn%JULT8u!Gwq2m9dnU^qSk9aBRH2
z%6?Jytx#7UjvB3ZTs#irspD8;!-^W(><62Xm5<t}11sKoG+S#@TU>^n^N~nuD$_Je
zw?kYOyHE$2ypcn$StNexnCX5{CNzl-AGCeS#URgeF&b^Y39+mRyiCrgUCT?S`B?ii
zGthC8#-~qrL$vD=ru{gxlZR@#E^E$?EXUCS<$)i}nIgf`tc#P{gA_0vLwA5Qf3arx
z<DYY+g(|NDFgTVUN9{}A&&TNXIp*5zgCw^2+d8Rbv%!|7fR|i_^jk0ePb(6!rIVl1
zdK^s;WRtwQe^`bveC6nA-YNFu*ueb3?iKtnWq(m*eTECQ^340ZCe$XCILh21#q;`2
zvPpk{w?g6{xW~Bd{ditULQgc?y=LhRSFRSzL?DeedrRT1Y?w6OiS@e*8>JaERUW`#
zq7oPx5c~j*dPRoxNtx_m2N>gx8BchG4YG#9$B4BE*8V10d#w1KMI(K)sehuVRJO{G
zMr(SaFr?d2|4vq!Dfq)UmvI&QX-t}BsSc7ASx;A3h4vGV<fUXV+xS^ia4JKw$2dLy
z`(A2>V#C<3PWp5*T?s{V@y9^SJXX>%jm6?{jHf8^&yHf?zBE(OaXuQbt?u(oQcm%n
zy(~4CE#jG&sFHz$j?x&bxaY?gDQJpQq}z-9IW6s(|5kx!e@us=Hd(<?Dmw15Pkg9I
zmDG95WTkATDqs0XH7E@w32Dk_sgA&6?zs#FgJCk{QRtJYm>-b7e3Pr03U5^7q-(55
zPlm#DT#~9&s3}cXBH}(_EEStilmfK4CCl%y$I5eF@Vcd{F!+&j&W+T-VujTQO02BK
zcrc?@lNE=G&+_YQc9Oyi<k|&Zx0hlPlo)!tilWmgamr&Jw^_F0a$=Q8%toit?0YfP
z0>=1rT0tcKbvKV$KPF>$6|9x9h)%PgysbpO%6-QD8zPlAXZWNrd{PFlE9Wb+-dD;(
zh;lKmkjyiTsgtCW?{=8==S9pY`zIgE&dm=FT1xjxyH{j!5uN(f9<{cg-v{yIF4@*Q
zOV$I4XG|`#>aSw^3|m;#ma?a8f6bXi(QGos_|#T=ZCGO3%MS|@SB&=XRp@kBxmJLm
zHVpN8J0+OC$!si5x@N9m`o2@h=?o_7zT+YKbS)aw?E1|e%c4@z?+Zg#oK3b2*aUOy
z#k_~;eTT6Sz9sRH2C~Bvx>%tYa#gZ=>mU|MZmvv{CaS!qXg7g{9NEcq#USzBE6l_u
zEBQrS3}Y&mKFSCS@LQ8`e;N41!jLB43eRJ~hXqa6A0&_hyKH%W=}&ac<O5-kHw-x+
z*fjsdXu27{PaaN>r5B7xl{9SHAH>@8(@Iftg4P+M=R7%wY2F>2B8`{mFt;do83_ne
zT+o=zG?<j1p~`VPOTX7PA=<^O=d=vl%3y%<Isc68MKQ}WLJOrP^@5ec6r4qsQvn%#
zjd}VFGq-M&NJozHd>w@{Zi&T+W9+FgVp-{9>PC7EpinnPB3)Q6ePt^2s0=&sy2U!t
z&8j+HWY60Qtv63ojojlhS&X9s^k9>wmnwTwL=QUE<1koxvabC}3*mLk1Uu&{9Auk8
zaK@HIT6+im^aK_Ht}<uLXh>cteunu6qL0+YzNGy*HqZKz67UbIBOhjok*%3}PJ6lx
z(5r!EYS4clR)e)C%kz5m!oN?Jwd%#c50*ZBR{bbHZ%7MO$J9+;{B}bVq&^1CH_ci9
zHl_Wl|4E%1qOEyJuZEVXVgFt8vbN?Gy&C?XHLw2vnjY^}*>Jyzex0ogS$yCuEwQRA
h=Ci|$3~0|H%Q`heTl1P;y<Vo?$SG5A)~UMS{{@a$>N@}c

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
deleted file mode 100644
index a6955f53292b26d734bcf8789324d89cbf5d5207..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 4176
zcmX|^c{J5q8^>`($dGxS=iyXBhU#qd>~JLG;W+1zsrYe>8Q+Q|5h^Mfnh4D@+&GU)
zMC67{U8IaRY0%9OUfuh?cki|KAA3LF^;yqe-{)EDNyX8QL?4cb$J4?i1Mr8V!x+K-
zk&*uK+<uV(wD2QQjL2vjgBxS{CyM(B{hwEi)t==9#%9h+#c&xII|<vOZqK^8ap@Sx
z|3c}E_=o^*jMIaSg@q;15Zh|cV$Yg^agi`?b$gr#>*noVjK|5z$!=qF{e9T8FtBaZ
zOmlI&_e`Cw3U3KG<DtQW$#)6#+uYLX_&EZ`%fR@kK`cRk<N1R&*#abN`!6=`7h5oh
zJJZ~zsAq!x)A(<MH%s=iG0$@d&Xb=rzZ`60hA=~}jkF9BShGu>W<D<_gm7J*uXb){
zzH0Y7Z14q`)04TJMz=$lgPNZ#b>EE>Fd+se{4Y)97fqCeiTz6x|C`oz!br2mKa9|G
zKl$^j+!ca3gxpwH$|pFJ0(ZMVuOkR|n2@fu*AdQ3-j7qbSw|R@=}yvVt|KJjoTfI2
zC4`!+5e}<@A_6ACz$B>|nAGO)*q{G@j?%xVG9*m)AL{vZOm35ECcwwS!riG^+q&!r
zA!nN=&jwLorZne$o~S?Aw!4en8uEvyR?RO~qp6U@O<H)-MFoMzPm$|uR0vvlBj-h+
zfpq$=J?htKP)|HWsl(C1$Wl@AoirWplKOC@cQkmdmUH^de`s*#V?}PJH4VnoBLbvI
zH250Kuj80S11E}8IirOJHz@Nt-TgF(IPR8?@1cR9zQY}-E*ji*ep{3>N`r>lY`5df
zG*~FwN+xV_jxRP0$4=9rc;Afnt4bQEcUStP2GU?Fb@4mXf(C7&v4$`FX>jlQT4Owu
z2AsSK*{tSt_)}@%m_{2Nv?+S_d-VdKY|UWp<jDXS5O-6#)foUf83(`h{S1H&6Ehy*
z4}|D%uPQQ^1K@3SSTB2X0O<K1y7?qL07MG!?*66}09yGkxo%F<!KN;_+TuPPBAeK}
z^-}4ef5m{X&z251UDQWT3eaI`Kz!)#_-0<NPuB~c(csM}BeV`^P{m!A{cSVfDv>6q
zw{tXLvy~JQ*vzjdRZn?VkPdBeCj1IgbU0%)-Q6xv2Op|V*F6<FNc=FyH!0A;z%jaQ
z|7LvnOH%={dKy%_?A;n_OoJFcGhfd_DtrspmaPBm4`*xh5-S7z;kHA_>sB6rkSS2l
zj~Jx@Be8IxkV%1AY1L%haSE`pbw<f>QeYjb501tALZSZPx2Q-T$loZjaCPy59{pEm
z&!u@lAS-KW3fUc0FLaeR3c16jeIdRN&bh-#asQjeTo0(6aA0jr^aPV#SCS>4dO~M%
z$N2RX4=5#jz2FdZhnAIhEgwh+pvYR_snb3e7_bXz=f3LziGz<`mMhypMbGO>e5@r%
zWN!?xtyzE+`$xZRWme!MX2YHR*cPJE@zFIIM_^BmrLj|pu)TeyoiBz6xndE!|4?!S
zsFFRsB54cX7i89}YAoP~&qB%=!4&YJfk~QC1jvb!8Er5?!1(>xvVo8p7>NBj?qz2I
z;_hA^2bL{?^I_fGA+UlBYW2YbJyu}c)L&9_%?hG<Q*=^GtUxKOD^22s6%gnA85akv
zpdz;-nmybKs#?PAE(BSElBZMw?W;L_K6J}hGR6$pb#AV{_B4e&6UueM69jusi5eFq
zAW+nC3{vAqutT(ADqna%w5BAGGyIG}{PItHvX(wn>Ph7C67Z0I(UnhPK>==21XP<u
zxS&?$3bj-ehbRrXZ8Z<%P@m<aVdLvsDAY1mW>Q-Z4F<JDnzb7vMWk`$#C3#v;w}-{
z>daAsvam?~X)6@39n({(YK#6*nB^?P>`{zhj!w6hBf3BSq3X{Mj;Lwib}cA4qQ^DA
zx9Aqwp)mu6vT{2sWN|*twec}Rvp0Ac!eo6U>BqKu_MH+EiDfCSws}wZ&f3%}<|76)
zj-&YfM-@SHB=UnRk1~Yilpo^RPyj`{Kd;_tmIiU7tG)f%q7WXe)@`*@1gwXx?@b?<
zg0|71on=>4;CQik)XeT(@K<$l!pyW0w122{BiWk4!)K44G(C3*;j>O|BG)~E$MR}X
z`&S>hlt<#yDy9HU$9bfpfeN)W>0Ml#bs`e2OepcBgL~y~ZiAb3fo(<Yy&C>t%1?fB
zvw;k_pJEzX5?(NE89ZHY<^i?ub*~J_c|c?pQJQDj18R7kiOalRa87+XHjUF8cojbA
z(U-kp;)qzdv>+K$>ksnV6MdojX3LTFBYrS<CMKnehXUKDwp@|opn!nPqvlZ?KM>6t
zDJWs_h2GBhr?PW=;QJLk(<RIs@D5sIx3_siZ(YnAm;2t32NUc=CS;(r=P*08{h+Mh
z<|p$g1v2&~_&ieghoo0*DJ3`jq0Pas!cm+GM?`*T4qT$bDPbw<{=GC%&5LYY|4xIV
z&@YpIPd59cT-(dhD*^EMY{;9e(}A#L^h9+_#vw4y@mD_>8w3YNR)xKFg28}Le^oXl
z7&bh=cZ9420Xx2keeLxj5ZC>t6CoW4vJ(9Knrd_i;ECw2bfQ8=k*^`&TM7vF(YC*w
z^@As)&0EZN`$3H2Q7c`t9~?eT9qQRifdRwxlGy|bgwgoMgC;1@m*@~ZqvsFmy>ml$
zC;dV0wMp{}UMg5I>xmbCQXz`#a?I&yI`FYAMsP$1LS-KPSU^V*Ot6a87dnQ54qvYO
z{F}pIzWa$_y+#Byi;ocBdt(q$l*}&{9|^-kD_v$-6dc=oEOthl0UhTB5;<}hP^muW
z@FR`^!pt+=%fClKu`$V=nimQFJgn-uT@jGb&0%#U;4nB#-CJX@2E(PZqP0Ws>9Aiz
z+D6ySA5<m|4K%*=f$5_@g)2YZ;k4R?<%^$OVbG<nTn=-Fu99_8Z5ewAb&`^q5U>X2
zIM({~P)l%{tPu#_>=)&h&Hmmgb}+QA`z49p89oi)vM+2VfsR8YyK>+G5E%*VHkUjA
zpVPOToey#WE3u%|xVJXISt!k!{lfqrudf$2<;$Q-`?<I}adQ;6*sCqr;EcA~gqo|&
z9z<IcgI8nPy;1V1cRyW}{7`(jsO0lF3KA;Y;yH7hf@V}5w>|KppyaUu**=0FGU?<L
zCy9_z3cGWl&~`8MAz>i2{7*Nu8mV8C6z+^Rp5_d-956>K=Y~2AIOWmav>{zW-X6f&
z2pl8i*+EX=7H5@QSKu1DAN7{v0bwccZkyM5gG>F@i;|^ekk&S5IhE=QcX0VN<3+yU
zIxi*tT-z6tId`Ar2_ysCXoSEkHy^-zen0(n+6&_2mW4@^9*{F;k-LFA2vV#?a_?{?
z$TGfC(Qv^Q95SEDiOd>->hxs5z;zsIba^al&4y4v4UIS3JE6IoF50n)Zm5*>HP|-E
z3!NqGJePT&j7+V<?_h1d=+1?c6VEP@Q8usMf{&XQLOPQbp@D8_?EKBlH>^ZN(fRUK
z>!TSuaQDp5Rpp&Xdg-JYHapK;^*_UZSyltOMpIkw?%fTai9;=c(FPEH+fYA8)essf
zt0lwA#_)dP;iSateZby$PB>%D6e<<$@@FHgfM$Rv4kel(CHMZUs%kvSmK!u(5|V`3
zRb2LdQ3QS4T9bO??ZD<+QD<126UZv<Yg<^dhgUNr!#CJ0fU_FQo6FV*Lw-?_JY_{_
z7^rQ&a8DlAC>44{-`(J?U+;EU+zeQ{)=Jz$Y(c`x@JE!9BQUw2M;ktKgn8LnMcD-h
zcs75v>g!Pl==rKZ>f~{R<+TUrwwXD>jfnz5J3k@_mhYqftRX^(5LuF=%n5Rpx)W)`
z_OQ#MTiL<Z8a`HenFM&7ZuSSVPmjC-j5X=zznoHqyn)kuABqTpx`kG>c03Mn_$-3m
z2@XgV@6*ak<^k!yG`rJewt!HyY->pR7;|o=!j)LOLO9X%Wjd%2haP1V`o=mf6Ru^;
zcDUB}GPy3j-LqX~gK78IZ)<zrvx0jlk6Llp1hcZ07$J0PE0W&3KfO>(0aX_{o_uq3
z2O{=c^kkhkL`LCd+7BvB(YLn_TCcxbqvJO_61EIGA#F1)yz2`Rx<n$9r;6Or3m;GB
zk6d?j?4fbTsfQj&RpUj~63G)`ms(WwJ3LY9tG1X2oBmkUfKqJ33+a45rgOj68$D$@
zZ3}So+4R%r+^p+9NMQWg7e{v=#M^j`m?-a!^g6?jYEOG0@2UB4!*Mrc@4tP>Ig5l!
z-$`E{kR~F<dv}i;u-l`tmLd+JOj{%wXdj9?>`;MB^HdIb)4y5jQ~IqgXvDq8aTgpw
zY3T~(3H9zM<<9WQ2nR2ebj*03rs;!*^)E>UE0Gay%Y{G|BQknfI-~dXoDa%aNK<?k
z>5Yu-635(^JW<d~3732N9*DkJkQR(Th%_Y%Z%%%5MYa!Zq~;#DAi>zA+bI+xdi8MU
z(t?cxvZ-vDoN}{69+x{C?5MWLY{tSXf87SXoVC|%xL|`mSqD^=lvtxhM$S}Zl_fF@
zqxmnMG($(!ZoQSixEJk+^@~fW)<wJQf=|7jlt(7N^UtUsn`O#-Jl-EKC=UZUGG^hS
z8sIC?%9j+U18eJ|3VyZPFwL?n|G|nHM7Bm|y*expp%$YymA-snbLU6YRrWrDjiT6K
zurL=g3Hrdhk-?Af^E0-ki#RmpEUc^I^N}gEb!^dMN(69140HEEW$?4(r4;evVLsr%
zo0u_OFj`ElxYMf-_tPGja?I}mvGh}Gi^vE*QXiAWhK;~_PsS<k+eQ#R>%MTb*bt&v
z4AqMg4Z!O^!8<-G?S@)chj;mWI#B2&M!s=Q0jTlsuZ?=MK{=PVZB^xV<h3Ngu}D`z
zp)L;XIsH4(+1>)YTjp-`Ny1#RSW6#?zHk|zxTc3>`8ib&^Xj6<=Tt89UDQHJfojd>
zn?Cl@3qJ0?lN(jdv<L^KEi#`>n_7PTV>|Q&mvYV<i^KjuI1`juB_LkwN#7r4q9CYz
zU2pqkK~U@|SSt+`0Mm-9^kT>D;8$WD)W0MGX#z$4uQ6HRmR|YIOIHP0erm=FHfn<!
zU*-v`L0$N$WrT*=459ObzPk3nJ}7@hU(!%DgTJ1d9q_BP1WcZRDf~OGD*hTAl}MQK
z--F``Oy$?GDt`3ZzSD|5ll#k>^c*s5I``vFF69`h%M&nF2B!9}R{fV&gM?}RPpkE}
pR{po@59gZ(38#v^6#r7cOCT@n5nW<G6EJNCrbA_5_;hRs{=WsRq1yle

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_800iters-Sigma_restarts_at_300s, eps:1, N:80, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
deleted file mode 100644
index 3a77140ab1594395319a20cd99c2ef735c6a0612..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 4176
zcmb`K`8U<u|HpAek$E07&rT&|s<R#QcsR&-IL<jlD&8DpgpVRq#)?XYCZaio8|S6r
zA{lN-)J4j8b9JSgA$-*ReD7cI-FvP5!``pgdaUPOuh(m>X9|vfH2O$HJf0pN8H7I)
zeV7><7#SHD&l3<CL=Qh2#f*%mGkGw!f1-GfGX8nR*d5qTVjLFiGz^!4agwnu8V>B6
z8<&o8{V$ZkjE@N7!MMFR*x1;Djj*i_Y!2)h*fui8qv3$_V&A+q!+4#Yo$WU^*U!U&
zjfriiWm-sPvNv>BXN?d@74G{k=F|`-cBwbLPn#lOd`yg=7Qz<t6E6_5$rdDILO<BJ
zA8g?eo=gkdoI4f26sWWiw8W?NSRUnsg!yQ_=-2J6)ne_N%k(jV?zM$2pL?zlY!+-6
zqnIyQ58rkhrHFU1I_iT(S3@td%Ce^0Y$C=9m<SUS{g)>8gC<VKB>tsI{-kZ{_<vcL
z6cdxCWneO!zvFQB|1`?}pvsXk`G2Tq)3F_!RH7h18yiojR{itk060|8JW&urh3SgC
z4~61^VAt*`abqYDo?5qbtVYuyiHAJj(M1EnravRs*Juzj|9XcHfey0iyZ35br9%Vh
zAoUTB0mfEJ((h#%aEIKFBfqD^EA_mb)BmBv=}*=9nKpD7OOFVWA=6<YR6y4$n-0!Y
z=PG6^9j;O5^127;5OKmI9p6g_VFSn8&Rul4<MOUFd4vv)^|>A=mgz8Gx|KrM<eXS+
z9FCo$L)pG*ot_#xXmr>3r3BMqD{b*B%aRUlVX;OXfpoZgb*(9$MF(y^#awm^2K=Es
zn5Nmr03E8ngPDF1RIVAWojMf+gOVPqH#&nrH{-yU{_jDsVM^o$fnbRK(o>ze90c#`
z4)<}k1cAQ)!Rt@LgFvk0&YmyIL7-jqa@+L@2G~9dt+TwxfXHSJU;PvY7+f+W?6YIQ
zbytnIrvw?WG$=WAXKYiK+tc;pXLNWy!VG%^bg1R2%>A;dw^pp#`Q0oXIP9cF1UL2d
zrs%882s5B9&Qw5Ah5@IIr@Gq}7~n_K?YgVV0I6>#_+~{07&=9_?ca<Ke`zi#(Ljef
zSF^2QCUl75C;EGr(BMm`j&#H4Kq#m$OsokCgj<e>UOndx1i503qKFYHFcV7#OITEx
zkySg6J3$2wj?O4KZYr!p-GR|~e<(3{^Cc?M4~jO*E!|vwpx2<M;7qC)1hcbOBvU*=
z?Oaz?lZYo=*mubP{uxhrTQ=}|G2aVn#vR$461~B6_od^~Prae@_=~ZtD_&4R@#)|a
z_Jr1z_pKkv?oetY_|$oyD-7BnYUjD*2#IeVzN}KQh3ei{HTYO7kjmW{<XE!=8O~1u
z+bgZXS;Cel_pu#BqZ1=*a!$Z`JeJN$C4o@;N;`iH3GyW(_WZ8w1W+rVvm$K=U+3l4
zYj0Y@H^2GhQGz+(!-A8vq6m-|B{$M&h=BQ9+OnYt5ey~%81u2W1W8XHFZX3D;C}FE
z_8?e82CeRZd#^Q^G!K;DykZT}e95{g<<_8lxGPnv!x~7lfz0!R)=-^a9nBeT4YjR@
z?azf+fwH$uF@3=TJ|Dc{FC9Y!PTlLPue{Bn(3E<W@C3o$EOC?K1O!UDP9f?72zH7$
zP8Nyohv&%&l#Boqki7UEe_Y!DYV@V@`3QJOKkvpbHLnOasDf(EV%wlz^%AW@9EYfl
z`E57v??C-l4~I>zYNIf#Sh)!uee@=zHImqFf|QWv(UVsZ>W#ZV;&^0%5>!OR8gi^r
zyiQDSjhY?$U2%rH5_3Q?!g;#g+D_=+*vHyGK02Z1!CUp9=!72M{H;~5*dC1<Dppq6
zTO-S}scub=5t_Ni%M_&;An5>()q?lRNGz7Etj_iW;VXOda|u5QpmQC=??0vlT5lsi
zy78*O;k>GYyc>$3WdFzI+byynX?(eFAXgm1L)E*jcZq?`u+8166Ee^?60)oEk}8}i
zlZ=|)vm5@dD@&N3GKTh#H6CO;B0PBZ@JaKpo*-J_>>+m58+ffQm$onX!G%KdHtjMh
z&~;tjRyWe1o-Vt4+h(7LMXM0Xy&2$H^NYuu&A!04qV_(`Krk1eJh|RT0o->9O>HS3
zn6e6;Y9M++{Rh2EgFCz+vX&&vyX*xw`CLfLd_HhSV=6Y4+ZXs0Kk74<ePR5lM7XRl
z1yULg2sn`Zq3(L?(e<MNFnc;Cxr>(yLX%rA$#79YQ0`&Nh;0CfXTL2jXY+@?&JS6+
zd4BNq5}xIH*cb4Q+M~C&`$FHNnAfiNe4!A=IfqOsKyA-sz0e7O$^qN&tYcKjFiY@z
zs1XQBJsip9*8-u<F`(K>k_Jb`zG)3!ph1?X3~j#|9n=aVo7TV5p)~BTiGU}Yb8<)9
z%aKb#@VMa6>+GChSTcU1wk6{rnB)a&9Ec48_qVH}zPh1cNNBh$e<&0-yuZFUv=RcG
z_)^ZbR|i2-?~87PY%s`62?%JZGa!gJqPxbK1{tOPM*Q!nAly$EdN~sSPexj{5cdQ?
zjM6b{JxTx^IYArh-AaW)qxABb1S%Y+^N)p$Q=vc6F?w1*5H$K`hwM)U!j4y_EggI`
zux2%o&VQ#t)V7N;InfN@=U9y3iVTLDLPlE9ix3!RmuM()3IkpKe9yVpN5Epw6X6EU
z2xyUfOZwo8K}6|s0g3oX7#3OSB4SaHW|kH^t;2*DX9W|v@|aMgG3)p(jtQcy(>%++
zMM0Sf*^^cn34y%q8u?ujkkHL#eKhC@xX9dHW3q?Bg#z*Vp$`n$uPJM*=Mf01;|B+u
zUi!h*F~5?P@1BsOes1~vpKkER^-<Lh%mup2*Tr?@93aeDMs8ft22|qM8`8t9z<J`P
zVCd$&sIqDa^i8&hp>@5#k~m%9&*2*mB`svob&TXx33dmux53>O((dp%eM`Yyh$~o2
zgrvm1vjy%FS?=6#hVXcOy`;HF4%Il!#yyg>KyizGI>L=EXsd0Qh3d=!v^6nwHKyGc
z9nX6I-BmdN#fOVa{~AX{B9&Xbr*Bcww3^fQ`#w~3d~{I0pAdjdJGmvvVic6j=@Kj=
z<bysY42D(x;el2o4Q?idyP%Dyc|)!47HH+n&<jIu1!R^wq(>;+3piWBG(w?0<OOeW
zQO$RQZA15>-ch~aaPs?G7B_vtwc+x4=?V(S>R7O4rTD{bT+z+3Qh#uplac*Z#~+S!
z?>WUAOaYFO2*Dl?Kfrr`%~_c8f%v#(QSyWr<c(V9Z{QAq414L0_c$_Sn_Q}HJZA@v
zna_5J%@~8))I`wWRUB$^eJpOnfzSXQjkP#9quJ}OI<bi!sDiu@YM11L3JAN-WZt77
zbL;TiSerk(eeTrwvkMfI%cno@=i!5p?nHH1um>7Fdp+|tI|)&B|60)gL`3d)PJdrj
z*@a}6P7$%0Io4|6X@QIKn$R_p^8Ak39{5ZeY7LGygz#HN26<{m&_rD=A67Ae591Fe
zq+aa<&ZaY>8EfWHqiA0=6JZT>Lp*6H(G)3r4rJHX;Zg36H|9$s(lE1%%iS-Ipnv=G
zq`r82u>DfndAQ9P<dyfe%`Z7X&-B~jYaEuqU56FU<{E&JfVfzpiV`#q*0-Fys{m`%
zYJHOb9`H42@Hiq#1h%fVa*soHAZ2azEy~ylSUkT*8$EM^Ir$kS`FTfpHg~yp;g}=z
zE+~>ad7WT+?f#kVL}$1*UMy@MKmy^aeYEd4NpMJnBF$Cl4Ef64iS%Iy*lpRZ;%H|B
zpK5(fgM7_5=L5yBSHTcQoArubPO3rSV2;@XF%i(P)Q;AP#{mwXO|U=71u2sK+S$i>
zLH2L0?o_!gAQCPA{80KRYj&mDja0TmINAKyR7gJ#J<KTak9A%qT*;Mx;nvW{+IHdH
zULn;Dmi^zqtnK~44xVAW>SbNyteWSf2$36Gk*wMN^b&1FR9EVB>h-amh%{i?n|;;@
z8HZQu+^;rAU*0)tzxrx}PF#PHuw~d8=@7N?ZXINFflQ)ImU^HLKX2Bzd{312z~n{N
z123ee*-^Vh_D0x+R<)uR-YBJ~E$04aeXMFi1-9XXbU&x*-mCXTPg%~}gFO5;>*;fT
z_EkS5IQHx>Cr>}b*OW#|RPaUmo#DrHro52v<XpJXm<MtQ6dH2LCZme?vKI$sNl5AL
zofC$f4rsKsluIPj4oL?)grOJqs93ILGLN!Zzu6j-2G3p5ThCsn-QbQ=(-o@{8az?*
z?cq}qjy@<U&18<Q<%fn1F35x`QxI;;xnMS93VK;Ft-o-_4`s}!Dm{zzMJD!%qn=CN
zDCDJ->s<pc#8@m&4aFZoT2dv~C%(8Ly9c&1v-e$*aBR}8WGV^uJlM4~Z|jI`Yg#8J
zJ?xR!#m+{1njIoeTly5O+oG2<4qA=pY|)=KLAB-OHfWKVHyK%Lg@}jgfs3bz=&1UQ
zcM9jt(9YO^xP&@AwA(&3>)nI`GW|_pS|e?SrQr2=f4r~)4CcuZ!^1SeU+_79Qk*WV
zt&1xL)a$?$+wP+KE9wyWJTklIhysLJj@Z`t^Mmc}Z&8;y`w6y65^q99w;|Jzk9->$
z0ti1hZC9~~Lz6C|da8b(SRz|T7cD2n04Kt<@O+~J0rq^<Qa(J)1-ZYD8Px;h#gyvX
zeFktZ^}adR++L7K&stkV#_);um?AN33^sc+vUqM8L->s6{IN14h+;F+C`~j3pZ|pJ
z{G_}G>fIdQ7xC*tiL(Ud+8ITl#ecXm;>!V5+kEY6YlM)`k|5V2LluR&I=1Hx>_i29
z#dwd*J?Kv<3+Xa#10>$zIyQbqAIS@Fs~zFfLyym>UgSTojgo@ZTP!x~*iS$7gy$|E
zR6E@&8j`xmdNO5hweY(T^oCY&&zVTV{@=M1l-Z>qUi(S^??iDBR=KJ#bWs?TdW+X8
z!UVy*x;DMcNeBYUZ9)c?#2{6$bl?>x4?MCfzxe2>0^4`3Sm7oeQ0LD)Y5hhIK4}}H
zVGbkc{Ai$|Gq?|`dKgQZYDD<^DbYQk#tJY6CZ_nWT~+#F9F@tK%1`5X5>x$QRwa*J
z*_Wf#JF&mAS>G|kwsSx3)KZ?Yh5`XoV`A$6=4$-N)g)tD|I5|>nae*z)lF-eB|M5)
ea1#@FKuG4&H<QlXw8=V5Oqa&Q@afo2{C@!^_I?il

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/contextHOs_80iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl
deleted file mode 100644
index da6da9554e2b676b03e6c9e9afb507aa89e1fdd9..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 4176
zcmX|^c{J5q8^>`($UKjkXQvV}<!r}19u6`dj&lx~iXX=q;Z;-;8H!4VZXq-)+&B;A
znj*K!P?u81o2x6`5PEg*``*3R+JEf*JnOT*dp*yy);9x3KOBE3CK*qUjt#*dijQK3
z2gk++C-VlyhR~xA$1!8$=}caX?Vl*#!;F7kF?J`m;~0k(I}O9-V4P%Zo2C={*2ZOH
zT>lGYFq30Kcrk7t4mLKnP-ASn6Ppuz4#q>qcr~4HKI~h!<`|!=tE=Ppt@Zce#Ky#S
z&`w%O={Db7OvJq*3_goIddsMpFk+Frt14xVfblai0a_Sa*xz`;ur0O_8591&#{FQ6
zgz=uVvcF99REzrBNsxvS5xi+NVb_^c4|x+hS%up2jz#>#1ktgLPP}L{!6}S;7M<&3
z9YFh}pUFOAb=>qcu4=i)5(tmrDY@~EfQd3Ov43geKWGwUO!8lv)Zes}jDva8#*Kuw
zx(jz_zg7_Pb}CDbuvZhjRZqn(8C@iJZS-;+&#EUhy5UJ{%T0uIIm*LFd+G=S+3S?O
z3=hJ5z#ic%%GU{)G!v7d<zTW~zvFc7{~G0fQ02*(!avk=+1So4Dp5#)jg7ZoyQyn6
z2qMn5O`Q#+!fa*!`$CCeaOm-tygnKXk8Im}zs1uajhDRGJ3s@W)<0r5H)s&H_-dyg
zfev!nd-iEwr9(66AoT%`0VX!eGVkOVaEtr`M}9|#XBzpZ&-{lDXFk>zoV26E+w7PS
zSu!1#!UgqQa_Qhob**N0(BT?&A%E~C9b%4oW#fnFAY$lz({+Fjx7^;8rH|90r76$r
z*eV?s%eGSpTbyIdEn|r@bSU3HtNXN$4w{2?0U4om*iKvi!m_4AcVwb*Z!jHhU)^X;
zX3>G0Un!5>iUGf?3}<O|GeDPW;ACzP0#zGE8z)YLz_65;+V%br(91dS`Net&d^aQV
zfnX@ae|}nfay10rG(<h;Y!3m0z=JiN(IFsSdTZ}zl@QP=dcspP#Q^&U;SJVz7!cdW
z;ct+^0K>~hg#8W-sBzbPeL{!<E5lNwx882)@_4ja{Fn}}#+i{1fDZM%Re7Jc^wx{F
zxxSgF1BZi*sL+<ap$r4nIS~eQCz%N<$ui)K$;@DnA_D?wdIPuB7$E)C6yK)A03(<9
zZi}t>=qHv!lFf8za5vu`X-bC#0b-zUDGfe{>&i5L3Wl>yg{gHR!EnPl;#n78Fvu5c
z7R8KHftgx5T*{)toSgbm+%YO}aP-H?b5mgx8V*b(2STag$mh7&04VxiVeR4W2SbKW
z&z5BRKqxzVWje(h)Xxu8w~Bhh#r+Y1cT2qCb@|Iz%LP7AH|fmYn(7N?doCZ9dE^WI
zNBiDhUGsrTieE36h&Obsz3ccu_JlG!p+~O!-C@`<qKEgEGo+5(e^RY#54A(j>hOs+
zAf5Mpm}A2lWH~<u?WnQ^S4n%`yoU}DkB*IR$h!dN(L_2Yl?1{)Ydr!9Bq)%K+54M{
z3qZZX=`|S#__8R!S>I?4Ujr7?CkU2+j|@%Ijw3*Rocwr;5d!9~S*u2(L@<*4{jHy)
zHAs2;`FO6{0QbEI^9R8ea%c?)Jcn$-wC!a@<4?8_&!4WBQDF-zQ3ILMy|zG_4`yB%
zwuRb)+IY@rTd41day%bq11i3<#q=dB_;m1kpiBZ0IQ442J@d7MLNn@BLMMWKrzA{^
zQxGWYxrAv5BG@I-GF>EQ0bS`Sl$;<_kh-*vKdNI0bq3M}`~*B?U+@r+UQ~kXR3Y^?
zaUN(=yG*N;z#(c&L3iWbo#=(l{V~(4Iw;a6QGQC-0F8uo#1eZ<kuuUceEcdxLrE7&
z91pBeimI4+^J!a@teY@Yr|y7$Q<~$h!kkcoNWR{njtjc;_Cx*eA6!t|@Qo%=azPIp
zf9=pOc0?0KN>$a4w#fQirbp{TgyydCF~ul`NG6Em+u3(2NIa3PyutoG;R}0Pmt=q>
z(7BG_EsiLI_UqUW9(<}0m0x|3@4FHxJN|y<X1g3nnOu4PGEV}c!!-tNcZ-AFnBDD}
zW3tda9=5ycvKkyKmx`O+y9fSiC{LN4F@c^BbzWo#BHVj?zq9QZZxB1{>Lq^F7x-+h
zl=UnHz{Nr`k4`xi=z4CiYg=g0M3>vcv(+c!@v4LhUj}&B{p>Ze)fd=y)bm^`7%T-T
zoi!~Kz^zMa=}7y*j7|7VGtmc{-s@i;-suCe^&~mIRUc^NcO$Lx`$383OkyUtKkzGk
zFkr0u!{lMfXgLuIWHcWTbRq>pLrurw&BH-3e<mS)fR75o)7vh~a#2A@{(k$oeGo|G
zzAmm{3xwzW?@#6B2f&xhc$RyVKj58pCT{HThvyFxUb)}#heDX-95tf=wI`p|ryB%S
zFYVV^N2rivo)U0hGZ@mIa->&W3x;mzpjsCx8XOk?sy%#>2B*YiX%^;mP%n&a-TXp_
zvdBNDf;zY6<j(FV<CjC=;n|2+xu-*6#iUbxTh2i+%@5W*kQfG@ufK`;>xF|6q4|nJ
zL^yo+{n8h)76zR7GR}=>2SG~zvtEo`C@4q^3TkUGAcQYwu+EhRIc0&y0&l1w@`5h>
zWG)Cg$J@6N_Xa_N@)28oN)Q}6MjIX4PK9CP?25S*Dn!u*-iA$5;YF%*{H#GRXg;4G
zbvzLaJD-`g_wv)gmeowUuug+Go=XX*;~5~pu^ht{8wz!WjI5BpFqmYQY%X<)1U-QQ
z?}b-~z-n)&NV8T9v`f7vz4yl;rtGMoWO6KwiLMP0u{g*w&q|!tWkTOMp;WGXCe&%p
zJAX}Lf*9)z@9M8{P;N^0rWM9QFdw^S!9Wb84072X4mkvFvbQ&w?BQ_ntVGl3dj?o&
z$=U0B1%uk;!Qs{?0WfnUpmc5B8%}GSU%l{$2aLEssNRXW!9c~Pgs!|3M7qk#PYT(A
zY7%>McBBosPBjXJZ_SHpoAzM;bVnH7)c-S$(+&O@yY5ulP6j>aSWeYYPY{0{I%p;1
z37@jJom~iX2V2RojHEaAz+EcGo%huU9&T=ywiU^vI;Z)h2U1ojY5BRXNQ)cVZXao-
zHg^DRPYwT;(BqGeo_e?Lt`dZjqa|d1Nunars%^frH>hY<-DSsJKPoypF|6={5QNP7
zxuwYB6qL^C7Ah+2hd!hXM^^vtg}%iaHl{_pq3@6KM>{;N&|1l8pAokrGS3{<Clu}j
zoV`#Mq0kZXL$|r96?g#8=$*JXR3C^+e|N*G(I4EKuUwF+q=1~R72BzdK)8u3YJ6K3
z2p$Wva=+*X!cp$MC-_1sz%d>p^wcW=@V;M8FU|Nta?+|8dCCX!C#(y;;|_o<d)dx+
zI5Om#UaoC9?*PsxAMX^OGXeFPsgU8TIMnL?P{NJ_p_g>@w%y4U&DXf=CZ>9!O7c>;
zLz*8tOW0j<@(u-A+D6~Rx&zV8^Cu=BU!<TseuKpTFF%CzrfMTYz0kzDnv<{CNr<ZV
z=aSAxBJ#X-X8oJ$ZX~yIf{4v6u)YPK5xk_J1q0(5UAN5l!Y9&bM`*keMBgwr%vU#t
zR_eEkF;!D|KY4FT`q_TqY%LMX*|3B<CC8$<7+atl;Yp*ZW=O^RWo~@~9_8&Ev0M?A
zfw^zEJPQc~FLrdLJx_K7`_E<lQQfYfpt8Svam5Lq&b}VI#$gTI4OroPo*@_uN{AP#
zDnrX~Q~UYbim*YgH6R7<1%Jb4uR~HqU>n$|@QQE%X<Os3aV9Rn;{7Gw_^}HtD9kA<
zEIPyEg)8+-N1S13Nr~Lg=K`x6cT08<UE$hfv4~?32}G**)7BeF5FtvD;i__l0+qp3
z`j`{!u^v=)cCdqw^?qg{{+3(wff6vJXao~&`bAHs)uC|swD~=8QP8y3iPufW0S=!_
za6HZh8B#BFa*y(X++W&*ney8}G+v=AB71^0zgFu(DqkZUZ~Jp5>;(?p&nXQ|bX_I<
zl&8?=(fpjnbMehSVYTlp$G?8w*!P|tyd(KE$_FM{bzP(w(d*lhoVi7Isg4qAD04aS
z>c}ocdTBkBd(Ie{L|5tFt+hm--#F_$`(lTV)%2xo8*@dvL>;_GFBx4VlPJ?=UZ^*~
zm-V&48)e-y?K^eP2dQiI)~}F#5q7ady{OL@WjyUpxVu#!-?X3-`|gMIK4t0MY4S&p
zSgt!lyaKlB=~F@O)c_>)_VJ%C-T{ceHH(z0=#LEgqmSs$_#pr3g=piqUdSm}c+@SI
zj4I#BT^g1nA?4e*ju~+}p^1(%F42<?NG8-N67@NvV)^#ze9Bh+=4wtGcDbY1-a{^X
zz!PO=D^;g7d!zK5V<%#q{ZLw#=>lCl0F4=5lnqy*Al$a|p=>4;^rUjuV5uYk<t%0@
zKaTZBrjDr--YdQ+?1{AdZ9^Z#ST4>C#~(o2(xo+1pFNPnJ$u>tyY5ING3`b=m4u$&
z+r6@A?~Ls0I;N(*9FfnZ{uW1?10v2^`xR~4qbGAt+AZhp(I0jp^%WI%XqlNm9b0dM
zh*9+5<r747SmXK|#S7+WS7K07N`pSy;~0MG&6FZC`&DpOGi#2e==0DbSws<r^W}-r
zky;Qa)FqIXqz4<D5=uc$x-i4Gr|9mQ2E=y7<~}{72$9y~_H}^*V1M&#+!f9j1p6%?
z6fVYt%)&nKf6oy__=Q=A%4Hmyb`#TA3;4(q-9E8wJuMD6QKpsmh$;j*@>9$B@vso$
z`6^*TA54}rYHvO_ggcpcEx8u<fn@fnjb&s4A88LMl4B-dw=d@u?+p`(p7UNjQf>@!
zY{r^psYc-UpYUBDRrW%Yhx5B40X-;nm84uNQ36`>`=7@BIiQ-y-=V%v82POTaV;~{
zP^7zaPyWkY=<M@iyw}OS=nrWtnQ|RNB+={scJe0!q#($xeu!TmJuFeXByd3orG;v=
zTW!^GfI;{%@7=tpezrp_EOVLFIb&(F^qVjYg;#Pfm`Z`gZ`>&=?9z~|)A`~zq6CPj
zUNsQDBm&Aq#T%88LSR{2pIz=E3_%rkVJ}z2AycUA<ugnHc;(i9_S07bwsq}9kyc&M
z5IA|<c0?aO>X@K04rA#5V5q4(ydSEcGFG(IiSXAWqGwQ@4Pc5)OzB^{s{F$^s*o|&
zzm4N@OznqRl{)g%{?p1sQx;Wi2F^M5{T8?rEBPjxiUdrZiD~?stN9~Wi;QXiFIVU9
pT-hY-fy|x}f`G)CQLXyBgy?%UWmUGH2$(Jt)1xsld^WZV|6jEhnUw$l

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/160_py2.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/learned_position_policy_parameters.pkl
similarity index 100%
rename from mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/160_py2.pkl
rename to mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/learned_policy_parameters/learned_position_policy_parameters.pkl
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights.yaml
similarity index 100%
rename from mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml
rename to mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights.yaml
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
deleted file mode 100644
index ad3774c36..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -187.49195238797435
-- -169.52512966931013
-- -35.69274095939003
-- -110.38385915264884
-- -68.02909986220745
-- -128.4282807320246
-- -173.64174760465733
-- 26.417804453060466
-- -265.1511968093563
-- -208.2345262812316
-- -236.22305824029408
-- -292.1579456291514
-- -192.16922081868017
-- -73.72506100791689
-- -227.88416746725505
-- -245.88529994956338
-- -87.801284061494
-- -51.537954121202
-- -218.31943366179135
-- -19.00084265897026
-- -50.73202708213524
-- -156.08245180901318
-- -52.57445775081901
-- -342.5208997441454
-- -81.77810876739974
-- -174.1958381815731
-- -272.58419638629414
-- -22.427456591272964
-- -67.94830216667698
-- -131.55116573738906
-- -51.52359182361245
-- -288.7416926303152
-- -66.70671027140779
-- -342.4841150689501
-- -166.87744597718262
-- -135.67945475651916
-- -115.80670276581463
-- -392.53333883637134
-- -36.90191158618114
-- -173.80064748268455
-- -163.10641272099767
-- 7.960374041073862
-- -45.734150579455836
-- -190.36730444634858
-- -64.03257774809776
-- 27.78625095467416
-- -118.17315931763699
-- -57.888007878300165
-- -292.44891798678657
-- -85.33059310547326
-- -100.37854674098924
-- 1.9952291209454511
-- -167.6282204094909
-- -133.57127509235013
-- -225.0075163851039
-- -111.17327201802011
-- -242.4716831428446
-- -284.25269822833735
-- -251.59393903872822
-- -13.405909200151447
-- -267.09360040817
-- -193.2961594890842
-- -173.45487948432415
-- -86.87912728215895
-- 137.3803737089481
-- 14.729982103733787
-- -20.43551374181382
-- -183.23852169109657
-- 39.56949470282565
-- -11.746766870171504
-- -165.16494799780983
-- -17.72624341018731
-- 5.573884446233519
-- 119.93511795969849
-- 5.406619106751105
-- -28.33944076716923
-- -93.42445394305315
-- 31.358207958221207
-- -127.19272668631712
-- 47.61360021679253
-- 177.24358365165094
-- -33.28653474678124
-- 113.66223403198586
-- 23.55404380676361
-- 18.589236158322773
-- 290.5105976047401
-- -21.873343777140963
-- 34.340774653167635
-- 123.0549442804791
-- -50.73662541049178
-- -1.7314731873450575
-- 82.52433599394868
-- 47.321040690639705
-- 126.87991145000166
-- 268.17453639949167
-- 78.9263070743615
-- -46.985695686376765
-- -33.15861159657017
-- 121.44805915038098
-- -106.33355817627528
-- 33.220209348132734
-- 222.96938720583944
-- 47.52574406413015
-- 200.37330393698673
-- -37.25494538781959
-- 99.80531016763302
-- -89.93774989127296
-- 105.89444419617598
-- 56.03368438186566
-- 40.309626973350035
-- 270.8644142655386
-- 43.16449749064583
-- 55.65751358535125
-- -30.452922323427053
-- 107.68085844090612
-- 206.11784411834313
-- 185.86376583310394
-- 77.09904197384182
-- 113.38528890435701
-- 154.3326219589348
-- 53.403925267536046
-- 115.85374833333367
-- 153.4670922296937
-- 291.63757494319856
-- 203.61974431849197
-- 213.60396469189791
-- 170.24873764605687
-- 136.85461496589278
-- 25.058861185881806
-- 145.00467183992313
-- 60.785220824967766
-- 236.67380042158126
-- 386.4651972576292
-- 149.45913716260057
-- 265.2513397719353
-- 264.91108470657554
-- 35.60643577997812
-- 53.2349506925678
-- 67.29109718480608
-- 177.09866691473454
-- 296.3408306128541
-- 165.8042545452507
-- 22.43858524866842
-- 107.52321752590935
-- 123.90852051072142
-- -40.14079797607926
-- 161.7140116579578
-- 284.8878210665913
-- 7.717525716715916
-- 125.9785847752208
-y:
-- -187.49195238797435
-- -169.52512966931013
-- -35.69274095939003
-- -110.38385915264884
-- -68.02909986220745
-- -128.4282807320246
-- -173.64174760465733
-- 26.417804453060466
-- -265.1511968093563
-- -208.2345262812316
-- -236.22305824029408
-- -292.1579456291514
-- -192.16922081868017
-- -73.72506100791689
-- -227.88416746725505
-- -245.88529994956338
-- -87.801284061494
-- -51.537954121202
-- -218.31943366179135
-- -19.00084265897026
-- -50.73202708213524
-- -156.08245180901318
-- -52.57445775081901
-- -342.5208997441454
-- -81.77810876739974
-- -174.1958381815731
-- -272.58419638629414
-- -22.427456591272964
-- -67.94830216667698
-- -131.55116573738906
-- -51.52359182361245
-- -288.7416926303152
-- -66.70671027140779
-- -342.4841150689501
-- -166.87744597718262
-- -135.67945475651916
-- -115.80670276581463
-- -392.53333883637134
-- -36.90191158618114
-- -173.80064748268455
-- -163.10641272099767
-- 7.960374041073862
-- -45.734150579455836
-- -190.36730444634858
-- -64.03257774809776
-- 27.78625095467416
-- -118.17315931763699
-- -57.888007878300165
-- -292.44891798678657
-- -85.33059310547326
-- -100.37854674098924
-- 1.9952291209454511
-- -167.6282204094909
-- -133.57127509235013
-- -225.0075163851039
-- -111.17327201802011
-- -242.4716831428446
-- -284.25269822833735
-- -251.59393903872822
-- -13.405909200151447
-- -267.09360040817
-- -193.2961594890842
-- -173.45487948432415
-- -86.87912728215895
-- 137.3803737089481
-- 14.729982103733787
-- -20.43551374181382
-- -183.23852169109657
-- 39.56949470282565
-- -11.746766870171504
-- -165.16494799780983
-- -17.72624341018731
-- 5.573884446233519
-- 119.93511795969849
-- 5.406619106751105
-- -28.33944076716923
-- -93.42445394305315
-- 31.358207958221207
-- -127.19272668631712
-- 47.61360021679253
-- 177.24358365165094
-- -33.28653474678124
-- 113.66223403198586
-- 23.55404380676361
-- 18.589236158322773
-- 290.5105976047401
-- -21.873343777140963
-- 34.340774653167635
-- 123.0549442804791
-- -50.73662541049178
-- -1.7314731873450575
-- 82.52433599394868
-- 47.321040690639705
-- 126.87991145000166
-- 268.17453639949167
-- 78.9263070743615
-- -46.985695686376765
-- -33.15861159657017
-- 121.44805915038098
-- -106.33355817627528
-- 33.220209348132734
-- 222.96938720583944
-- 47.52574406413015
-- 200.37330393698673
-- -37.25494538781959
-- 99.80531016763302
-- -89.93774989127296
-- 105.89444419617598
-- 56.03368438186566
-- 40.309626973350035
-- 270.8644142655386
-- 43.16449749064583
-- 55.65751358535125
-- -30.452922323427053
-- 107.68085844090612
-- 206.11784411834313
-- 185.86376583310394
-- 77.09904197384182
-- 113.38528890435701
-- 154.3326219589348
-- 53.403925267536046
-- 115.85374833333367
-- 153.4670922296937
-- 291.63757494319856
-- 203.61974431849197
-- 213.60396469189791
-- 170.24873764605687
-- 136.85461496589278
-- 25.058861185881806
-- 145.00467183992313
-- 60.785220824967766
-- 236.67380042158126
-- 386.4651972576292
-- 149.45913716260057
-- 265.2513397719353
-- 264.91108470657554
-- 35.60643577997812
-- 53.2349506925678
-- 67.29109718480608
-- 177.09866691473454
-- 296.3408306128541
-- 165.8042545452507
-- 22.43858524866842
-- 107.52321752590935
-- 123.90852051072142
-- -40.14079797607926
-- 161.7140116579578
-- 284.8878210665913
-- 7.717525716715916
-- 125.9785847752208
-z:
-- 147.0697286588254
-- 147.74588784221456
-- 112.65566266136821
-- 18.359232177402554
-- -111.9742511820463
-- 15.31172882006791
-- 6.609434781575087
-- 135.57361669924134
-- 89.8413925818162
-- 191.02570989931755
-- 121.68098347990863
-- 310.18469024245763
-- -107.20626443396715
-- 14.210755115910601
-- 68.47681492369398
-- -53.39821489494769
-- -160.0670830406598
-- 57.91893447524476
-- 143.91248619820385
-- 85.5563450002862
-- -17.376218538929496
-- 109.06118809206095
-- 0.12632833243893538
-- -71.9225993333965
-- 7.2990835702683645
-- 192.10773090491654
-- 121.89379742888093
-- 179.66036483456833
-- 127.17688444271954
-- 241.32545602197393
-- 104.89248092035686
-- 16.381743635433864
-- 112.18795371996411
-- 119.33459557930752
-- 242.74757392701636
-- -117.28935615734264
-- 117.16627871389215
-- 13.469368866837582
-- -4.529395518536364
-- 245.09004846012132
-- 142.67893335445106
-- 113.42158548718666
-- 171.89491633957874
-- 37.58001564045737
-- 297.6674674270416
-- -38.36747257612112
-- 322.9240932227268
-- 334.65733991230314
-- 335.0318034746627
-- 450.14589353644396
-- 355.3340767107325
-- 436.97904739499404
-- 99.95734081688825
-- 291.63120762804675
-- 341.0811466216531
-- 470.60709366837676
-- 271.24215997323773
-- 332.59719180039184
-- 363.15201384486556
-- 370.54329377801184
-- 352.4324627185788
-- 272.1076188601076
-- 193.5154978871954
-- 285.19615003876777
-- 485.2043077127898
-- 199.85713032051675
-- 232.79115369372064
-- 368.7763609306992
-- 258.2277315298386
-- 431.8340999377656
-- 281.05661759873607
-- 303.9240761547915
-- 439.89157193620275
-- 399.2473016024376
-- 352.686354922071
-- 271.8967458082695
-- 342.3068343729032
-- 363.9795427725253
-- 522.1903026124583
-- 468.83337875499006
-- 391.5060739650679
-- 381.5820600899362
-- 151.22523327357072
-- 439.29634480040147
-- 189.10718421103832
-- 407.65805256539613
-- 251.12864627901752
-- 173.68656601733437
-- 64.4314952848051
-- 231.5543646649201
-- 115.04391184200999
-- 419.5292522009196
-- 63.02135408071294
-- 335.2168582715738
-- 190.13034431554857
-- 10.247919743513544
-- 173.420969982708
-- 234.86765440011254
-- 124.04387428861862
-- 212.67267316149872
-- 222.2869721518006
-- 51.072435123440755
-- 119.96146399624313
-- 235.15905582840983
-- -6.449655145401507
-- 153.40182343075242
-- 290.30123878513825
-- 100.81602872463795
-- 55.66033162087278
-- 123.46886876126574
-- 51.24113167810543
-- -31.79771727056099
-- 188.4114987950756
-- 231.83198061552278
-- 50.83621979811791
-- 226.83855617509082
-- -71.98439831506886
-- 235.6002360525736
-- -1.4830821361137023
-- 71.25253644792186
-- 12.868868326837722
-- 95.86686059120905
-- 141.00922400017984
-- 248.8011363353016
-- 34.531073331684986
-- 254.651501486854
-- 158.52662670520084
-- 211.19999911135668
-- 203.44046518137026
-- 147.85661102132698
-- 140.80016347894465
-- 222.31673242383246
-- 282.0261807013606
-- 22.487483903790896
-- 77.77002397719545
-- 65.01869511417857
-- 131.4165488618371
-- 117.95918980344477
-- 175.77414442932096
-- 125.11179322132699
-- 172.4173230733195
-- 25.243442056303145
-- 36.26024110752745
-- 316.36891113395444
-- 243.97128445773103
-- 130.61931796016933
-- 92.40271625000324
-- -149.04938390927575
-- 84.38242545119284
-- 80.72949701618471
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
deleted file mode 100644
index 034340a77..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -201.04147127024706
-- -167.2523471707599
-- -23.50135210896892
-- -153.13294915070543
-- -30.619781729849734
-- -125.57368293643788
-- -178.08439653188327
-- 62.83275160635712
-- -239.15284128925578
-- -228.03087956235342
-- -344.8906958677936
-- -275.33749943999624
-- -255.7609565519209
-- 2.279808122905889
-- -214.27116514894607
-- -371.1288460507514
-- -39.959069906873296
-- 25.69977166716337
-- -246.29003388552277
-- -66.56237063945625
-- -85.47673594534648
-- -216.557964259047
-- -44.256620618839115
-- -308.41706660891793
-- -141.83656062154273
-- -186.5737208290361
-- -264.8159580639565
-- 21.09428847793057
-- -99.02263135039374
-- -241.45140915041816
-- -60.0153002212281
-- -281.4226021933602
-- -139.67077463266855
-- -398.3843932248262
-- -191.4638108446459
-- -145.68718027802007
-- -105.96600739077311
-- -434.71412153803016
-- -65.94347855017091
-- -143.58957932066278
-- -220.24193966712153
-- 42.23677704057419
-- -26.489960612303424
-- -208.84164265949022
-- 2.0147261992437735
-- 42.95136987656696
-- -34.66600593993395
-- -137.23795534595922
-- -330.6151271417497
-- -142.72606625502956
-- -96.49725745562209
-- 52.24933122390811
-- -172.44942696005236
-- -197.14756176873803
-- -239.51094761047256
-- -35.42406003348334
-- -189.90600347995877
-- -263.0995480025847
-- -261.38202538433524
-- 19.031078473612904
-- -340.8858161607905
-- -174.35873238049166
-- -205.86303683202112
-- -72.65734662654334
-- 279.4061625440003
-- 64.68537566450004
-- -13.186075154329556
-- -112.23346234543293
-- 134.3271343911593
-- 19.329528222566616
-- -43.112315515462235
-- 59.18580744908775
-- -7.367179176849072
-- 156.46592900612185
-- -21.0628015724817
-- 40.062659109965736
-- -121.09100219333905
-- 55.58073784666084
-- -98.7634737409404
-- -44.222306993784265
-- 260.6374894645528
-- -74.24302907024753
-- 139.06408596403213
-- 74.5132284592363
-- 37.782902039733045
-- 308.6128578878652
-- 56.210995895351495
-- 44.096706376260315
-- 245.95854032779206
-- -71.04479576485645
-- 67.66139996703141
-- 73.74526490662923
-- 106.90223500221825
-- 166.94351470134933
-- 285.5251346087024
-- -5.567322951604867
-- -72.68258307194573
-- -45.151988554119214
-- 67.80374005810233
-- -58.072895339898224
-- 90.24905134888387
-- 254.37723583013167
-- 72.29549320333595
-- 260.34176464676153
-- -62.531555267807704
-- 141.35374244849817
-- -62.18005929453309
-- 153.92359000530257
-- 105.23951868310732
-- 47.27948737622441
-- 253.80568421218246
-- 44.67792618406605
-- -4.102749792143297
-- 27.411615717986262
-- 91.51731269469626
-- 258.9568884245361
-- 302.1519261293808
-- 171.60350739134424
-- 161.66629226379598
-- 173.82461332097418
-- 0.006196471457665639
-- 133.11550045790452
-- 130.85157497111507
-- 270.6715274563171
-- 242.52754536508093
-- 68.74489243472402
-- 171.8568371983085
-- 90.14523822958101
-- 22.18229039361379
-- 183.18193882456524
-- 91.78535767377343
-- 277.4074125496988
-- 471.0134425098973
-- 181.0957245116598
-- 191.63677933453334
-- 306.46640679256234
-- 114.76828630347208
-- 20.965552811472538
-- 20.716058857589132
-- 193.08059043698137
-- 390.55719089887765
-- 210.57986191532072
-- 52.15275437102519
-- 199.58823721871573
-- 65.72488807252493
-- -123.34417482594056
-- 232.20209175345101
-- 272.29857135174734
-- -106.0729419699781
-- 234.37020913355548
-y:
-- -201.04147127024706
-- -167.2523471707599
-- -23.50135210896892
-- -153.13294915070543
-- -30.619781729849734
-- -125.57368293643788
-- -178.08439653188327
-- 62.83275160635712
-- -239.15284128925578
-- -228.03087956235342
-- -344.8906958677936
-- -275.33749943999624
-- -255.7609565519209
-- 2.279808122905889
-- -214.27116514894607
-- -371.1288460507514
-- -39.959069906873296
-- 25.69977166716337
-- -246.29003388552277
-- -66.56237063945625
-- -85.47673594534648
-- -216.557964259047
-- -44.256620618839115
-- -308.41706660891793
-- -141.83656062154273
-- -186.5737208290361
-- -264.8159580639565
-- 21.09428847793057
-- -99.02263135039374
-- -241.45140915041816
-- -60.0153002212281
-- -281.4226021933602
-- -139.67077463266855
-- -398.3843932248262
-- -191.4638108446459
-- -145.68718027802007
-- -105.96600739077311
-- -434.71412153803016
-- -65.94347855017091
-- -143.58957932066278
-- -220.24193966712153
-- 42.23677704057419
-- -26.489960612303424
-- -208.84164265949022
-- 2.0147261992437735
-- 42.95136987656696
-- -34.66600593993395
-- -137.23795534595922
-- -330.6151271417497
-- -142.72606625502956
-- -96.49725745562209
-- 52.24933122390811
-- -172.44942696005236
-- -197.14756176873803
-- -239.51094761047256
-- -35.42406003348334
-- -189.90600347995877
-- -263.0995480025847
-- -261.38202538433524
-- 19.031078473612904
-- -340.8858161607905
-- -174.35873238049166
-- -205.86303683202112
-- -72.65734662654334
-- 279.4061625440003
-- 64.68537566450004
-- -13.186075154329556
-- -112.23346234543293
-- 134.3271343911593
-- 19.329528222566616
-- -43.112315515462235
-- 59.18580744908775
-- -7.367179176849072
-- 156.46592900612185
-- -21.0628015724817
-- 40.062659109965736
-- -121.09100219333905
-- 55.58073784666084
-- -98.7634737409404
-- -44.222306993784265
-- 260.6374894645528
-- -74.24302907024753
-- 139.06408596403213
-- 74.5132284592363
-- 37.782902039733045
-- 308.6128578878652
-- 56.210995895351495
-- 44.096706376260315
-- 245.95854032779206
-- -71.04479576485645
-- 67.66139996703141
-- 73.74526490662923
-- 106.90223500221825
-- 166.94351470134933
-- 285.5251346087024
-- -5.567322951604867
-- -72.68258307194573
-- -45.151988554119214
-- 67.80374005810233
-- -58.072895339898224
-- 90.24905134888387
-- 254.37723583013167
-- 72.29549320333595
-- 260.34176464676153
-- -62.531555267807704
-- 141.35374244849817
-- -62.18005929453309
-- 153.92359000530257
-- 105.23951868310732
-- 47.27948737622441
-- 253.80568421218246
-- 44.67792618406605
-- -4.102749792143297
-- 27.411615717986262
-- 91.51731269469626
-- 258.9568884245361
-- 302.1519261293808
-- 171.60350739134424
-- 161.66629226379598
-- 173.82461332097418
-- 0.006196471457665639
-- 133.11550045790452
-- 130.85157497111507
-- 270.6715274563171
-- 242.52754536508093
-- 68.74489243472402
-- 171.8568371983085
-- 90.14523822958101
-- 22.18229039361379
-- 183.18193882456524
-- 91.78535767377343
-- 277.4074125496988
-- 471.0134425098973
-- 181.0957245116598
-- 191.63677933453334
-- 306.46640679256234
-- 114.76828630347208
-- 20.965552811472538
-- 20.716058857589132
-- 193.08059043698137
-- 390.55719089887765
-- 210.57986191532072
-- 52.15275437102519
-- 199.58823721871573
-- 65.72488807252493
-- -123.34417482594056
-- 232.20209175345101
-- 272.29857135174734
-- -106.0729419699781
-- 234.37020913355548
-z:
-- 249.30936970701725
-- 120.61855374577983
-- 88.51943638748523
-- -37.27638287099523
-- -249.83266708212986
-- -18.177204468805662
-- -98.74885491605013
-- 176.97962575030346
-- 149.45102926216487
-- 222.50692355023767
-- 96.84267677464365
-- 237.66034703685702
-- -126.70915445427974
-- -44.65420067168505
-- 38.04639606786077
-- -196.01010647253312
-- -200.78799593386236
-- 15.036720386788945
-- 52.54811037578521
-- 18.07877118426772
-- -57.4093398932157
-- 3.6025400862264245
-- -53.156746387694376
-- -221.40087385758676
-- -31.326460682719077
-- 153.89632299904753
-- 98.72067944380399
-- 170.5520255489363
-- 51.58451076299889
-- 231.261612637923
-- -3.026138910427871
-- 108.99064480497259
-- 33.0448811253988
-- 70.16689432543035
-- 230.10084011498412
-- -188.66331130041365
-- 59.77004478430251
-- 55.247626585229504
-- -15.111463163670479
-- 301.9343390150364
-- 53.12875931817165
-- 86.47901096035412
-- 126.9803736646344
-- 6.657043193290982
-- 338.2610723382575
-- -35.089663976047895
-- 362.7873744869064
-- 408.6517567046632
-- 313.1947391612431
-- 462.4823592374066
-- 384.1008702702842
-- 488.94229060927137
-- 164.79345374558173
-- 419.4527587833925
-- 346.3038587428371
-- 530.5884101358595
-- 360.79908258410836
-- 389.01921341251165
-- 415.67617029093947
-- 308.08129249135584
-- 403.01728601048393
-- 311.8332733496349
-- 267.61198616890204
-- 256.1061830113214
-- 621.1958712803905
-- 161.83008760845297
-- 257.4914767960563
-- 437.9875316315236
-- 250.86316212986466
-- 412.0685566775606
-- 256.37068422284017
-- 319.58769118215133
-- 509.44667572187643
-- 425.7620586153827
-- 340.25901185268805
-- 299.69580910803
-- 387.0631841218825
-- 430.35087816709466
-- 572.347267701468
-- 519.4380676617408
-- 448.3695343523369
-- 310.71202956241456
-- 203.55516496425884
-- 357.1802053274863
-- 132.37004571025543
-- 328.9035863936466
-- 235.01277959089552
-- 280.5841730327106
-- 118.81667680234366
-- 188.02938084047207
-- 161.64114366187053
-- 326.2691306784173
-- 112.06424178318245
-- 469.6477058640201
-- 285.5232385757413
-- -29.545754882842296
-- 307.49874707157056
-- 290.13524210030073
-- 212.48909881041584
-- 264.18144289665895
-- 238.84314137575788
-- 19.26112277757816
-- 164.7374235094159
-- 248.53827390660908
-- 48.11169185251769
-- 221.3731851843131
-- 349.9103175456074
-- 139.79215196283744
-- -7.323857813999613
-- 122.06816138591932
-- 70.63873784282016
-- 23.763489310211057
-- 129.79665714678893
-- 280.0777290498324
-- 28.619509788763942
-- 284.9047996431252
-- -121.14505589764663
-- 257.8155542450373
-- -136.80047380386117
-- 20.54770093401279
-- 25.641797135780646
-- 27.022963128249547
-- 85.60048054120156
-- 187.47694604471644
-- 56.51758064821176
-- 235.83209314242077
-- 134.74271756936014
-- 271.3715291700889
-- 303.3303493797271
-- 63.678494901385974
-- 172.13689452739587
-- 173.90286722217647
-- 255.38444320773692
-- 8.809125152185375
-- 59.733563534633056
-- 86.13954853681179
-- 57.258267215494065
-- 198.46902308963632
-- 111.29466235293138
-- 151.92572265717888
-- 148.99031509355103
-- -30.005864551173687
-- 7.613704816481292
-- 375.0646843902285
-- 277.2043147617383
-- 195.06683472314032
-- 96.87061248081244
-- -105.89449755489385
-- 33.73473466239024
-- 136.2264101780908
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
deleted file mode 100644
index 231edc66c..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -137.45017437663398
-- -168.4801777684752
-- -84.03258737390696
-- -138.8989087267409
-- -2.082247962073727
-- -179.31723443328136
-- -88.42880815947395
-- -4.445333881119488
-- -242.64902467142977
-- -154.9719472652605
-- -242.0149929666361
-- -249.6424766495926
-- -187.5763161104164
-- -103.11645543685971
-- -156.91824303049796
-- -194.47157838400528
-- -83.05585362938487
-- -80.71853643395194
-- -258.3820101897301
-- -43.4329355576356
-- -82.8625965667567
-- -110.65002805845242
-- -84.10590340102833
-- -244.3473862277645
-- -87.37067637111183
-- -209.3757736776577
-- -194.4547880511376
-- -54.453791166857265
-- -47.85715533265113
-- -140.85880857135035
-- -82.301704209561
-- -305.036589417813
-- -88.51830760605337
-- -315.93045937552273
-- -198.54928242188186
-- -51.39778560761003
-- -117.76813593842118
-- -304.60018141822195
-- -154.20041553041935
-- -114.05395035507439
-- -144.7469051481917
-- -17.032567351447295
-- -126.08846452513698
-- -108.60229805313843
-- -61.70125602392602
-- -28.798590721569045
-- -105.1350744180009
-- -88.33021111454664
-- -227.77270995236424
-- -134.5288083889458
-- -108.11291016528452
-- -82.56938099982168
-- -217.23078139291084
-- -129.31246309353085
-- -265.2911672445468
-- -165.85228547123336
-- -289.52740630277026
-- -227.80079397765286
-- -226.78578207175408
-- 2.9445483397250083
-- -264.27926385368545
-- -161.63836424107424
-- -175.12507393760865
-- -61.83230511477935
-- 24.481477359040678
-- -111.267164036688
-- -62.89273463909397
-- -193.98677985400676
-- 75.07146587979011
-- 2.674657153233626
-- -89.63278756755514
-- -20.601486640645298
-- -2.744145913602563
-- 55.30311671136163
-- -34.9435583572032
-- -100.99868192265687
-- -85.29395233795546
-- 85.20039568455698
-- -176.76835907614668
-- 52.48593025487103
-- 97.31060265994239
-- -30.929742511464113
-- 106.63932096002576
-- 70.18662462848711
-- -10.065425939117953
-- 233.25664578824694
-- -6.447420142414308
-- 70.90920106859251
-- 49.249585826868326
-- -36.66468333200608
-- -8.240139082380157
-- 10.465166172261355
-- 31.417228186284632
-- 55.539924515738676
-- 202.74956671021314
-- 48.025112189715855
-- 44.361990849823506
-- -49.71439627804482
-- 105.97308932272578
-- -54.681766545070616
-- 98.75944838370496
-- 102.01544848216479
-- 25.080873351041458
-- 162.22640090937497
-- -56.31446975078492
-- 85.7752854141861
-- -62.429637512669245
-- 126.79934690184507
-- 131.7605060120888
-- 96.83130931872333
-- 175.34372538316487
-- -0.6565281246829979
-- 48.37504287501842
-- -21.534370462439625
-- 189.09948694132626
-- 180.52136054926117
-- 194.04524677409466
-- 87.25803792296719
-- 69.52770132418094
-- 114.85244694985934
-- 42.35868287770348
-- 170.59899218234273
-- 162.11679795069406
-- 212.00537694330902
-- 183.0337295001356
-- 195.29117473562292
-- 243.69238172951475
-- 95.26529740030269
-- 80.35386774469501
-- 187.86256733907143
-- 89.96477987585646
-- 190.7813245297523
-- 347.311248546426
-- 247.19234149548112
-- 204.08466094102704
-- 226.51696102301247
-- -6.892250336537785
-- 8.575084972868773
-- 164.82689518856662
-- 81.02253267814615
-- 232.71530374822984
-- 167.5681009949404
-- -8.604598362567197
-- 130.16109198377552
-- 146.88239969621665
-- 55.797281491238046
-- 165.4153603423753
-- 178.21146609083027
-- 17.487743041172827
-- 126.81818383969758
-y:
-- -137.45017437663398
-- -168.4801777684752
-- -84.03258737390696
-- -138.8989087267409
-- -2.082247962073727
-- -179.31723443328136
-- -88.42880815947395
-- -4.445333881119488
-- -242.64902467142977
-- -154.9719472652605
-- -242.0149929666361
-- -249.6424766495926
-- -187.5763161104164
-- -103.11645543685971
-- -156.91824303049796
-- -194.47157838400528
-- -83.05585362938487
-- -80.71853643395194
-- -258.3820101897301
-- -43.4329355576356
-- -82.8625965667567
-- -110.65002805845242
-- -84.10590340102833
-- -244.3473862277645
-- -87.37067637111183
-- -209.3757736776577
-- -194.4547880511376
-- -54.453791166857265
-- -47.85715533265113
-- -140.85880857135035
-- -82.301704209561
-- -305.036589417813
-- -88.51830760605337
-- -315.93045937552273
-- -198.54928242188186
-- -51.39778560761003
-- -117.76813593842118
-- -304.60018141822195
-- -154.20041553041935
-- -114.05395035507439
-- -144.7469051481917
-- -17.032567351447295
-- -126.08846452513698
-- -108.60229805313843
-- -61.70125602392602
-- -28.798590721569045
-- -105.1350744180009
-- -88.33021111454664
-- -227.77270995236424
-- -134.5288083889458
-- -108.11291016528452
-- -82.56938099982168
-- -217.23078139291084
-- -129.31246309353085
-- -265.2911672445468
-- -165.85228547123336
-- -289.52740630277026
-- -227.80079397765286
-- -226.78578207175408
-- 2.9445483397250083
-- -264.27926385368545
-- -161.63836424107424
-- -175.12507393760865
-- -61.83230511477935
-- 24.481477359040678
-- -111.267164036688
-- -62.89273463909397
-- -193.98677985400676
-- 75.07146587979011
-- 2.674657153233626
-- -89.63278756755514
-- -20.601486640645298
-- -2.744145913602563
-- 55.30311671136163
-- -34.9435583572032
-- -100.99868192265687
-- -85.29395233795546
-- 85.20039568455698
-- -176.76835907614668
-- 52.48593025487103
-- 97.31060265994239
-- -30.929742511464113
-- 106.63932096002576
-- 70.18662462848711
-- -10.065425939117953
-- 233.25664578824694
-- -6.447420142414308
-- 70.90920106859251
-- 49.249585826868326
-- -36.66468333200608
-- -8.240139082380157
-- 10.465166172261355
-- 31.417228186284632
-- 55.539924515738676
-- 202.74956671021314
-- 48.025112189715855
-- 44.361990849823506
-- -49.71439627804482
-- 105.97308932272578
-- -54.681766545070616
-- 98.75944838370496
-- 102.01544848216479
-- 25.080873351041458
-- 162.22640090937497
-- -56.31446975078492
-- 85.7752854141861
-- -62.429637512669245
-- 126.79934690184507
-- 131.7605060120888
-- 96.83130931872333
-- 175.34372538316487
-- -0.6565281246829979
-- 48.37504287501842
-- -21.534370462439625
-- 189.09948694132626
-- 180.52136054926117
-- 194.04524677409466
-- 87.25803792296719
-- 69.52770132418094
-- 114.85244694985934
-- 42.35868287770348
-- 170.59899218234273
-- 162.11679795069406
-- 212.00537694330902
-- 183.0337295001356
-- 195.29117473562292
-- 243.69238172951475
-- 95.26529740030269
-- 80.35386774469501
-- 187.86256733907143
-- 89.96477987585646
-- 190.7813245297523
-- 347.311248546426
-- 247.19234149548112
-- 204.08466094102704
-- 226.51696102301247
-- -6.892250336537785
-- 8.575084972868773
-- 164.82689518856662
-- 81.02253267814615
-- 232.71530374822984
-- 167.5681009949404
-- -8.604598362567197
-- 130.16109198377552
-- 146.88239969621665
-- 55.797281491238046
-- 165.4153603423753
-- 178.21146609083027
-- 17.487743041172827
-- 126.81818383969758
-z:
-- 158.02326918578157
-- 198.08270713502085
-- 150.40732024803066
-- 146.3603572079876
-- -41.19500332089733
-- 117.37981494138036
-- 2.302923059645904
-- 181.04525381157558
-- 172.63396790383476
-- 178.07163815708606
-- 161.19685538675498
-- 235.8854511963726
-- -19.226544814188046
-- -74.31841279243989
-- 127.29680678830495
-- -25.953878744418766
-- -110.50814340389257
-- 82.83930976554892
-- 120.91697818606839
-- 88.87446888789336
-- -16.902812891557343
-- 56.85478338905072
-- 69.00934463754652
-- -49.68741744154214
-- -9.101679383661015
-- 163.95892454666617
-- 61.43258379892186
-- 216.84420247404822
-- 185.22218746765108
-- 203.21048014672095
-- 97.38371513193675
-- 101.42786754109076
-- 168.99377875245335
-- 170.32463594119045
-- 161.06439769521265
-- -67.42510279636797
-- 159.8491514275903
-- 15.744297785606754
-- 36.96042354896769
-- 204.83440133079174
-- 164.7283072076716
-- 202.1410248341884
-- 77.70482171897073
-- 168.7922911943027
-- 244.00097801951026
-- 2.9843023004946025
-- 273.0393607004863
-- 256.41347082002375
-- 320.6619059534485
-- 401.71251984834294
-- 341.72170179270955
-- 350.1329908329461
-- 91.87121610461129
-- 255.47703533999365
-- 274.5606498965892
-- 342.2655000927155
-- 321.1279205451655
-- 285.48466346517364
-- 308.34554329406774
-- 369.36211101141447
-- 364.1232760577248
-- 240.19948245263313
-- 171.9618405697482
-- 284.3654144937668
-- 518.510436206006
-- 271.4532208272329
-- 138.7198458101441
-- 275.7730273345756
-- 188.89329236924783
-- 343.3625050778569
-- 275.2280291538374
-- 296.65913913594903
-- 405.5704033246094
-- 393.2103893208043
-- 370.27149169906664
-- 317.53326527607913
-- 380.31814269515246
-- 233.59783512066912
-- 342.147557717733
-- 362.9535851810041
-- 419.67160399069854
-- 265.45835389381506
-- 100.90811921422763
-- 354.0969035622084
-- 230.89361930084132
-- 324.40583740601636
-- 244.63191105585182
-- 99.70165437657711
-- 76.0001399413896
-- 261.90507855888444
-- 173.61056687477335
-- 329.9591859361577
-- 132.02454477101037
-- 219.9810010049257
-- 194.0331551005202
-- 55.502571068916865
-- 201.82715465650645
-- 215.71368923197895
-- 78.57045961284781
-- 91.17072503894212
-- 175.74982186894604
-- 41.27330521388918
-- 115.06087974810093
-- 187.2033099661109
-- 89.25423227897329
-- 187.79758532081587
-- 257.12729321932153
-- 125.31070453998093
-- 7.955194514485095
-- 153.56522817739673
-- 70.43107619025358
-- 24.83903870702466
-- 230.27040641909835
-- 307.4996805449256
-- 26.17525713219486
-- 218.64259246883253
-- -20.36896974357387
-- 223.9102604418838
-- 23.735580544108043
-- 109.11430168072079
-- 34.253522859618116
-- 142.25785742496066
-- 98.61000683239934
-- 260.3342095432354
-- 55.89480661163995
-- 311.1381924564003
-- 184.92109716413043
-- 157.19414087415316
-- 226.5880298111837
-- 151.92276857957907
-- 185.68381485151733
-- 215.93525411408908
-- 224.76084322528237
-- 55.86519063765026
-- 84.67151183335257
-- 85.30795753273236
-- 115.51125717589561
-- 37.6626762064897
-- 176.87293034901475
-- 84.54961955131282
-- 174.2064416597917
-- 133.86149628523765
-- 37.05246748249443
-- 288.640906832307
-- 169.94704947901835
-- 132.27640210349853
-- 75.31353581349363
-- -101.88672783775839
-- 83.0232257931865
-- 43.383037925239414
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights.yaml
similarity index 100%
rename from mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_7700iters.yaml
rename to mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights.yaml
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
deleted file mode 100644
index 91e5c9c17..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_1600iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -174.74077331965347
-- -317.77599446383755
-- -219.5467121612789
-- -34.26527761832079
-- -147.44317330782587
-- -98.57620030231213
-- -151.65272221714105
-- -44.52912641332917
-- -120.52842073352534
-- -243.33107798985682
-- -146.3566941741793
-- -66.8814686102606
-- -161.4122190812015
-- -114.40013392269593
-- -38.20685411333554
-- -76.49757335132858
-- -197.17504303148857
-- -210.53168931461644
-- -143.66662711064376
-- -233.84458118237575
-- -242.51407729894962
-- -104.91425287388788
-- -275.61592897105527
-- -243.75920267794496
-- -148.99487137986938
-- -284.76720175195896
-- -153.2791290521435
-- -106.26379155244385
-- -58.315973561353324
-- -202.43553120675654
-- -271.8081796267609
-- -128.4538837575546
-- -256.8534165017398
-- -167.40086328050833
-- -203.42908148748006
-- -44.36420499045635
-- -128.6498221073226
-- -269.27474389913846
-- -187.54103386976064
-- -315.71738715196653
-- -94.25734610085317
-- -144.04783055304117
-- -175.92457602755331
-- -213.4156276574594
-- -230.8093559485084
-- -185.84014917003304
-- -291.64188526827536
-- -241.01551292767718
-- -130.3470358146145
-- -198.0324329405659
-- -118.50455467300665
-- -109.13480914568198
-- -193.32769271564604
-- -85.61335922073
-- -124.51840287108118
-- -60.773949608455375
-- -92.21173022310548
-- -182.67613016758864
-- -260.849987866747
-- -78.95419597293886
-- -183.53742148620532
-- -126.55170547396766
-- -125.66020139022245
-- -109.19253515982136
-- -249.09387545053977
-- -126.6997017235669
-- -165.4853241880315
-- -183.1706321258363
-- -199.8006557550405
-- -142.0527901441929
-- -167.10657425912635
-- -72.13940725945159
-- -279.8488971483063
-- -190.76855531083888
-- -132.5052445914984
-- -186.31908792026428
-- -209.89053502992988
-- -194.54755649800435
-- -153.0563005077373
-- -175.31810037916114
-- -41.63516928736321
-- -289.6292074142173
-- -92.88580865593525
-- -201.57744071303082
-- -78.38496389675379
-- -102.37369818911844
-- -74.6941585482721
-- -237.56831160628943
-- -83.0022006028396
-- -242.9428963930302
-- -85.03874434022363
-- -105.36384492482497
-- -28.85214575091783
-- -121.28597754698242
-- -112.98964492193501
-- -2.9704925482979343
-- -119.57960415938308
-- -54.39173259806981
-- -60.4551546110456
-- -185.4739817360681
-- -164.71170765702635
-- -149.98905385415978
-- -211.76348838481712
-- -154.637051920341
-- -91.12614465455991
-- -95.57479207224236
-- -101.00267532749719
-- -111.93048321131528
-- -107.33955687007294
-- -52.93771532559152
-- -209.68143299171118
-- -64.20048990968746
-- -135.17922853492266
-- 42.619874283916594
-- -158.7504767982497
-- -139.61399692578732
-- -81.7830212971333
-- -93.49980450264374
-- -84.55991328286864
-- -183.16373315868958
-- -92.29208141458975
-- -33.488686584469505
-- -134.74813619936114
-- -99.31665471393255
-- -19.88126490813449
-- -16.409316431238704
-- -51.283508604150256
-- 22.053827055762667
-- 40.94648542458586
-- 4.900737059650758
-- -187.48534202576323
-- 34.45397696624966
-- -147.99163815898328
-- -60.54858213778748
-- -60.44634044362913
-- -115.13511819292981
-- -77.03381078997329
-- -39.63800983797401
-- -63.16467555367657
-- -96.24228971822636
-- -112.2598000100672
-- -232.1311472797242
-- -64.84084437939381
-- -112.0269519592484
-- -185.77267272757916
-- -80.48193848533441
-- -52.301216673442084
-- -52.03501539130916
-- -74.83519492876805
-- -170.03463198683247
-y:
-- -174.74077331965347
-- -317.77599446383755
-- -219.5467121612789
-- -34.26527761832079
-- -147.44317330782587
-- -98.57620030231213
-- -151.65272221714105
-- -44.52912641332917
-- -120.52842073352534
-- -243.33107798985682
-- -146.3566941741793
-- -66.8814686102606
-- -161.4122190812015
-- -114.40013392269593
-- -38.20685411333554
-- -76.49757335132858
-- -197.17504303148857
-- -210.53168931461644
-- -143.66662711064376
-- -233.84458118237575
-- -242.51407729894962
-- -104.91425287388788
-- -275.61592897105527
-- -243.75920267794496
-- -148.99487137986938
-- -284.76720175195896
-- -153.2791290521435
-- -106.26379155244385
-- -58.315973561353324
-- -202.43553120675654
-- -271.8081796267609
-- -128.4538837575546
-- -256.8534165017398
-- -167.40086328050833
-- -203.42908148748006
-- -44.36420499045635
-- -128.6498221073226
-- -269.27474389913846
-- -187.54103386976064
-- -315.71738715196653
-- -94.25734610085317
-- -144.04783055304117
-- -175.92457602755331
-- -213.4156276574594
-- -230.8093559485084
-- -185.84014917003304
-- -291.64188526827536
-- -241.01551292767718
-- -130.3470358146145
-- -198.0324329405659
-- -118.50455467300665
-- -109.13480914568198
-- -193.32769271564604
-- -85.61335922073
-- -124.51840287108118
-- -60.773949608455375
-- -92.21173022310548
-- -182.67613016758864
-- -260.849987866747
-- -78.95419597293886
-- -183.53742148620532
-- -126.55170547396766
-- -125.66020139022245
-- -109.19253515982136
-- -249.09387545053977
-- -126.6997017235669
-- -165.4853241880315
-- -183.1706321258363
-- -199.8006557550405
-- -142.0527901441929
-- -167.10657425912635
-- -72.13940725945159
-- -279.8488971483063
-- -190.76855531083888
-- -132.5052445914984
-- -186.31908792026428
-- -209.89053502992988
-- -194.54755649800435
-- -153.0563005077373
-- -175.31810037916114
-- -41.63516928736321
-- -289.6292074142173
-- -92.88580865593525
-- -201.57744071303082
-- -78.38496389675379
-- -102.37369818911844
-- -74.6941585482721
-- -237.56831160628943
-- -83.0022006028396
-- -242.9428963930302
-- -85.03874434022363
-- -105.36384492482497
-- -28.85214575091783
-- -121.28597754698242
-- -112.98964492193501
-- -2.9704925482979343
-- -119.57960415938308
-- -54.39173259806981
-- -60.4551546110456
-- -185.4739817360681
-- -164.71170765702635
-- -149.98905385415978
-- -211.76348838481712
-- -154.637051920341
-- -91.12614465455991
-- -95.57479207224236
-- -101.00267532749719
-- -111.93048321131528
-- -107.33955687007294
-- -52.93771532559152
-- -209.68143299171118
-- -64.20048990968746
-- -135.17922853492266
-- 42.619874283916594
-- -158.7504767982497
-- -139.61399692578732
-- -81.7830212971333
-- -93.49980450264374
-- -84.55991328286864
-- -183.16373315868958
-- -92.29208141458975
-- -33.488686584469505
-- -134.74813619936114
-- -99.31665471393255
-- -19.88126490813449
-- -16.409316431238704
-- -51.283508604150256
-- 22.053827055762667
-- 40.94648542458586
-- 4.900737059650758
-- -187.48534202576323
-- 34.45397696624966
-- -147.99163815898328
-- -60.54858213778748
-- -60.44634044362913
-- -115.13511819292981
-- -77.03381078997329
-- -39.63800983797401
-- -63.16467555367657
-- -96.24228971822636
-- -112.2598000100672
-- -232.1311472797242
-- -64.84084437939381
-- -112.0269519592484
-- -185.77267272757916
-- -80.48193848533441
-- -52.301216673442084
-- -52.03501539130916
-- -74.83519492876805
-- -170.03463198683247
-z:
-- -14.76633010405184
-- -77.08097113274366
-- -105.94106972116744
-- -168.2090926517627
-- -65.26551043544558
-- -20.6880089422812
-- -209.32696148448377
-- -24.40713791495554
-- -15.75365922487368
-- -254.89155753037986
-- -152.5052404899702
-- -1.1269359922330804
-- -116.56557358745104
-- -13.061463181014744
-- -271.5965621707225
-- -152.31276368325067
-- -92.2902567904771
-- -141.07917492886594
-- 21.69011355459171
-- -183.34206401478107
-- 10.140801536451853
-- -35.95343467815701
-- -26.211057572707027
-- -112.78898766720883
-- -133.29063998526433
-- -61.18238284950703
-- -80.06503202029097
-- 12.422039484044944
-- -5.12101512804493
-- -73.39808051629576
-- -110.4416005782697
-- -111.83179889807782
-- -179.2128322791779
-- -79.85810810889649
-- -126.23405124577191
-- -96.50900697911743
-- -18.125097261698677
-- -36.57092491856446
-- 51.943730895861094
-- -157.448224578843
-- -53.56098577197906
-- 36.21099531070003
-- 30.264159110500028
-- -128.81850270881264
-- -81.23129461063239
-- -16.456676449321943
-- -86.63729199837879
-- 24.60007469359832
-- -6.6448094065609515
-- -60.979163653950344
-- 48.874877564359494
-- 17.360966984906003
-- -23.29246474410196
-- -29.828427844083933
-- -33.31738239206807
-- -122.86436652286201
-- -111.42238758800114
-- 56.19728485994878
-- 115.70687319588221
-- -83.61672979309319
-- -120.59739183438491
-- -115.46930927809868
-- -139.47286172515123
-- -110.90601146953438
-- -121.22900189214741
-- -43.967041616093915
-- -63.48893808876695
-- -89.77046020404639
-- -102.48135990357295
-- -3.941185563845633
-- -48.45091405021118
-- -45.43625821365923
-- -69.85638375546341
-- -103.33442348332125
-- -23.02226952758559
-- -223.90372628887343
-- -103.82545014924584
-- -31.392814108062087
-- -99.22982433555555
-- -122.95666748923703
-- -31.6125389866287
-- -214.46597305215195
-- -33.16077540127025
-- 24.654472285743843
-- -126.06881773113969
-- -67.15036556927743
-- -103.51112759867529
-- -112.49273446342748
-- -30.58986293113707
-- -163.59145625259794
-- -134.38496336676516
-- -20.866372315040756
-- 65.51547950531781
-- -130.19304612277347
-- -64.13112509975545
-- 17.532331154672615
-- -152.28636639791674
-- -55.41347054315299
-- 10.891619057837156
-- -96.59563281422255
-- -48.856284332006204
-- 19.128375666118075
-- -100.39353753428146
-- -62.00837479965283
-- -97.80252980411373
-- -172.83316495582778
-- -145.1309805338278
-- -98.20698079942892
-- -190.75540032222162
-- -185.38968463427997
-- -139.3738320774594
-- -9.253472269775633
-- -139.07770738320693
-- -46.58774101188062
-- -67.47683277359685
-- -46.000235484769
-- -128.71964463728597
-- -170.5977947504874
-- -171.99109962011153
-- -211.3584965405287
-- -80.57163032129169
-- -59.78250802996865
-- -62.23533058218616
-- -117.40181697835742
-- -241.97887047936482
-- -142.29890186792912
-- -28.72563853627365
-- -36.10084290447162
-- -81.1411882563499
-- -153.2271564287201
-- -21.680286042756165
-- -169.83235917233586
-- -81.7474230057096
-- 24.328386528281765
-- -155.73154838932504
-- -123.70063663965863
-- -217.83196054157236
-- -48.865261345157315
-- -64.46898611738675
-- -27.99725847937989
-- -150.85674510009306
-- -275.2284467608632
-- -54.48520332289754
-- -169.19356035925688
-- -71.20531706370637
-- -148.68804594177016
-- -81.77316440334143
-- -169.1920506442319
-- -108.23662676360165
-- -207.56861909360228
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
deleted file mode 100644
index 19a5cff5a..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2100iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -205.5073692403897
-- -333.8446422195872
-- -240.82848008347725
-- -34.622646511974
-- -231.33240480034596
-- -85.15589672049516
-- -197.97259670710187
-- -96.13649621053189
-- -117.25003949936774
-- -265.745212141466
-- -165.66798157239478
-- -67.0350020400283
-- -165.6821548611103
-- -157.30589525965246
-- -67.42267898495412
-- -106.78925116367964
-- -231.98113815964334
-- -186.94504476809632
-- -80.1500018270306
-- -243.5120854871698
-- -201.97475895474537
-- -117.3697995059603
-- -314.71066180673483
-- -172.4539994573838
-- -121.71782922037677
-- -271.2296926982156
-- -149.53285959062183
-- -72.32153197984707
-- -127.63059956232463
-- -243.22740742519383
-- -280.3560979666143
-- -135.13571134484133
-- -297.1012270733985
-- -193.27899031735822
-- -213.4462636898764
-- 42.10367780595005
-- -32.122599213996295
-- -380.69262010862843
-- -200.5824703573119
-- -301.90687604870493
-- -161.08961691428095
-- -161.0344471628078
-- -110.00772679029647
-- -228.99670042626514
-- -207.5491966101806
-- -186.80013895821764
-- -387.589252741825
-- -225.66502306034883
-- -83.31435689593634
-- -218.66335734305827
-- -69.25486177770466
-- -105.4457913281818
-- -223.41387263225707
-- -85.45899984407099
-- -95.90034310580366
-- -32.63396447619502
-- -109.12913821181753
-- -141.64720436748453
-- -227.12572391663437
-- -66.33070888759842
-- -230.11521592700862
-- -86.86377324570762
-- -224.71639415324486
-- -102.77590262940494
-- -273.64514899235655
-- -127.23005150607317
-- -209.51848772760508
-- -163.6540354561236
-- -247.55780106506907
-- -179.36027234893194
-- -140.49840958849435
-- -132.17841652905238
-- -418.40899382971077
-- -206.7592009101982
-- -214.24443972554084
-- -201.04947109656618
-- -243.868201376032
-- -215.0975985449305
-- -139.17310216223078
-- -234.36410147342414
-- -29.96159604631133
-- -288.22566714032934
-- -176.4287024356889
-- -192.76384105737392
-- -14.631224403025561
-- -53.805996201129986
-- 44.83703287605151
-- -202.49438461174935
-- -129.37833935516377
-- -250.081667630867
-- -122.89175588898127
-- -194.1740790035445
-- -59.98724901084508
-- -139.2262557594861
-- -177.03448844433083
-- -50.669669531058986
-- -183.80272176628054
-- -28.837170523677816
-- -49.162610117278746
-- -181.3411409109815
-- -168.89529428518708
-- -138.03375359420133
-- -199.49155690007035
-- -205.62261337454152
-- -136.40088062435441
-- -88.088204348715
-- -57.10571256735283
-- -65.98440925687532
-- -131.2768328086759
-- -62.00050974039761
-- -238.72599963259066
-- -163.19014187616423
-- -119.54986616157132
-- -5.142747722953934
-- -145.7991459659883
-- -193.01963318383585
-- -43.09932930022754
-- -153.77073722641566
-- -57.395317195035936
-- -178.41875877799393
-- -76.69822796371398
-- -20.398463084317548
-- -126.71888439794202
-- -110.05448964238323
-- -8.083631023659926
-- -87.75469300588043
-- -89.17418101790668
-- -3.0415198841386974
-- 19.66450814303073
-- 89.06742995211957
-- -226.98416387334797
-- 56.38445627805352
-- -90.97952413487529
-- -4.854519176660294
-- -94.72569691060215
-- -151.39086769029157
-- -87.57889059581153
-- 24.53879952610265
-- 25.100432078623555
-- -85.44774617358404
-- -53.161816966462084
-- -238.04823141630519
-- -50.98954520951396
-- -95.77405665909292
-- -172.29037747270138
-- -117.37996263003917
-- -23.16330476817926
-- -70.40785375242959
-- -65.3325153105929
-- -207.12566352810137
-y:
-- -205.5073692403897
-- -333.8446422195872
-- -240.82848008347725
-- -34.622646511974
-- -231.33240480034596
-- -85.15589672049516
-- -197.97259670710187
-- -96.13649621053189
-- -117.25003949936774
-- -265.745212141466
-- -165.66798157239478
-- -67.0350020400283
-- -165.6821548611103
-- -157.30589525965246
-- -67.42267898495412
-- -106.78925116367964
-- -231.98113815964334
-- -186.94504476809632
-- -80.1500018270306
-- -243.5120854871698
-- -201.97475895474537
-- -117.3697995059603
-- -314.71066180673483
-- -172.4539994573838
-- -121.71782922037677
-- -271.2296926982156
-- -149.53285959062183
-- -72.32153197984707
-- -127.63059956232463
-- -243.22740742519383
-- -280.3560979666143
-- -135.13571134484133
-- -297.1012270733985
-- -193.27899031735822
-- -213.4462636898764
-- 42.10367780595005
-- -32.122599213996295
-- -380.69262010862843
-- -200.5824703573119
-- -301.90687604870493
-- -161.08961691428095
-- -161.0344471628078
-- -110.00772679029647
-- -228.99670042626514
-- -207.5491966101806
-- -186.80013895821764
-- -387.589252741825
-- -225.66502306034883
-- -83.31435689593634
-- -218.66335734305827
-- -69.25486177770466
-- -105.4457913281818
-- -223.41387263225707
-- -85.45899984407099
-- -95.90034310580366
-- -32.63396447619502
-- -109.12913821181753
-- -141.64720436748453
-- -227.12572391663437
-- -66.33070888759842
-- -230.11521592700862
-- -86.86377324570762
-- -224.71639415324486
-- -102.77590262940494
-- -273.64514899235655
-- -127.23005150607317
-- -209.51848772760508
-- -163.6540354561236
-- -247.55780106506907
-- -179.36027234893194
-- -140.49840958849435
-- -132.17841652905238
-- -418.40899382971077
-- -206.7592009101982
-- -214.24443972554084
-- -201.04947109656618
-- -243.868201376032
-- -215.0975985449305
-- -139.17310216223078
-- -234.36410147342414
-- -29.96159604631133
-- -288.22566714032934
-- -176.4287024356889
-- -192.76384105737392
-- -14.631224403025561
-- -53.805996201129986
-- 44.83703287605151
-- -202.49438461174935
-- -129.37833935516377
-- -250.081667630867
-- -122.89175588898127
-- -194.1740790035445
-- -59.98724901084508
-- -139.2262557594861
-- -177.03448844433083
-- -50.669669531058986
-- -183.80272176628054
-- -28.837170523677816
-- -49.162610117278746
-- -181.3411409109815
-- -168.89529428518708
-- -138.03375359420133
-- -199.49155690007035
-- -205.62261337454152
-- -136.40088062435441
-- -88.088204348715
-- -57.10571256735283
-- -65.98440925687532
-- -131.2768328086759
-- -62.00050974039761
-- -238.72599963259066
-- -163.19014187616423
-- -119.54986616157132
-- -5.142747722953934
-- -145.7991459659883
-- -193.01963318383585
-- -43.09932930022754
-- -153.77073722641566
-- -57.395317195035936
-- -178.41875877799393
-- -76.69822796371398
-- -20.398463084317548
-- -126.71888439794202
-- -110.05448964238323
-- -8.083631023659926
-- -87.75469300588043
-- -89.17418101790668
-- -3.0415198841386974
-- 19.66450814303073
-- 89.06742995211957
-- -226.98416387334797
-- 56.38445627805352
-- -90.97952413487529
-- -4.854519176660294
-- -94.72569691060215
-- -151.39086769029157
-- -87.57889059581153
-- 24.53879952610265
-- 25.100432078623555
-- -85.44774617358404
-- -53.161816966462084
-- -238.04823141630519
-- -50.98954520951396
-- -95.77405665909292
-- -172.29037747270138
-- -117.37996263003917
-- -23.16330476817926
-- -70.40785375242959
-- -65.3325153105929
-- -207.12566352810137
-z:
-- 44.54967297927881
-- -117.16757118661646
-- -68.33755789455105
-- -138.74005721401338
-- 34.58285849524722
-- 15.330254703611928
-- -114.05530604971351
-- -47.60804643043136
-- 19.271767405679974
-- -256.8454546028996
-- -157.19920585852938
-- 104.15974226892733
-- -104.65170305868624
-- 61.493441947002204
-- -291.7568700944766
-- -159.40968476667373
-- -56.03453672222339
-- -63.55137320513116
-- 50.822290127165665
-- -175.0549242860695
-- 40.85264895848032
-- -41.0224334277619
-- -24.534338456391033
-- -110.377582980965
-- -105.47069495224905
-- -94.71463256051653
-- -34.156812190008395
-- 46.61081171776186
-- 53.89730961060682
-- -147.50050733652608
-- -99.60710327037035
-- -141.62986356696658
-- -75.46486054768573
-- -86.83438468006057
-- -175.76748346837178
-- -53.66361281353443
-- 76.2349046407901
-- 42.89867715681811
-- 44.36087324888505
-- -151.31488346461322
-- 11.001424898658124
-- -42.033029630440815
-- 34.201178146930665
-- -119.7652257231709
-- -114.09731803946741
-- -43.453364804491706
-- -89.35198575438656
-- 65.32418853178122
-- 18.053772029339843
-- -128.16621400153113
-- 10.818175764679484
-- 20.613799151053524
-- 56.03312143173241
-- 32.480263524770805
-- -90.97817406112694
-- -206.39409072735708
-- -82.34300404109355
-- 146.6328223094321
-- 109.44929187541884
-- -195.38953562541084
-- -119.78042498811313
-- -162.42497560793868
-- -141.46675576366178
-- -222.9598593555579
-- -219.3985470081475
-- 8.371432339208495
-- -27.515931464708974
-- -66.8034902186602
-- -147.90505169357098
-- 9.04071412975171
-- -102.34268473601138
-- -9.010132318158092
-- -101.88906053637167
-- -213.7318632222936
-- -40.37989777188767
-- -229.46131818547656
-- -115.73436418036627
-- -112.09995046482273
-- -133.85742307890467
-- -170.61867168059226
-- -114.13383761396045
-- -276.03491377429583
-- -53.72887029097178
-- -3.0950423485502823
-- -225.02704377362778
-- -29.942434206076715
-- -142.10677112799746
-- -144.5980482004309
-- -78.30768393814688
-- -163.39440966340845
-- -168.84461183155804
-- -19.517681806181628
-- -20.355612948011064
-- -139.50687114077869
-- -34.00713601611943
-- 72.90963935350096
-- -149.97649850251122
-- -13.058609028247039
-- -5.112707222930133
-- -82.83757405410596
-- -44.62838684114432
-- -16.376617136832387
-- -129.74816320140673
-- -25.679562224728357
-- -127.47448603673031
-- -170.8167299619752
-- -132.9305810037043
-- -89.62982210011754
-- -262.3178944687407
-- -167.29115281640043
-- -205.44334214206472
-- -30.46881931069258
-- -140.0084272382212
-- -41.43019684893294
-- -29.382703424136345
-- -88.73459328600485
-- -195.60376665146785
-- -104.52072341663803
-- -123.57116121667725
-- -206.44903101292434
-- -42.518860984416555
-- -32.839369965735386
-- -133.79440841705545
-- -76.985328068373
-- -252.76459902899148
-- -150.44906624211123
-- -87.0292319728042
-- 80.414598235004
-- -46.457279898543376
-- -135.1024382257102
-- -72.86667366907751
-- -119.61553450500578
-- -42.07280456612249
-- 17.36961952148788
-- -135.28675415983665
-- -91.28263410661891
-- -178.57408057229455
-- -104.37552702097844
-- -66.67739184951493
-- -32.0515952864862
-- -167.64973060057116
-- -271.996689188636
-- -37.09870662152558
-- -154.41410030569045
-- -121.17914986100394
-- -169.10121709846476
-- -87.52442642504427
-- -144.308647591676
-- -170.29484389514315
-- -162.37157500544316
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
deleted file mode 100644
index d56bdec49..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_2900iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -272.43926763181287
-- -448.95097433506004
-- -236.16535004240544
-- -37.76856392971942
-- -180.658962619701
-- -109.20163171085781
-- -199.945554996803
-- -68.90194452971632
-- -135.30613178737158
-- -223.60190482205596
-- -190.55699992587364
-- -130.6451273673297
-- -200.68440674896308
-- -262.2682066040087
-- -47.7449269217694
-- -95.04431831226468
-- -145.35864396914786
-- -200.6743090884891
-- 3.7251780830580787
-- -294.93243387150204
-- -313.69099949174336
-- -99.11066833399032
-- -313.3951354508127
-- -205.5559672495546
-- -139.1387047489721
-- -275.63217701758543
-- -107.07211702595991
-- -5.9469051443245045
-- -163.72650355207833
-- -270.99411753819425
-- -277.4419276241587
-- -18.788685401667642
-- -253.62296756705157
-- -142.5147933674709
-- -187.72562194153429
-- 36.43638736764589
-- 34.48678419943111
-- -289.83782982823584
-- -176.40183289325586
-- -351.33906087051105
-- -121.36788858125071
-- -174.24380692956095
-- -98.62036069690163
-- -240.9168724677785
-- -231.4324677236045
-- -206.6613718247246
-- -406.4727718202573
-- -225.1610009072952
-- 29.49527751562229
-- -273.65646034143714
-- -96.59222344210355
-- -162.23353009771722
-- -285.8236665686258
-- -199.74511755862125
-- -215.07805281823997
-- -36.18075902907856
-- -191.31623789060947
-- -101.6375852133703
-- -243.13184112407623
-- -19.2220979651675
-- -308.0664791104669
-- -83.4234720411692
-- -221.7978116147051
-- -54.60773482968091
-- -298.4940871611296
-- -44.74363435246878
-- -333.532488561383
-- -217.03368408170132
-- -264.2771572798883
-- -154.1746872913021
-- -197.51980936266472
-- -158.70581789257093
-- -335.9275171564795
-- -222.87545271783046
-- -157.3683247757442
-- -233.7513146482435
-- -296.57684315928617
-- -219.48252748393207
-- -48.133547220421235
-- -155.26662426992243
-- -36.10288902977462
-- -194.83693975598814
-- -241.6387007049145
-- -242.48913974641644
-- 36.02518983336963
-- -0.1576404832974825
-- 113.58065834324691
-- -271.2810951422604
-- -174.88764583856945
-- -124.37969764150394
-- -84.85385273337539
-- -181.70365569998665
-- -73.74984082720982
-- -129.10692313606563
-- -134.64557613856581
-- 30.91736025594859
-- -167.3711207815298
-- -49.8615388491973
-- -4.6387999504108945
-- -215.2502217817841
-- -156.89246466465733
-- -108.41620488140606
-- -183.8464655986025
-- -264.7692849942317
-- -97.35901020025415
-- -162.16983307806206
-- -52.2525646803888
-- 17.23526023845099
-- -94.96670981662531
-- -55.02980165867298
-- -257.3030900525307
-- -110.99333526558722
-- -83.18280817502327
-- 43.3175391799086
-- -252.90093098239393
-- -194.0767310127264
-- -90.02653456048056
-- -161.17870613607627
-- -66.10552713628458
-- -184.4202808248762
-- -8.88045546106721
-- -18.59324777165611
-- -56.03783797870011
-- -53.2062297088678
-- 41.482966759745146
-- -65.49093472082363
-- -49.90731228303498
-- 36.708761810916734
-- -3.7616971171956592
-- 96.68901411086348
-- -169.96599244788874
-- 11.53652344408845
-- -107.20161855221879
-- -33.57451345086991
-- -24.044150838980794
-- -132.7895303438664
-- -29.23104392019398
-- 14.998460109321712
-- 49.94060138120499
-- -78.2161582237574
-- -104.2790849906903
-- -313.4798845657733
-- -6.362771935071262
-- -38.65859508262323
-- -149.16338626268612
-- -118.58877401216215
-- -8.565606447612362
-- -185.41328952312594
-- -26.70780831753186
-- -158.37188125783825
-y:
-- -272.43926763181287
-- -448.95097433506004
-- -236.16535004240544
-- -37.76856392971942
-- -180.658962619701
-- -109.20163171085781
-- -199.945554996803
-- -68.90194452971632
-- -135.30613178737158
-- -223.60190482205596
-- -190.55699992587364
-- -130.6451273673297
-- -200.68440674896308
-- -262.2682066040087
-- -47.7449269217694
-- -95.04431831226468
-- -145.35864396914786
-- -200.6743090884891
-- 3.7251780830580787
-- -294.93243387150204
-- -313.69099949174336
-- -99.11066833399032
-- -313.3951354508127
-- -205.5559672495546
-- -139.1387047489721
-- -275.63217701758543
-- -107.07211702595991
-- -5.9469051443245045
-- -163.72650355207833
-- -270.99411753819425
-- -277.4419276241587
-- -18.788685401667642
-- -253.62296756705157
-- -142.5147933674709
-- -187.72562194153429
-- 36.43638736764589
-- 34.48678419943111
-- -289.83782982823584
-- -176.40183289325586
-- -351.33906087051105
-- -121.36788858125071
-- -174.24380692956095
-- -98.62036069690163
-- -240.9168724677785
-- -231.4324677236045
-- -206.6613718247246
-- -406.4727718202573
-- -225.1610009072952
-- 29.49527751562229
-- -273.65646034143714
-- -96.59222344210355
-- -162.23353009771722
-- -285.8236665686258
-- -199.74511755862125
-- -215.07805281823997
-- -36.18075902907856
-- -191.31623789060947
-- -101.6375852133703
-- -243.13184112407623
-- -19.2220979651675
-- -308.0664791104669
-- -83.4234720411692
-- -221.7978116147051
-- -54.60773482968091
-- -298.4940871611296
-- -44.74363435246878
-- -333.532488561383
-- -217.03368408170132
-- -264.2771572798883
-- -154.1746872913021
-- -197.51980936266472
-- -158.70581789257093
-- -335.9275171564795
-- -222.87545271783046
-- -157.3683247757442
-- -233.7513146482435
-- -296.57684315928617
-- -219.48252748393207
-- -48.133547220421235
-- -155.26662426992243
-- -36.10288902977462
-- -194.83693975598814
-- -241.6387007049145
-- -242.48913974641644
-- 36.02518983336963
-- -0.1576404832974825
-- 113.58065834324691
-- -271.2810951422604
-- -174.88764583856945
-- -124.37969764150394
-- -84.85385273337539
-- -181.70365569998665
-- -73.74984082720982
-- -129.10692313606563
-- -134.64557613856581
-- 30.91736025594859
-- -167.3711207815298
-- -49.8615388491973
-- -4.6387999504108945
-- -215.2502217817841
-- -156.89246466465733
-- -108.41620488140606
-- -183.8464655986025
-- -264.7692849942317
-- -97.35901020025415
-- -162.16983307806206
-- -52.2525646803888
-- 17.23526023845099
-- -94.96670981662531
-- -55.02980165867298
-- -257.3030900525307
-- -110.99333526558722
-- -83.18280817502327
-- 43.3175391799086
-- -252.90093098239393
-- -194.0767310127264
-- -90.02653456048056
-- -161.17870613607627
-- -66.10552713628458
-- -184.4202808248762
-- -8.88045546106721
-- -18.59324777165611
-- -56.03783797870011
-- -53.2062297088678
-- 41.482966759745146
-- -65.49093472082363
-- -49.90731228303498
-- 36.708761810916734
-- -3.7616971171956592
-- 96.68901411086348
-- -169.96599244788874
-- 11.53652344408845
-- -107.20161855221879
-- -33.57451345086991
-- -24.044150838980794
-- -132.7895303438664
-- -29.23104392019398
-- 14.998460109321712
-- 49.94060138120499
-- -78.2161582237574
-- -104.2790849906903
-- -313.4798845657733
-- -6.362771935071262
-- -38.65859508262323
-- -149.16338626268612
-- -118.58877401216215
-- -8.565606447612362
-- -185.41328952312594
-- -26.70780831753186
-- -158.37188125783825
-z:
-- 124.57747204454569
-- -121.94731283243988
-- -126.56271330646996
-- -112.52506529681581
-- 111.84006675738534
-- -10.239780687477998
-- -129.40171936532252
-- -71.93133365614328
-- 129.35372191916255
-- -283.9623901835431
-- -134.82889622818732
-- 155.40490774304192
-- -68.72362884968071
-- 14.641506473353738
-- -316.88396368414703
-- -166.2550474058331
-- -60.294336072837574
-- -124.66984347102944
-- 57.989467179887384
-- -213.96827177603473
-- 67.39398238079399
-- -16.49897735603517
-- 57.81813545001022
-- -151.0875822842148
-- -72.40843493966484
-- -86.13286036511306
-- 57.0274088708988
-- 123.813661903704
-- 70.81124080975727
-- -141.03475610168258
-- -20.6009597723412
-- -220.90071425002117
-- -7.155264997956961
-- -93.68018930666585
-- -86.33561755686196
-- -88.70235774148611
-- 38.600483227150185
-- -23.490281209885353
-- 100.17141885122756
-- -117.51420568382622
-- 61.27652531381027
-- -91.31899669642479
-- 79.99658412138454
-- -155.73416936908632
-- -192.04143425045172
-- 43.506173590381586
-- -68.85001006111823
-- 29.3336986485646
-- -51.37955584463
-- -156.31200332582014
-- -44.36632869522208
-- 37.371284814496
-- -19.038473678991394
-- 119.03995979661157
-- -138.3768289372469
-- -232.00562414557456
-- -142.37593295321378
-- 176.91568433929385
-- -10.932853233306853
-- -143.59868184934047
-- -204.4487164006846
-- -151.05438910443263
-- -222.4148720716179
-- -248.81173690786622
-- -384.9068023836999
-- -82.01848217627835
-- -20.57408724153294
-- -117.97140666411855
-- -195.48809940928075
-- -139.90701538379025
-- -83.34090458005305
-- -42.79122366878542
-- -109.30063060111074
-- -165.32982134003694
-- -65.80340068091773
-- -239.48836465386094
-- -158.6173832006208
-- -172.20378206597073
-- -203.64645596162404
-- -229.7719051374468
-- -122.47150344241452
-- -213.2823807404075
-- -87.10066114492069
-- -13.167198095349415
-- -243.93714190837866
-- -28.453369455803983
-- -188.7196445212271
-- -105.41711481724624
-- -108.1354006318777
-- -56.242518098961604
-- -257.6777248502073
-- 58.84899341475319
-- -27.42180667742504
-- -157.38936980766042
-- -47.344595672173625
-- 74.97224991724782
-- -120.52346040193677
-- -79.7146187030521
-- -40.82187666019393
-- -190.56672793182375
-- -40.006537212489285
-- -93.30526465734067
-- -134.78802918564884
-- 31.923504674280327
-- -87.29057209262477
-- -196.03259697679803
-- -98.4343674791983
-- -126.94109522819964
-- -349.76581839417526
-- -176.07681326431566
-- -224.66008538510738
-- 29.933741187412725
-- -123.95944303183958
-- -20.720566803283322
-- 90.38755013043577
-- -46.52055130423836
-- -190.36554085387718
-- -37.275677534262485
-- -108.5631784099128
-- -259.8011713719343
-- -95.08005177883054
-- -86.75138906952814
-- -122.0460091387302
-- -49.062248288068865
-- -229.7146564892436
-- -201.92998766642376
-- -108.76032605399638
-- 85.28221084750679
-- -11.81228223985709
-- -185.78475524974968
-- -20.90891174074232
-- -74.86710648006371
-- -128.38830281127127
-- 131.395680993159
-- -118.01473503047335
-- -64.05863974700276
-- -219.6977471095694
-- -19.38851411650937
-- -47.82382740185165
-- 67.6280800778232
-- -208.00733502584072
-- -263.12702086481744
-- -52.01001338587882
-- -152.6557062149739
-- -95.56890304553689
-- -200.29100674666495
-- -47.512498615468914
-- -103.85651781817371
-- -131.53030224442517
-- -122.21710160107816
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
deleted file mode 100644
index 9eadff47c..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_3700iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -318.33839265125806
-- -503.8732576178618
-- -177.77775964139093
-- -6.7492803260761995
-- -242.03813817935685
-- -77.71979481050029
-- -220.73421600647907
-- -90.27435824161067
-- -153.0275889294634
-- -200.48419383845444
-- -156.471884436833
-- -103.21870798204141
-- -222.84791678035685
-- -303.96349284278716
-- -76.73068739780327
-- -31.624809568016307
-- -153.82357481298533
-- -234.22262159091488
-- 86.72169514942202
-- -256.36840286665006
-- -309.9685665351012
-- -63.1932884859172
-- -322.6519017052047
-- -243.47703866255534
-- -158.51341919021516
-- -275.6868839717208
-- -104.84781924329988
-- 55.86783354402407
-- -148.4564593378266
-- -295.1219553718082
-- -276.5514759444907
-- -9.19806696670726
-- -226.3586168241507
-- -135.77779938569918
-- -124.7290399124978
-- 40.97147405813215
-- 47.9572155566829
-- -209.71160766724336
-- -206.8022387130818
-- -346.19115925055246
-- -194.68448470169963
-- -232.38547362052498
-- -46.81884319670084
-- -284.83539234521766
-- -291.55024983848756
-- -215.3595844343681
-- -469.2967940360148
-- -230.07806535442114
-- 15.756084988812708
-- -183.70819458902477
-- -115.7163594887839
-- -215.5334693249218
-- -280.6752838173983
-- -221.57247204002815
-- -300.5664169621706
-- -79.32812332194474
-- -179.40836211563925
-- -168.79969478638898
-- -276.29289348584774
-- -46.37362441829391
-- -231.29505409608572
-- -70.18085780324427
-- -252.23939541471805
-- -19.12968984192443
-- -296.13422955076334
-- -100.16294002462993
-- -342.90714896056124
-- -215.61769241860063
-- -296.75155474429926
-- -152.6591211649526
-- -188.693765231233
-- -205.91018839738655
-- -416.03035939248605
-- -199.40149048359942
-- -126.83154515228814
-- -222.1926200930844
-- -309.4416518904729
-- -254.09193980366175
-- -0.5418580888411685
-- -155.89526333672188
-- -9.890265411787345
-- -193.7291905240682
-- -263.22603077790814
-- -290.4470899118978
-- 42.80107774241674
-- 16.546189863000382
-- 87.11142726739735
-- -266.371727169003
-- -167.52530632906425
-- -194.22729876901454
-- -119.62496574880039
-- -251.5452385054409
-- -85.58574830889465
-- -175.86565293488013
-- -168.68157901172373
-- 43.27471001227153
-- -211.51235301490655
-- -33.882881166671396
-- 17.816896055177363
-- -220.08370985785794
-- -126.94814249553465
-- -123.36080588341422
-- -191.66925871785097
-- -217.41619524997745
-- -82.53148473527165
-- -119.01021633447931
-- -18.880832531609574
-- 94.810132809961
-- -126.10340115041222
-- -65.41712024089234
-- -242.27824026663114
-- -127.62421001785404
-- -109.29264926087524
-- 105.00869338854466
-- -264.95747004603106
-- -170.19316659517534
-- -62.51174523604564
-- -139.03783023029405
-- 7.091799579847809
-- -100.1137463217048
-- 40.52331999512637
-- -0.3977207066748197
-- -6.042863622712464
-- -10.462392479298
-- 82.40140712057627
-- -52.05978955014146
-- -59.374221005472805
-- 8.062674595752625
-- 48.37570132908054
-- 125.45986768578996
-- -167.0755789114162
-- 78.7851699212989
-- -135.53045791212642
-- -58.05486635241099
-- -56.274920510975655
-- -140.67880641684334
-- 8.52274740872368
-- 36.394675838985634
-- 74.04058190042416
-- -40.43735106510951
-- -97.63861742488488
-- -228.5431282058966
-- 25.72368610182627
-- 21.425417388147995
-- -109.71892342624676
-- -72.98589345917944
-- -33.15774660044493
-- -233.72394361694236
-- 10.062230522679108
-- -127.33721300361697
-y:
-- -318.33839265125806
-- -503.8732576178618
-- -177.77775964139093
-- -6.7492803260761995
-- -242.03813817935685
-- -77.71979481050029
-- -220.73421600647907
-- -90.27435824161067
-- -153.0275889294634
-- -200.48419383845444
-- -156.471884436833
-- -103.21870798204141
-- -222.84791678035685
-- -303.96349284278716
-- -76.73068739780327
-- -31.624809568016307
-- -153.82357481298533
-- -234.22262159091488
-- 86.72169514942202
-- -256.36840286665006
-- -309.9685665351012
-- -63.1932884859172
-- -322.6519017052047
-- -243.47703866255534
-- -158.51341919021516
-- -275.6868839717208
-- -104.84781924329988
-- 55.86783354402407
-- -148.4564593378266
-- -295.1219553718082
-- -276.5514759444907
-- -9.19806696670726
-- -226.3586168241507
-- -135.77779938569918
-- -124.7290399124978
-- 40.97147405813215
-- 47.9572155566829
-- -209.71160766724336
-- -206.8022387130818
-- -346.19115925055246
-- -194.68448470169963
-- -232.38547362052498
-- -46.81884319670084
-- -284.83539234521766
-- -291.55024983848756
-- -215.3595844343681
-- -469.2967940360148
-- -230.07806535442114
-- 15.756084988812708
-- -183.70819458902477
-- -115.7163594887839
-- -215.5334693249218
-- -280.6752838173983
-- -221.57247204002815
-- -300.5664169621706
-- -79.32812332194474
-- -179.40836211563925
-- -168.79969478638898
-- -276.29289348584774
-- -46.37362441829391
-- -231.29505409608572
-- -70.18085780324427
-- -252.23939541471805
-- -19.12968984192443
-- -296.13422955076334
-- -100.16294002462993
-- -342.90714896056124
-- -215.61769241860063
-- -296.75155474429926
-- -152.6591211649526
-- -188.693765231233
-- -205.91018839738655
-- -416.03035939248605
-- -199.40149048359942
-- -126.83154515228814
-- -222.1926200930844
-- -309.4416518904729
-- -254.09193980366175
-- -0.5418580888411685
-- -155.89526333672188
-- -9.890265411787345
-- -193.7291905240682
-- -263.22603077790814
-- -290.4470899118978
-- 42.80107774241674
-- 16.546189863000382
-- 87.11142726739735
-- -266.371727169003
-- -167.52530632906425
-- -194.22729876901454
-- -119.62496574880039
-- -251.5452385054409
-- -85.58574830889465
-- -175.86565293488013
-- -168.68157901172373
-- 43.27471001227153
-- -211.51235301490655
-- -33.882881166671396
-- 17.816896055177363
-- -220.08370985785794
-- -126.94814249553465
-- -123.36080588341422
-- -191.66925871785097
-- -217.41619524997745
-- -82.53148473527165
-- -119.01021633447931
-- -18.880832531609574
-- 94.810132809961
-- -126.10340115041222
-- -65.41712024089234
-- -242.27824026663114
-- -127.62421001785404
-- -109.29264926087524
-- 105.00869338854466
-- -264.95747004603106
-- -170.19316659517534
-- -62.51174523604564
-- -139.03783023029405
-- 7.091799579847809
-- -100.1137463217048
-- 40.52331999512637
-- -0.3977207066748197
-- -6.042863622712464
-- -10.462392479298
-- 82.40140712057627
-- -52.05978955014146
-- -59.374221005472805
-- 8.062674595752625
-- 48.37570132908054
-- 125.45986768578996
-- -167.0755789114162
-- 78.7851699212989
-- -135.53045791212642
-- -58.05486635241099
-- -56.274920510975655
-- -140.67880641684334
-- 8.52274740872368
-- 36.394675838985634
-- 74.04058190042416
-- -40.43735106510951
-- -97.63861742488488
-- -228.5431282058966
-- 25.72368610182627
-- 21.425417388147995
-- -109.71892342624676
-- -72.98589345917944
-- -33.15774660044493
-- -233.72394361694236
-- 10.062230522679108
-- -127.33721300361697
-z:
-- 181.0863154483668
-- -31.17771450941217
-- -131.0439545278052
-- -86.90343977743969
-- 139.16394124520718
-- -37.32702121025883
-- -146.42539146983646
-- -62.26586514105592
-- 208.69645305438297
-- -251.368482905382
-- -186.97390109596176
-- 175.69142574945545
-- -107.32570635749657
-- 56.249565649821996
-- -311.4308506465853
-- -86.25609939656523
-- 12.339637683177527
-- -227.4292833033472
-- 42.942002678669795
-- -153.39218203343864
-- 119.12930254316275
-- -10.107340020103656
-- 79.62407823144244
-- -156.02869946276996
-- -26.206848777533665
-- -50.43475326943761
-- 85.94651594284883
-- 106.48464201450267
-- 100.2685013112651
-- -121.87667475466651
-- -86.38750249227874
-- -218.0532151541864
-- 15.37185934879268
-- -70.02864771017883
-- -101.09853560534076
-- -92.46275643231517
-- 46.297066297691934
-- -14.555285738533223
-- 129.5173471667389
-- -46.180364966683705
-- 65.4830928379543
-- -58.17953821547616
-- 84.28074434004336
-- -155.1641325587134
-- -180.3359495774464
-- 88.00479798175125
-- -17.850090482332654
-- 15.372150066234237
-- -27.647258581804355
-- -192.91010730104782
-- -81.20236428206617
-- 6.467237778465594
-- -78.40533655530155
-- 172.22679093564656
-- -228.36796902747824
-- -225.6689941838593
-- -206.28994187021976
-- 131.48524184946956
-- -57.81804179017013
-- -147.16545572823645
-- -240.67299674890594
-- -116.14786022256757
-- -201.58749895550676
-- -308.07009408010566
-- -381.51470627309277
-- -137.39140431935613
-- -10.63189954057172
-- -118.493935903444
-- -164.1140641970783
-- -160.18465262262697
-- -117.57657250910667
-- -43.836529664241546
-- -112.71822942406038
-- -145.30820262460696
-- -90.81939319176844
-- -311.7923659232393
-- -175.3190301125731
-- -163.64011238983196
-- -247.39627434657896
-- -281.6861325550744
-- -175.08297411333973
-- -211.5739326575852
-- -10.988979072514425
-- -63.21629300640135
-- -202.53635529860406
-- -79.6957224677569
-- -177.162143434471
-- -92.84307934741052
-- -49.25448884632998
-- -44.780500209675225
-- -306.68069860558774
-- 4.39395972183376
-- -16.597486223665378
-- -206.44476503921166
-- -57.53302317731632
-- 105.23684385084307
-- -90.6662697103156
-- -60.903732325556234
-- -89.5421941328788
-- -220.0909473496956
-- -80.48441922343073
-- -123.94102877280065
-- -82.24771338364724
-- 25.78095332212615
-- -18.137890561524372
-- -220.67832950606126
-- -80.3513128301996
-- -116.30708725243996
-- -376.2694198773608
-- -90.08644334520498
-- -226.72579589042797
-- -41.920975507066316
-- -123.01540567914972
-- -10.086676126013069
-- 146.20193939546007
-- -7.818257625137203
-- -220.36826843029272
-- -48.97386136378574
-- -100.66630784546683
-- -247.64603285451435
-- -13.685647568036465
-- -19.602285641253204
-- -156.06514791914643
-- -82.58806190097447
-- -250.156508003731
-- -179.87376837556948
-- -18.67009670602568
-- 118.57247270205394
-- 11.177827711965824
-- -200.67282682077555
-- 26.279149574743716
-- -49.32413067128099
-- -149.32913362268766
-- 117.21131129001682
-- -81.87955805358436
-- -147.37638486925798
-- -180.83020297576527
-- 33.182852456140495
-- -8.764771582780666
-- 29.41976078723741
-- -169.09783435506606
-- -243.5270112908626
-- -45.83266948178252
-- -76.83693008095817
-- -84.60599041816408
-- -143.657215226681
-- -161.8069368693742
-- -151.858761996727
-- -161.10172004637832
-- -135.60897240328893
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
deleted file mode 100644
index 18aeb4a9f..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_4500iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -307.0050762731142
-- -481.078226408732
-- -167.35765769210175
-- 3.2155857868734596
-- -271.1441857672259
-- -65.71942771314525
-- -298.20344582051285
-- -56.839033663601775
-- -108.79519844570966
-- -230.15275347697178
-- -116.11665259730907
-- -40.1020956889466
-- -232.259229024552
-- -310.3034303967311
-- -64.01604336364709
-- -6.871892053932016
-- -153.22202647516366
-- -231.04322544639018
-- 74.44877889082618
-- -284.214278295163
-- -361.9818510757614
-- -79.40574675282046
-- -364.6297374793298
-- -170.77036590217787
-- -97.41558710663041
-- -311.4691059677862
-- -56.73355477193985
-- -49.98251311430054
-- -67.40310998563878
-- -342.94082838674996
-- -309.2813674649258
-- -1.8255852913501187
-- -199.10081582866306
-- -107.45000108810967
-- -113.4256835187521
-- 52.922554582910614
-- 61.83451450204298
-- -210.3273655230436
-- -151.56655048276252
-- -371.60746274998536
-- -201.7018308944319
-- -240.17038850383202
-- -5.02027956779233
-- -312.0700042595691
-- -307.47880192647176
-- -207.10639493970586
-- -504.4596940836522
-- -178.0663511545844
-- 108.1741087313954
-- -230.75292320355743
-- -99.26999716801097
-- -379.1967698869467
-- -268.17451088412747
-- -180.49119043160024
-- -319.1171815112415
-- -123.57804031969677
-- -217.72584276080778
-- -169.0552370338435
-- -295.73093381122334
-- -6.1624140590478635
-- -288.5456167792755
-- 8.128368780179871
-- -273.4409600387709
-- -34.38967766878111
-- -296.19214044239845
-- -57.092905112259864
-- -453.772881518744
-- -268.29410838503196
-- -225.46523867454502
-- -156.1360371937783
-- -254.85169701862418
-- -213.79962660208486
-- -420.7133824901222
-- -153.41659041261036
-- -58.20115031531576
-- -174.32911654101838
-- -319.93632296786006
-- -277.2669761654481
-- 10.358418007309556
-- -228.58808121230973
-- -40.498796490170996
-- -183.78539945929293
-- -253.07966794632128
-- -267.91199415308455
-- 25.292479767968373
-- -40.55947065570325
-- 143.37142124642537
-- -307.9697948735147
-- -204.15879357109165
-- -196.19898274338985
-- -129.46743231083366
-- -183.52437037382262
-- -62.14512122772048
-- -162.5147403785735
-- -188.1042411180911
-- 57.69519127285521
-- -296.0368433425237
-- -35.76061843155276
-- -42.10739776259322
-- -185.2120503709206
-- -141.76104241334673
-- -92.19931773917446
-- -250.46694930472722
-- -258.90185824926914
-- -70.87022098637703
-- -80.03237743157227
-- 36.648975295721776
-- 141.91109829680673
-- -79.64190548182395
-- 16.837115098236232
-- -291.0429952038719
-- -121.18311505521861
-- -104.97029871240828
-- 114.87662910759532
-- -286.4749590409721
-- -139.17496546848045
-- -10.191441705467936
-- -91.36563686432785
-- 33.83164333840472
-- -103.6053919809268
-- 100.73893860726871
-- -4.228885833076896
-- 0.29518205436492817
-- -32.384367296407866
-- 138.5644133058073
-- -75.63515950163199
-- 10.364619026260018
-- 20.980765426247974
-- 38.57937515486196
-- 167.88603431318387
-- -222.7287631138857
-- 176.68860940434107
-- -173.79227333486338
-- -109.48595595747148
-- 19.34516917949695
-- -82.38344597164956
-- 39.61516695813081
-- 53.45483727835639
-- 52.8044146763662
-- -52.30604369572495
-- -80.70848404692364
-- -201.04026922813614
-- -2.6918820856202066
-- 17.067603376143943
-- -87.01859880298362
-- -51.461962229891306
-- -64.43473942423898
-- -275.7793739824045
-- 22.21096374603982
-- -193.56704806257392
-y:
-- -307.0050762731142
-- -481.078226408732
-- -167.35765769210175
-- 3.2155857868734596
-- -271.1441857672259
-- -65.71942771314525
-- -298.20344582051285
-- -56.839033663601775
-- -108.79519844570966
-- -230.15275347697178
-- -116.11665259730907
-- -40.1020956889466
-- -232.259229024552
-- -310.3034303967311
-- -64.01604336364709
-- -6.871892053932016
-- -153.22202647516366
-- -231.04322544639018
-- 74.44877889082618
-- -284.214278295163
-- -361.9818510757614
-- -79.40574675282046
-- -364.6297374793298
-- -170.77036590217787
-- -97.41558710663041
-- -311.4691059677862
-- -56.73355477193985
-- -49.98251311430054
-- -67.40310998563878
-- -342.94082838674996
-- -309.2813674649258
-- -1.8255852913501187
-- -199.10081582866306
-- -107.45000108810967
-- -113.4256835187521
-- 52.922554582910614
-- 61.83451450204298
-- -210.3273655230436
-- -151.56655048276252
-- -371.60746274998536
-- -201.7018308944319
-- -240.17038850383202
-- -5.02027956779233
-- -312.0700042595691
-- -307.47880192647176
-- -207.10639493970586
-- -504.4596940836522
-- -178.0663511545844
-- 108.1741087313954
-- -230.75292320355743
-- -99.26999716801097
-- -379.1967698869467
-- -268.17451088412747
-- -180.49119043160024
-- -319.1171815112415
-- -123.57804031969677
-- -217.72584276080778
-- -169.0552370338435
-- -295.73093381122334
-- -6.1624140590478635
-- -288.5456167792755
-- 8.128368780179871
-- -273.4409600387709
-- -34.38967766878111
-- -296.19214044239845
-- -57.092905112259864
-- -453.772881518744
-- -268.29410838503196
-- -225.46523867454502
-- -156.1360371937783
-- -254.85169701862418
-- -213.79962660208486
-- -420.7133824901222
-- -153.41659041261036
-- -58.20115031531576
-- -174.32911654101838
-- -319.93632296786006
-- -277.2669761654481
-- 10.358418007309556
-- -228.58808121230973
-- -40.498796490170996
-- -183.78539945929293
-- -253.07966794632128
-- -267.91199415308455
-- 25.292479767968373
-- -40.55947065570325
-- 143.37142124642537
-- -307.9697948735147
-- -204.15879357109165
-- -196.19898274338985
-- -129.46743231083366
-- -183.52437037382262
-- -62.14512122772048
-- -162.5147403785735
-- -188.1042411180911
-- 57.69519127285521
-- -296.0368433425237
-- -35.76061843155276
-- -42.10739776259322
-- -185.2120503709206
-- -141.76104241334673
-- -92.19931773917446
-- -250.46694930472722
-- -258.90185824926914
-- -70.87022098637703
-- -80.03237743157227
-- 36.648975295721776
-- 141.91109829680673
-- -79.64190548182395
-- 16.837115098236232
-- -291.0429952038719
-- -121.18311505521861
-- -104.97029871240828
-- 114.87662910759532
-- -286.4749590409721
-- -139.17496546848045
-- -10.191441705467936
-- -91.36563686432785
-- 33.83164333840472
-- -103.6053919809268
-- 100.73893860726871
-- -4.228885833076896
-- 0.29518205436492817
-- -32.384367296407866
-- 138.5644133058073
-- -75.63515950163199
-- 10.364619026260018
-- 20.980765426247974
-- 38.57937515486196
-- 167.88603431318387
-- -222.7287631138857
-- 176.68860940434107
-- -173.79227333486338
-- -109.48595595747148
-- 19.34516917949695
-- -82.38344597164956
-- 39.61516695813081
-- 53.45483727835639
-- 52.8044146763662
-- -52.30604369572495
-- -80.70848404692364
-- -201.04026922813614
-- -2.6918820856202066
-- 17.067603376143943
-- -87.01859880298362
-- -51.461962229891306
-- -64.43473942423898
-- -275.7793739824045
-- 22.21096374603982
-- -193.56704806257392
-z:
-- 254.52608087703956
-- -31.460089338187732
-- -192.58073184894693
-- -118.52116549001673
-- 210.1737596156136
-- 36.78739348943514
-- -157.9024815236414
-- -116.11308392329033
-- 287.7843644908441
-- -250.69566068504813
-- -162.2856828167312
-- 252.064773772356
-- -152.16425981798878
-- 132.7459879913642
-- -409.98377371714093
-- -154.08563400540893
-- 112.03451444957001
-- -185.6484590597303
-- 80.54736096333752
-- -95.1442097426821
-- 160.07801628924997
-- 69.17803357017898
-- 77.52491351187382
-- -236.0127743143916
-- 56.169164878231655
-- -42.52977141410982
-- 123.84846531219658
-- 117.34049688631372
-- 158.4614707975258
-- -110.80063553388132
-- -51.751849761337894
-- -217.18757070277127
-- 29.294022225722266
-- -159.80189813545167
-- -98.0363360284864
-- -65.89005493794272
-- 97.23585021726144
-- 64.70535341404307
-- 122.88420470169062
-- 39.83731820265176
-- 80.76191426920725
-- -107.67937238334108
-- 32.73966581258587
-- -176.31679697677316
-- -157.05398798176645
-- 181.93695908987735
-- -22.204754687753276
-- 59.99996717282246
-- -43.62795843310611
-- -312.53381496997486
-- -39.741000135179796
-- 53.067107180690954
-- -94.02174504681692
-- 177.35460868059923
-- -278.39968211818746
-- -225.12024774102943
-- -261.18242012509285
-- 115.62981538281554
-- -98.77693755528435
-- -210.2718582996535
-- -272.01811798799883
-- -62.45265432971509
-- -253.15023775303436
-- -384.92067732976045
-- -475.3826677890834
-- -228.20692134710097
-- -96.13894507456048
-- -141.4995708372378
-- -134.28722392293884
-- -150.64608080511906
-- -127.56128538438503
-- -69.51462954026584
-- -165.96070501197542
-- -166.97446231475635
-- -105.01986203061864
-- -321.02750772665246
-- -179.74190590873235
-- -185.54048781133775
-- -283.0771623774315
-- -334.92854929352
-- -258.5036346594376
-- -340.2881399991007
-- 2.9251683017013326
-- -168.65125561864767
-- -274.7806613989737
-- -83.04249868201047
-- -180.92286540243515
-- -89.92432207277365
-- -11.627718736702299
-- -30.520520524489353
-- -308.2836685615376
-- 55.79159635947377
-- -1.028721057308561
-- -189.6408838060553
-- -75.39509171836379
-- 109.37174480760984
-- -47.151999749684684
-- -112.82172688972774
-- -122.46439859266991
-- -198.31238169600485
-- -110.08302317236765
-- -50.59160254068123
-- -79.08748078315834
-- 77.19380631825918
-- 14.248904365055418
-- -159.51798216420912
-- -56.11555257880312
-- -90.2440339055217
-- -392.1570105168502
-- -106.62345638777008
-- -180.5086141785024
-- -29.97062046444948
-- -117.84327291890007
-- -8.7540768117932
-- 129.46179350310734
-- 76.24881788567006
-- -183.12799672686822
-- -57.21196583403689
-- -48.789839744624985
-- -279.2667029477613
-- 12.943836342975453
-- 78.83995804778267
-- -126.25983712568782
-- -103.81640988851102
-- -183.7736218366929
-- -213.2531756682584
-- 50.39518284318662
-- 139.2192226443896
-- 67.49626762271218
-- -237.96811773997374
-- 25.08274372002822
-- -33.560313013774774
-- -173.01918177503993
-- 113.66472259892426
-- -121.97747942264799
-- -180.02930875136644
-- -170.19088534655762
-- 136.35987821597578
-- 96.6350544673526
-- 66.84156244822071
-- -195.28568574540293
-- -220.00818881027183
-- -52.78103015741021
-- -31.713914220009627
-- -49.72178214507511
-- -131.51961371834844
-- -96.90912203690274
-- -137.19949307427692
-- -164.01851373080234
-- -202.79022329301824
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
deleted file mode 100644
index 4e840d95d..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_5300iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -350.1756893430689
-- -541.839591849715
-- -168.09051010792922
-- 23.891080268019962
-- -190.48961933517865
-- -67.44798262750935
-- -283.8058731578255
-- -56.15096503224349
-- -101.2346887712504
-- -195.33894651640188
-- -87.71099758636797
-- 39.17429077313801
-- -218.88876034230438
-- -352.47526826165256
-- -15.556951705333246
-- -41.73090530661492
-- -188.87796866892165
-- -260.69546435839635
-- 41.921004001942755
-- -321.3496047921541
-- -340.9114314869833
-- -88.6149858528528
-- -336.9313792206388
-- -116.6713232366248
-- -79.01195649215286
-- -283.13525196591814
-- -98.29442399057879
-- -83.05468453334208
-- -89.30784504344854
-- -291.31917135721966
-- -286.2304713566369
-- 23.119800906054724
-- -203.4129006780318
-- -143.90917559464495
-- -96.44351771382031
-- 53.980492632643795
-- 103.0322664275742
-- -255.00922777323314
-- -154.46041207654082
-- -364.5926198488748
-- -198.6546844201885
-- -212.9173461870956
-- 59.658033602559534
-- -270.04984736677096
-- -336.35657260629785
-- -266.65686399693897
-- -514.2196806515818
-- -120.10722036554137
-- 113.52869021149299
-- -237.6369679704182
-- -74.26798003541333
-- -339.72200748643263
-- -235.0124746907095
-- -176.56605105155353
-- -278.31327634763636
-- -116.14672694815732
-- -222.6616976333995
-- -145.76263098777707
-- -372.19799397554095
-- -25.853912380006452
-- -330.4202124057533
-- 16.43185888956116
-- -254.91043143425807
-- 3.3308684759710268
-- -324.51006240626964
-- -96.86723694649756
-- -508.4929776644908
-- -259.10577651514296
-- -318.8078203008651
-- -139.98429087522803
-- -248.63840539191472
-- -202.26919987841785
-- -426.2186369681213
-- -169.06364530328938
-- -33.35515658672454
-- -164.09394972387616
-- -337.1130694168003
-- -282.0444988365109
-- 6.06943893903796
-- -166.1827223918531
-- -30.224050520413314
-- -218.74552179593502
-- -269.3423722562611
-- -325.0804145865783
-- 12.855214821346305
-- -31.462306979422557
-- 142.08182548397568
-- -318.3073344575577
-- -275.4660164735274
-- -172.2856633527697
-- -136.00869665393006
-- -180.58338113264952
-- -53.867654762685056
-- -193.69227086106378
-- -153.48472522766124
-- 70.83394962373872
-- -312.20137806872106
-- -5.436211416654895
-- -16.86409970640226
-- -269.1224943182721
-- -150.57572960315383
-- -39.45088777140532
-- -234.46137210174433
-- -264.94918101667207
-- -97.71585111996367
-- -117.27256437465216
-- 12.254605128927793
-- 211.6909142036703
-- -50.47304500828327
-- 52.96115143895034
-- -325.13465560598604
-- -112.16005062195325
-- -90.48471683489367
-- 183.12977894674228
-- -261.35354077163066
-- -118.52294406996586
-- 35.12940340171155
-- -133.25150347597543
-- -19.21068382378661
-- -103.94333131956081
-- 154.95791767035342
-- 53.28836431186244
-- 23.311612591066716
-- 13.98251101050205
-- 98.39063100089848
-- 5.091491197537112
-- 4.187730240374253
-- 66.2047418570116
-- 86.79675792688921
-- 167.03614718355487
-- -203.9496008273403
-- 183.23521880780507
-- -135.12354001160443
-- -85.60522160005215
-- 63.878143391082126
-- -120.94036593531315
-- 2.028043073945513
-- 36.18909245474933
-- 3.5450117587459555
-- -73.31230476231222
-- -95.08115674171863
-- -184.89239069984512
-- 42.04986153347611
-- 18.10411440538753
-- -68.9587453387462
-- -58.7607039169071
-- -91.50981697394722
-- -215.65242975397166
-- 82.904752101266
-- -238.97406849426307
-y:
-- -350.1756893430689
-- -541.839591849715
-- -168.09051010792922
-- 23.891080268019962
-- -190.48961933517865
-- -67.44798262750935
-- -283.8058731578255
-- -56.15096503224349
-- -101.2346887712504
-- -195.33894651640188
-- -87.71099758636797
-- 39.17429077313801
-- -218.88876034230438
-- -352.47526826165256
-- -15.556951705333246
-- -41.73090530661492
-- -188.87796866892165
-- -260.69546435839635
-- 41.921004001942755
-- -321.3496047921541
-- -340.9114314869833
-- -88.6149858528528
-- -336.9313792206388
-- -116.6713232366248
-- -79.01195649215286
-- -283.13525196591814
-- -98.29442399057879
-- -83.05468453334208
-- -89.30784504344854
-- -291.31917135721966
-- -286.2304713566369
-- 23.119800906054724
-- -203.4129006780318
-- -143.90917559464495
-- -96.44351771382031
-- 53.980492632643795
-- 103.0322664275742
-- -255.00922777323314
-- -154.46041207654082
-- -364.5926198488748
-- -198.6546844201885
-- -212.9173461870956
-- 59.658033602559534
-- -270.04984736677096
-- -336.35657260629785
-- -266.65686399693897
-- -514.2196806515818
-- -120.10722036554137
-- 113.52869021149299
-- -237.6369679704182
-- -74.26798003541333
-- -339.72200748643263
-- -235.0124746907095
-- -176.56605105155353
-- -278.31327634763636
-- -116.14672694815732
-- -222.6616976333995
-- -145.76263098777707
-- -372.19799397554095
-- -25.853912380006452
-- -330.4202124057533
-- 16.43185888956116
-- -254.91043143425807
-- 3.3308684759710268
-- -324.51006240626964
-- -96.86723694649756
-- -508.4929776644908
-- -259.10577651514296
-- -318.8078203008651
-- -139.98429087522803
-- -248.63840539191472
-- -202.26919987841785
-- -426.2186369681213
-- -169.06364530328938
-- -33.35515658672454
-- -164.09394972387616
-- -337.1130694168003
-- -282.0444988365109
-- 6.06943893903796
-- -166.1827223918531
-- -30.224050520413314
-- -218.74552179593502
-- -269.3423722562611
-- -325.0804145865783
-- 12.855214821346305
-- -31.462306979422557
-- 142.08182548397568
-- -318.3073344575577
-- -275.4660164735274
-- -172.2856633527697
-- -136.00869665393006
-- -180.58338113264952
-- -53.867654762685056
-- -193.69227086106378
-- -153.48472522766124
-- 70.83394962373872
-- -312.20137806872106
-- -5.436211416654895
-- -16.86409970640226
-- -269.1224943182721
-- -150.57572960315383
-- -39.45088777140532
-- -234.46137210174433
-- -264.94918101667207
-- -97.71585111996367
-- -117.27256437465216
-- 12.254605128927793
-- 211.6909142036703
-- -50.47304500828327
-- 52.96115143895034
-- -325.13465560598604
-- -112.16005062195325
-- -90.48471683489367
-- 183.12977894674228
-- -261.35354077163066
-- -118.52294406996586
-- 35.12940340171155
-- -133.25150347597543
-- -19.21068382378661
-- -103.94333131956081
-- 154.95791767035342
-- 53.28836431186244
-- 23.311612591066716
-- 13.98251101050205
-- 98.39063100089848
-- 5.091491197537112
-- 4.187730240374253
-- 66.2047418570116
-- 86.79675792688921
-- 167.03614718355487
-- -203.9496008273403
-- 183.23521880780507
-- -135.12354001160443
-- -85.60522160005215
-- 63.878143391082126
-- -120.94036593531315
-- 2.028043073945513
-- 36.18909245474933
-- 3.5450117587459555
-- -73.31230476231222
-- -95.08115674171863
-- -184.89239069984512
-- 42.04986153347611
-- 18.10411440538753
-- -68.9587453387462
-- -58.7607039169071
-- -91.50981697394722
-- -215.65242975397166
-- 82.904752101266
-- -238.97406849426307
-z:
-- 257.61172539326617
-- -6.618130669387091
-- -206.00715019186396
-- -86.30976067908074
-- 183.94514580380573
-- 59.280825073240536
-- -186.11338025477139
-- -116.47715113741444
-- 287.51960729933734
-- -287.32377562080916
-- -131.33531487385537
-- 200.29634539732655
-- -190.83469360027274
-- 141.18011313450324
-- -425.6957359539522
-- -137.4351307293813
-- 84.2334387295801
-- -167.5500539151613
-- 82.03453459321035
-- -113.74144685312307
-- 185.94863632159903
-- 66.8247118350421
-- 94.78550550055948
-- -301.22319808793543
-- 45.06039341490167
-- -40.52042981744621
-- 56.92518720546565
-- 119.21807228089337
-- 202.48608879948932
-- -71.06337143901112
-- -81.1877093346863
-- -233.11684463745996
-- 12.399466726228109
-- -190.0882035555663
-- -127.1838737973734
-- -43.436042328212025
-- 78.97443709887077
-- 47.569203023743306
-- 104.55812227817013
-- 74.13873305090655
-- 52.23069966386027
-- -116.08259030101665
-- -18.09364660434971
-- -179.44847295083076
-- -209.74027182688067
-- 148.19708758709768
-- -39.33733183565245
-- 29.9935063917469
-- -104.23570486350854
-- -352.06341012938367
-- -6.635367260097158
-- 96.68614567574384
-- -72.2525547152538
-- 229.18525391302035
-- -219.35401435771962
-- -261.02973563045543
-- -238.30069050160995
-- 112.74859045888273
-- -118.74227954945005
-- -199.61277005395687
-- -304.73627419671664
-- -62.06110537692866
-- -197.7609297870173
-- -376.0564558353367
-- -515.140300110365
-- -258.0924715445956
-- -21.707570024800344
-- -173.909407836862
-- -103.80808560229377
-- -84.68307037830921
-- -191.2990090644455
-- -80.01627779115191
-- -157.55735202680265
-- -152.57390711919544
-- -182.87754184649867
-- -385.46930530861545
-- -177.4443837439734
-- -216.78276439827755
-- -280.9291853700707
-- -382.28182496758507
-- -267.3550572789101
-- -374.06938964318357
-- -10.187531902064277
-- -184.4263283234975
-- -308.5049554241409
-- -139.70029390370794
-- -256.9684816712291
-- -69.53075309959198
-- -41.28763965694097
-- -48.24368546249476
-- -292.3879930896808
-- 58.68474300197547
-- -35.70933708415813
-- -158.8934500668035
-- -60.39367654698243
-- 149.72891397183068
-- -136.238393107494
-- -94.29894742097432
-- -142.03863797248454
-- -197.85287296329656
-- -53.18282711678025
-- -48.873060782231356
-- -109.02214835056309
-- 79.86260442253865
-- 9.203603753015521
-- -162.70806374119755
-- -56.14554446151242
-- -44.009174944212475
-- -406.6073343830843
-- -104.38633207583894
-- -189.38781565670146
-- 22.646049157775945
-- -81.84696125013302
-- -8.64475300616172
-- 133.09252108208128
-- 146.51257652526837
-- -228.07723192799574
-- 5.988954533203216
-- -13.25364631535978
-- -272.79141272086486
-- 72.83925693399259
-- 90.30157311917212
-- -68.79915364910022
-- -145.3687115803004
-- -148.97947822743163
-- -226.74252325912974
-- 59.014515013192614
-- 142.69442446848947
-- 89.77504479152044
-- -228.46076257438364
-- 22.937464184836305
-- -55.01263843325899
-- -137.17279266455355
-- 138.08816457284763
-- -73.53670893966546
-- -210.0748941243596
-- -150.71463118578984
-- 126.05819899711024
-- 145.5305672018292
-- 77.74374513197803
-- -216.53309268844833
-- -247.89898518428765
-- -16.77660653440476
-- -33.81717836339604
-- -59.817905675544196
-- -79.9069061760625
-- -97.95723595746524
-- -144.99990784662083
-- -164.950361248946
-- -208.17585911641996
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
deleted file mode 100644
index 37bf470ba..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6100iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -354.37547672825644
-- -578.4076892945358
-- -107.7214176118289
-- -5.583194356537376
-- -174.8024529666291
-- -49.71176993081215
-- -235.2852709481214
-- -80.04293719508337
-- -40.22294207313038
-- -183.2105895721838
-- -73.28299934813013
-- 21.72680263310102
-- -198.4519648778521
-- -357.23691456093695
-- -12.366725869355843
-- 23.833973892903572
-- -196.49991574516423
-- -323.4728961928933
-- 56.60422744480935
-- -318.5150359750603
-- -264.4038404647167
-- -56.32823405461204
-- -306.3199961193478
-- -85.69903013083811
-- -63.39444878558671
-- -344.68250556952955
-- -125.3937652593784
-- -91.79964988527658
-- -61.16933951887898
-- -279.47332050663397
-- -316.4996515092117
-- -3.4622872707177628
-- -192.61775644693725
-- -139.0374595204804
-- -88.62035793625397
-- -1.328738456243122
-- 178.80725728588084
-- -299.70469920872335
-- -71.2304985986049
-- -404.5156670814688
-- -235.88938025975474
-- -161.95859759241196
-- 89.14052082237625
-- -305.37184796671727
-- -408.51417713480623
-- -294.075466208455
-- -559.6037961107711
-- -90.98429815623903
-- 226.06762923822137
-- -293.07833276547865
-- -42.66124934132576
-- -334.6843577841629
-- -233.96529376377381
-- -216.0911324687805
-- -331.0621260970212
-- -88.41873611381575
-- -247.46253796639644
-- -156.85694547149083
-- -379.0250466471387
-- 9.319291455900977
-- -311.8736746536096
-- 45.491505165810246
-- -207.71450965334355
-- -31.694731733508842
-- -336.1125221668325
-- -61.433142613975996
-- -577.0441442077778
-- -303.5310673802776
-- -352.01529804845586
-- -71.49213445848079
-- -206.50260697053224
-- -151.93994188874612
-- -515.6518855638963
-- -150.28461386623766
-- 10.603840189052226
-- -172.21810964866498
-- -351.5932887407071
-- -362.65366390513134
-- 25.11936106635793
-- -253.32021353565395
-- 24.051487321846913
-- -259.6102556958661
-- -308.84420582985393
-- -375.15089787666915
-- -28.30428619760369
-- -33.419786400218804
-- 217.68244288493824
-- -332.1423289064766
-- -334.22536159179964
-- -163.0294109054836
-- -130.80526578867776
-- -107.7354759742875
-- -91.4099529250846
-- -175.55952879483604
-- -164.81804504720563
-- 61.859148898200615
-- -309.7945367460495
-- -37.11275024272886
-- 3.0380902027438337
-- -290.4791670863065
-- -115.79720539484173
-- -52.35668217371858
-- -199.29036314591102
-- -255.02683975888831
-- -87.18777704131581
-- -47.189115460126914
-- 10.421528098201627
-- 204.27357257872373
-- -124.498080153961
-- 86.79126426810572
-- -305.1122133509904
-- -92.23333307004609
-- -84.66773944957221
-- 212.12045530161723
-- -274.9300636493045
-- -133.6688509480514
-- 111.91825767203322
-- -111.8149729122468
-- -9.80459749015382
-- -86.3350360735338
-- 207.65127689722652
-- 46.987818022734295
-- -45.78997057332551
-- 40.23318136554408
-- 137.78460311183883
-- 6.026689042247037
-- 44.599274553512274
-- 56.4244623184312
-- 90.79637174151507
-- 187.80050347548678
-- -241.5699612092039
-- 161.43853717767013
-- -122.96982180302591
-- -79.44977832618412
-- 157.05035144082998
-- -127.54906772046294
-- 34.87633076652711
-- 103.8954481247204
-- 17.011849578170438
-- -90.39381045710203
-- -95.52756259423225
-- -228.93535283985594
-- 55.35461895380736
-- 98.68583828345103
-- -40.729841980180694
-- -85.12237752832773
-- -77.01310301831063
-- -281.2572901636379
-- 92.09804458954045
-- -240.22038057546192
-y:
-- -354.37547672825644
-- -578.4076892945358
-- -107.7214176118289
-- -5.583194356537376
-- -174.8024529666291
-- -49.71176993081215
-- -235.2852709481214
-- -80.04293719508337
-- -40.22294207313038
-- -183.2105895721838
-- -73.28299934813013
-- 21.72680263310102
-- -198.4519648778521
-- -357.23691456093695
-- -12.366725869355843
-- 23.833973892903572
-- -196.49991574516423
-- -323.4728961928933
-- 56.60422744480935
-- -318.5150359750603
-- -264.4038404647167
-- -56.32823405461204
-- -306.3199961193478
-- -85.69903013083811
-- -63.39444878558671
-- -344.68250556952955
-- -125.3937652593784
-- -91.79964988527658
-- -61.16933951887898
-- -279.47332050663397
-- -316.4996515092117
-- -3.4622872707177628
-- -192.61775644693725
-- -139.0374595204804
-- -88.62035793625397
-- -1.328738456243122
-- 178.80725728588084
-- -299.70469920872335
-- -71.2304985986049
-- -404.5156670814688
-- -235.88938025975474
-- -161.95859759241196
-- 89.14052082237625
-- -305.37184796671727
-- -408.51417713480623
-- -294.075466208455
-- -559.6037961107711
-- -90.98429815623903
-- 226.06762923822137
-- -293.07833276547865
-- -42.66124934132576
-- -334.6843577841629
-- -233.96529376377381
-- -216.0911324687805
-- -331.0621260970212
-- -88.41873611381575
-- -247.46253796639644
-- -156.85694547149083
-- -379.0250466471387
-- 9.319291455900977
-- -311.8736746536096
-- 45.491505165810246
-- -207.71450965334355
-- -31.694731733508842
-- -336.1125221668325
-- -61.433142613975996
-- -577.0441442077778
-- -303.5310673802776
-- -352.01529804845586
-- -71.49213445848079
-- -206.50260697053224
-- -151.93994188874612
-- -515.6518855638963
-- -150.28461386623766
-- 10.603840189052226
-- -172.21810964866498
-- -351.5932887407071
-- -362.65366390513134
-- 25.11936106635793
-- -253.32021353565395
-- 24.051487321846913
-- -259.6102556958661
-- -308.84420582985393
-- -375.15089787666915
-- -28.30428619760369
-- -33.419786400218804
-- 217.68244288493824
-- -332.1423289064766
-- -334.22536159179964
-- -163.0294109054836
-- -130.80526578867776
-- -107.7354759742875
-- -91.4099529250846
-- -175.55952879483604
-- -164.81804504720563
-- 61.859148898200615
-- -309.7945367460495
-- -37.11275024272886
-- 3.0380902027438337
-- -290.4791670863065
-- -115.79720539484173
-- -52.35668217371858
-- -199.29036314591102
-- -255.02683975888831
-- -87.18777704131581
-- -47.189115460126914
-- 10.421528098201627
-- 204.27357257872373
-- -124.498080153961
-- 86.79126426810572
-- -305.1122133509904
-- -92.23333307004609
-- -84.66773944957221
-- 212.12045530161723
-- -274.9300636493045
-- -133.6688509480514
-- 111.91825767203322
-- -111.8149729122468
-- -9.80459749015382
-- -86.3350360735338
-- 207.65127689722652
-- 46.987818022734295
-- -45.78997057332551
-- 40.23318136554408
-- 137.78460311183883
-- 6.026689042247037
-- 44.599274553512274
-- 56.4244623184312
-- 90.79637174151507
-- 187.80050347548678
-- -241.5699612092039
-- 161.43853717767013
-- -122.96982180302591
-- -79.44977832618412
-- 157.05035144082998
-- -127.54906772046294
-- 34.87633076652711
-- 103.8954481247204
-- 17.011849578170438
-- -90.39381045710203
-- -95.52756259423225
-- -228.93535283985594
-- 55.35461895380736
-- 98.68583828345103
-- -40.729841980180694
-- -85.12237752832773
-- -77.01310301831063
-- -281.2572901636379
-- 92.09804458954045
-- -240.22038057546192
-z:
-- 293.92236286138564
-- 19.241452133628222
-- -178.41933038279544
-- -147.0074323909161
-- 192.94005499915357
-- 33.77595909683745
-- -161.44377017159454
-- -111.99113759130483
-- 267.74360394463275
-- -326.5374000425544
-- -69.30458356048919
-- 164.4084883508337
-- -220.27140830867683
-- 176.81453608793652
-- -434.15062967005457
-- -188.7408733123947
-- 85.69818502704535
-- -158.89138642636277
-- 50.20887652377516
-- -160.8305257642651
-- 182.73354476031733
-- 90.39211452153127
-- 126.39133212594713
-- -321.5323137580511
-- 64.61567481188047
-- -52.81716295547091
-- 105.79934092625282
-- 112.09310582523618
-- 224.0250325659121
-- -129.9806706961594
-- -70.56853529090154
-- -275.73295097316606
-- 31.232613884774455
-- -216.1477444370246
-- -69.53333636704727
-- -23.23087843940836
-- 61.49921720803479
-- 66.2511171803064
-- 76.43369675431458
-- 122.80839411993087
-- 78.008525125722
-- -97.93405791579586
-- -37.48326993315027
-- -155.07042759537129
-- -215.6980838717558
-- 156.93235852981812
-- -38.9553098237568
-- 64.34348593444682
-- -69.93608871721042
-- -406.93438179643607
-- 1.739395361893391
-- 93.70806390977725
-- -64.70249343408463
-- 271.45546365247975
-- -227.93573522432246
-- -282.05929950281734
-- -326.1545440373919
-- 141.45526175457692
-- -105.76836588318919
-- -230.11495205568335
-- -329.23423347911614
-- -45.71005031607355
-- -202.25994672806672
-- -359.49877791738425
-- -551.7278452351626
-- -301.07226339518064
-- -63.5150242100091
-- -148.67505599039512
-- -121.46053304970646
-- -93.37093156057227
-- -195.40151517449308
-- -79.92444602019467
-- -168.4095127647589
-- -148.11530917097863
-- -219.82234677078625
-- -436.2791163720004
-- -195.46775918565436
-- -259.64651580465033
-- -295.84083527656486
-- -411.3334310705196
-- -300.56886364344155
-- -408.76562128605383
-- -35.97498119708834
-- -124.35957334358373
-- -314.7502413312763
-- -166.90451719918136
-- -247.16628211001097
-- -115.38659431426734
-- -29.838930937522342
-- -33.27036950805728
-- -342.6852970289899
-- 58.049657180501754
-- -105.16251169888999
-- -189.99987624016552
-- -80.9738437935302
-- 159.1326775311695
-- -166.40754333444988
-- -71.4460796821466
-- -109.32677736714189
-- -240.19786312624714
-- -91.3264776290697
-- -34.51395036269785
-- -90.27194293651411
-- 84.26609542156059
-- 47.42899256617139
-- -113.03090691352375
-- -56.63492264635932
-- -45.9412345476945
-- -431.3194457594861
-- -106.43355072795664
-- -187.38528258445865
-- 66.75572114259693
-- -66.36411824095501
-- 42.22835793425958
-- 177.73845023522196
-- 171.3302268946976
-- -228.17141701604191
-- 27.260792680540664
-- 5.889945909417749
-- -285.89941821763455
-- 152.51764436502387
-- 158.34210364178855
-- 51.86098635830103
-- -105.06659889602022
-- -144.59533257551183
-- -199.7905570224701
-- 42.73820513063766
-- 214.89392708348026
-- 138.47767815649885
-- -230.8829240411559
-- 57.02100988993763
-- -31.750840864820702
-- -124.57036261647724
-- 197.56676054716746
-- -35.77568324466903
-- -217.23830600410275
-- -164.07271513415145
-- 185.6209126476897
-- 133.54127708571707
-- 59.302865225121735
-- -218.96327874462574
-- -271.1150876277143
-- -4.577854877414626
-- -0.872197402222953
-- -97.60415567801225
-- -26.193253475528515
-- -87.42289444036297
-- -183.92027475378654
-- -177.78294904663525
-- -194.0016286625082
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
deleted file mode 100644
index 74e3c4c62..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_6900iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -328.7146602458411
-- -607.8480109459788
-- -70.49741332152209
-- 3.796692637047578
-- -162.95954309910817
-- -19.33972914626468
-- -235.09499521014578
-- -107.81384573412645
-- -31.53291462742378
-- -212.14886603050897
-- -89.46516739532265
-- 19.667982651940264
-- -216.33514516843985
-- -323.94021603169455
-- 3.7495919998539446
-- -18.021739092082814
-- -178.17970317466126
-- -346.25709247409145
-- 54.58157208827058
-- -367.15453044986333
-- -268.56541940327196
-- -81.23299020243925
-- -309.8770966018138
-- -100.67991512115881
-- -67.07968197377089
-- -347.2361920567107
-- -158.80898708863495
-- -104.01739402591244
-- -49.383573776489996
-- -298.4424163083782
-- -284.59719829638186
-- 1.9523807141375829
-- -228.8291871431349
-- -164.38882205876453
-- -56.82795956826953
-- 29.181611857038934
-- 197.59244963556043
-- -291.65009486814546
-- -73.96025052809098
-- -413.24345176232146
-- -289.00049014132344
-- -138.12231990311398
-- 122.85209320325096
-- -292.21133060332454
-- -371.8834044482743
-- -292.7279115117123
-- -584.8788687697738
-- -95.33440230176042
-- 239.5339774386685
-- -265.12892232434035
-- -53.34571751364334
-- -319.31875247104443
-- -226.3464861485659
-- -227.7066292952969
-- -322.42046471465534
-- -66.77461133038851
-- -247.64260359977254
-- -128.70180326118776
-- -371.86119903327614
-- 25.939839889457698
-- -303.22971631311106
-- 68.73563774325528
-- -220.62010394260605
-- 1.913655145979548
-- -321.05786511717696
-- -7.480576699947837
-- -562.6168351641293
-- -272.59111196344
-- -352.89209393639175
-- -63.22199637626166
-- -190.819897528042
-- -171.66970452269976
-- -542.3756048762308
-- -147.5045699695532
-- -32.951634458915514
-- -178.31868993964395
-- -370.4598127663352
-- -382.7765759441242
-- 70.55718440121618
-- -275.84240189804007
-- 24.52055475154436
-- -267.85699106997123
-- -362.6936740571793
-- -394.94518637379
-- -42.62824006660958
-- -37.4206296874913
-- 228.65827424080354
-- -368.34337229210706
-- -335.9848146528929
-- -185.57919006723577
-- -182.8323636774351
-- -148.05736988839385
-- -97.9632827112206
-- -172.58472121166204
-- -193.5201220514574
-- 52.94458737541407
-- -321.57488746239267
-- -62.46443325224946
-- 31.459204508390602
-- -320.34494746894666
-- -133.35752677467624
-- -33.91251403894966
-- -212.1959457043817
-- -257.8482070032987
-- -123.65941355538556
-- -46.78896318283864
-- 9.432068300432947
-- 195.92446684229847
-- -92.47988107673412
-- 88.53287931482706
-- -282.7293574505644
-- -56.04024780508934
-- -82.88216300356626
-- 256.79684689104766
-- -260.74896707781465
-- -120.52637661802831
-- 116.14986091913589
-- -66.23371973712261
-- 15.895059395863738
-- -103.5226695817851
-- 210.70305548700676
-- 75.5169908210367
-- -37.984599501492156
-- 43.13787154468077
-- 129.34416144787176
-- 17.434899592911798
-- 56.42418539420781
-- 40.27183357779729
-- 98.78430657831635
-- 215.67691442422276
-- -252.7746044156219
-- 142.22049019547737
-- -123.82476100073752
-- -76.28157211598544
-- 176.92621790810435
-- -137.5335650117231
-- 53.6554360276977
-- 132.97662711499208
-- 40.88040400506932
-- -91.68926179011021
-- -117.31681032188678
-- -222.9524498197763
-- 46.32122638601927
-- 99.43427727081166
-- 19.197643617274366
-- -62.516626238707644
-- -85.33848766172152
-- -292.1531296078384
-- 126.56353108153971
-- -246.1397928436873
-y:
-- -328.7146602458411
-- -607.8480109459788
-- -70.49741332152209
-- 3.796692637047578
-- -162.95954309910817
-- -19.33972914626468
-- -235.09499521014578
-- -107.81384573412645
-- -31.53291462742378
-- -212.14886603050897
-- -89.46516739532265
-- 19.667982651940264
-- -216.33514516843985
-- -323.94021603169455
-- 3.7495919998539446
-- -18.021739092082814
-- -178.17970317466126
-- -346.25709247409145
-- 54.58157208827058
-- -367.15453044986333
-- -268.56541940327196
-- -81.23299020243925
-- -309.8770966018138
-- -100.67991512115881
-- -67.07968197377089
-- -347.2361920567107
-- -158.80898708863495
-- -104.01739402591244
-- -49.383573776489996
-- -298.4424163083782
-- -284.59719829638186
-- 1.9523807141375829
-- -228.8291871431349
-- -164.38882205876453
-- -56.82795956826953
-- 29.181611857038934
-- 197.59244963556043
-- -291.65009486814546
-- -73.96025052809098
-- -413.24345176232146
-- -289.00049014132344
-- -138.12231990311398
-- 122.85209320325096
-- -292.21133060332454
-- -371.8834044482743
-- -292.7279115117123
-- -584.8788687697738
-- -95.33440230176042
-- 239.5339774386685
-- -265.12892232434035
-- -53.34571751364334
-- -319.31875247104443
-- -226.3464861485659
-- -227.7066292952969
-- -322.42046471465534
-- -66.77461133038851
-- -247.64260359977254
-- -128.70180326118776
-- -371.86119903327614
-- 25.939839889457698
-- -303.22971631311106
-- 68.73563774325528
-- -220.62010394260605
-- 1.913655145979548
-- -321.05786511717696
-- -7.480576699947837
-- -562.6168351641293
-- -272.59111196344
-- -352.89209393639175
-- -63.22199637626166
-- -190.819897528042
-- -171.66970452269976
-- -542.3756048762308
-- -147.5045699695532
-- -32.951634458915514
-- -178.31868993964395
-- -370.4598127663352
-- -382.7765759441242
-- 70.55718440121618
-- -275.84240189804007
-- 24.52055475154436
-- -267.85699106997123
-- -362.6936740571793
-- -394.94518637379
-- -42.62824006660958
-- -37.4206296874913
-- 228.65827424080354
-- -368.34337229210706
-- -335.9848146528929
-- -185.57919006723577
-- -182.8323636774351
-- -148.05736988839385
-- -97.9632827112206
-- -172.58472121166204
-- -193.5201220514574
-- 52.94458737541407
-- -321.57488746239267
-- -62.46443325224946
-- 31.459204508390602
-- -320.34494746894666
-- -133.35752677467624
-- -33.91251403894966
-- -212.1959457043817
-- -257.8482070032987
-- -123.65941355538556
-- -46.78896318283864
-- 9.432068300432947
-- 195.92446684229847
-- -92.47988107673412
-- 88.53287931482706
-- -282.7293574505644
-- -56.04024780508934
-- -82.88216300356626
-- 256.79684689104766
-- -260.74896707781465
-- -120.52637661802831
-- 116.14986091913589
-- -66.23371973712261
-- 15.895059395863738
-- -103.5226695817851
-- 210.70305548700676
-- 75.5169908210367
-- -37.984599501492156
-- 43.13787154468077
-- 129.34416144787176
-- 17.434899592911798
-- 56.42418539420781
-- 40.27183357779729
-- 98.78430657831635
-- 215.67691442422276
-- -252.7746044156219
-- 142.22049019547737
-- -123.82476100073752
-- -76.28157211598544
-- 176.92621790810435
-- -137.5335650117231
-- 53.6554360276977
-- 132.97662711499208
-- 40.88040400506932
-- -91.68926179011021
-- -117.31681032188678
-- -222.9524498197763
-- 46.32122638601927
-- 99.43427727081166
-- 19.197643617274366
-- -62.516626238707644
-- -85.33848766172152
-- -292.1531296078384
-- 126.56353108153971
-- -246.1397928436873
-z:
-- 318.2524160356094
-- -12.81841367532651
-- -193.1287544958996
-- -133.63892640590154
-- 204.56723729302252
-- 64.89053292856171
-- -127.84584777350406
-- -136.72974321901606
-- 265.8967844680789
-- -336.67287081313646
-- -81.38469290012515
-- 184.97457481256922
-- -206.1684398552655
-- 178.00750310058874
-- -495.436688572458
-- -184.43643300262082
-- 70.61722830440735
-- -180.53381374269597
-- 61.15147986279117
-- -201.2155198196378
-- 216.42275755355072
-- 75.10195659923006
-- 105.89063668574734
-- -312.93764497623437
-- 67.16122480021578
-- -51.418462010262246
-- 112.0958000868312
-- 101.15634031816816
-- 208.6217673626878
-- -138.010997156574
-- -44.28128581710678
-- -236.62248445940946
-- 43.15533417477952
-- -225.65915729611567
-- -58.21445126098314
-- -15.866666458719003
-- 74.69944948664964
-- 64.08204316537748
-- 97.03165011394645
-- 114.56913004719722
-- 69.4787649122138
-- -105.5269029435403
-- -12.134569507386843
-- -164.8138790490863
-- -230.94019616856926
-- 169.85000089522825
-- -32.6976537911571
-- 61.63993704700873
-- -42.24733910474994
-- -419.57316919610236
-- 8.750229447832593
-- 79.69667187137283
-- -47.81355328320213
-- 287.15248208579277
-- -230.74405474895386
-- -282.5527547209933
-- -342.25135478038527
-- 187.2173874299178
-- -140.9169062236822
-- -230.96167702054072
-- -310.9662394801051
-- -17.32391046987855
-- -226.30122339576295
-- -358.4738424481016
-- -589.8343707622778
-- -278.7087857945023
-- -42.11374263217224
-- -100.79323152793262
-- -146.82423428698908
-- -114.09268567895346
-- -199.50434588100805
-- -112.1359537930152
-- -186.20288786412223
-- -120.66079987326846
-- -181.84723982168921
-- -418.97775621234376
-- -213.1222839881335
-- -291.8090957914463
-- -302.7289002214794
-- -464.26002291202616
-- -282.7260422501955
-- -427.3427612583488
-- -7.52533734197854
-- -141.15940652480538
-- -322.11798023734565
-- -205.3080403096517
-- -283.6248535441436
-- -121.85634073301752
-- -10.386750269561213
-- -60.56173074990178
-- -350.4564659625923
-- 68.19015121550102
-- -148.47295886483207
-- -185.75356805338285
-- -112.29743473122012
-- 189.92420830080493
-- -210.29869438123163
-- -71.53468992439852
-- -94.78836717876979
-- -273.2731007496173
-- -68.11104997702215
-- -52.98663520098969
-- -68.48341395216713
-- 120.20413190025302
-- 52.140332200565595
-- -115.75886751075116
-- -50.620987760471564
-- -42.80291792239785
-- -419.473604990822
-- -56.3871344677163
-- -201.06219586176312
-- 94.2974811911247
-- -34.53199240151439
-- 48.183596631482935
-- 196.31453096731906
-- 191.7814341469975
-- -226.3267741263968
-- 36.55784553908029
-- -4.444206276831911
-- -269.65346359957186
-- 187.22423003269267
-- 199.12359312594646
-- 59.384532474532065
-- -71.2652919102802
-- -148.28452193150292
-- -172.03941811352055
-- 53.119560217802615
-- 238.27762162827779
-- 132.7491820828763
-- -213.46909802749275
-- 82.48618282765918
-- -1.8356810571683047
-- -136.6766586962044
-- 191.14763171417005
-- -17.31448628033703
-- -201.57759561405004
-- -162.47596852664273
-- 262.32005188164777
-- 165.87873799997138
-- 78.67759494035175
-- -243.3700332084363
-- -293.33086145315184
-- 14.78420456461757
-- 14.394439887667057
-- -62.39227894825053
-- -5.181470901694216
-- -94.85502949056276
-- -173.18252574263116
-- -163.59185250182108
-- -184.34441485108246
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
deleted file mode 100644
index b00633da1..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_corrected_underhand_weights_800iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -121.15478633343794
-- -130.4159808404925
-- -130.27646944026458
-- -113.64328655416284
-- -121.44628695154886
-- -166.2990451476203
-- -261.2636454017961
-- -184.76630901132344
-- -122.10815924542676
-- -282.2547492865744
-- -138.2182137765148
-- -109.88726609443934
-- -150.720646999703
-- -207.41906706153105
-- -75.78930140492815
-- -183.1141566294361
-- -145.86465327098085
-- -235.36504545335836
-- -281.0945547173969
-- -233.18214620754406
-- -207.12616237540004
-- -152.43461471702767
-- -203.04141984763248
-- -150.02455852136328
-- -187.34518426809518
-- -265.6111612741161
-- -129.04348845917977
-- -193.41142225312666
-- -90.7049859565704
-- -127.25169814282474
-- -182.64504946140917
-- -157.8249434663258
-- -217.7562185578996
-- -181.93348129015948
-- -242.71386084285342
-- -146.79164939442038
-- -146.71336252409114
-- -229.87361647392953
-- -133.77193019885993
-- -221.35140091427792
-- -155.94743088882197
-- -166.81724509985116
-- -113.09320678295467
-- -171.27027465849542
-- -159.377602672052
-- -187.21305388503686
-- -238.80090511164062
-- -253.1262646143503
-- -109.08487387042756
-- -184.49469971168386
-- -191.5132311229388
-- -70.44496776860801
-- -215.95165963817644
-- -107.85620819625484
-- -149.06971162669294
-- -119.58156072478897
-- -159.54179461640751
-- -230.98093410223288
-- -201.63392035010048
-- -90.5827797630511
-- -142.80374779196856
-- -95.45514778314696
-- -165.28086739578055
-- -88.62227912176544
-- -119.88567178023406
-- -135.31015574629714
-- -223.9174629585549
-- -146.8804645936043
-- -288.5096537730227
-- -176.28728372651457
-- -254.21306186323173
-- -57.47901626585383
-- -111.89370887483302
-- -167.58998789322217
-- -156.84442274791556
-- -178.862535446946
-- -131.98705585336998
-- -155.53653700458938
-- -114.87982704608743
-- -127.6053182111233
-- -169.29952702677664
-- -230.71280999323812
-- -110.69697751547955
-- -147.60343628352976
-- -77.0222276016208
-- -116.05610464762434
-- -59.98167452309284
-- -145.59731787917104
-- -161.10254812375408
-- -243.46339060931686
-- -60.274220954704276
-- -34.39861924673747
-- -126.93520435774637
-- -118.75874670731521
-- -73.14398889461458
-- -91.82329391402995
-- -124.65378712679401
-- -120.06852899132907
-- -38.848619248791735
-- -148.5131138691994
-- -96.11620901748239
-- -166.62503100670511
-- -146.99077907273647
-- -103.29466946283068
-- -129.278248987387
-- -40.509302977612506
-- -163.45472246452005
-- -13.143062898443247
-- -97.43882615461838
-- -86.60275662369912
-- -145.7810199124991
-- -125.2793809409674
-- -165.23145616311692
-- 132.4120951658318
-- -45.79313797853327
-- -127.40765968376884
-- -64.48246125134183
-- -70.96459335268244
-- -85.77909066650177
-- -94.10623918741744
-- -76.58296444224553
-- -4.976619258568157
-- 5.413038481518587
-- -10.133824365383301
-- -119.32397167560802
-- -25.615445843241176
-- -15.827317551960947
-- -21.449299818373746
-- 81.63003002905442
-- -23.37984925727442
-- -25.08432384280621
-- 29.63273278568562
-- -80.38488086520215
-- 33.28432641495689
-- -58.79565430878664
-- 13.956506074278591
-- -47.47275973086286
-- -0.16772532349929142
-- 21.53988916730403
-- -73.84273357578618
-- -72.00767054173988
-- -172.19982217673925
-- -1.5198048382999299
-- -116.49223499770191
-- -91.15236024297808
-- -55.5915083964266
-- -62.02646446322047
-- -131.33091040195643
-- -142.89761461974723
-- -129.5469960360492
-y:
-- -121.15478633343794
-- -130.4159808404925
-- -130.27646944026458
-- -113.64328655416284
-- -121.44628695154886
-- -166.2990451476203
-- -261.2636454017961
-- -184.76630901132344
-- -122.10815924542676
-- -282.2547492865744
-- -138.2182137765148
-- -109.88726609443934
-- -150.720646999703
-- -207.41906706153105
-- -75.78930140492815
-- -183.1141566294361
-- -145.86465327098085
-- -235.36504545335836
-- -281.0945547173969
-- -233.18214620754406
-- -207.12616237540004
-- -152.43461471702767
-- -203.04141984763248
-- -150.02455852136328
-- -187.34518426809518
-- -265.6111612741161
-- -129.04348845917977
-- -193.41142225312666
-- -90.7049859565704
-- -127.25169814282474
-- -182.64504946140917
-- -157.8249434663258
-- -217.7562185578996
-- -181.93348129015948
-- -242.71386084285342
-- -146.79164939442038
-- -146.71336252409114
-- -229.87361647392953
-- -133.77193019885993
-- -221.35140091427792
-- -155.94743088882197
-- -166.81724509985116
-- -113.09320678295467
-- -171.27027465849542
-- -159.377602672052
-- -187.21305388503686
-- -238.80090511164062
-- -253.1262646143503
-- -109.08487387042756
-- -184.49469971168386
-- -191.5132311229388
-- -70.44496776860801
-- -215.95165963817644
-- -107.85620819625484
-- -149.06971162669294
-- -119.58156072478897
-- -159.54179461640751
-- -230.98093410223288
-- -201.63392035010048
-- -90.5827797630511
-- -142.80374779196856
-- -95.45514778314696
-- -165.28086739578055
-- -88.62227912176544
-- -119.88567178023406
-- -135.31015574629714
-- -223.9174629585549
-- -146.8804645936043
-- -288.5096537730227
-- -176.28728372651457
-- -254.21306186323173
-- -57.47901626585383
-- -111.89370887483302
-- -167.58998789322217
-- -156.84442274791556
-- -178.862535446946
-- -131.98705585336998
-- -155.53653700458938
-- -114.87982704608743
-- -127.6053182111233
-- -169.29952702677664
-- -230.71280999323812
-- -110.69697751547955
-- -147.60343628352976
-- -77.0222276016208
-- -116.05610464762434
-- -59.98167452309284
-- -145.59731787917104
-- -161.10254812375408
-- -243.46339060931686
-- -60.274220954704276
-- -34.39861924673747
-- -126.93520435774637
-- -118.75874670731521
-- -73.14398889461458
-- -91.82329391402995
-- -124.65378712679401
-- -120.06852899132907
-- -38.848619248791735
-- -148.5131138691994
-- -96.11620901748239
-- -166.62503100670511
-- -146.99077907273647
-- -103.29466946283068
-- -129.278248987387
-- -40.509302977612506
-- -163.45472246452005
-- -13.143062898443247
-- -97.43882615461838
-- -86.60275662369912
-- -145.7810199124991
-- -125.2793809409674
-- -165.23145616311692
-- 132.4120951658318
-- -45.79313797853327
-- -127.40765968376884
-- -64.48246125134183
-- -70.96459335268244
-- -85.77909066650177
-- -94.10623918741744
-- -76.58296444224553
-- -4.976619258568157
-- 5.413038481518587
-- -10.133824365383301
-- -119.32397167560802
-- -25.615445843241176
-- -15.827317551960947
-- -21.449299818373746
-- 81.63003002905442
-- -23.37984925727442
-- -25.08432384280621
-- 29.63273278568562
-- -80.38488086520215
-- 33.28432641495689
-- -58.79565430878664
-- 13.956506074278591
-- -47.47275973086286
-- -0.16772532349929142
-- 21.53988916730403
-- -73.84273357578618
-- -72.00767054173988
-- -172.19982217673925
-- -1.5198048382999299
-- -116.49223499770191
-- -91.15236024297808
-- -55.5915083964266
-- -62.02646446322047
-- -131.33091040195643
-- -142.89761461974723
-- -129.5469960360492
-z:
-- -12.587038106327535
-- -75.8693731388532
-- -93.00580404520751
-- -82.28073181181468
-- -55.017515432835864
-- -78.58553297105018
-- -181.7216171468009
-- -105.8153065907965
-- -113.72834735653169
-- -123.77835231502743
-- -94.36124237121506
-- 7.999874703690282
-- -88.505402572207
-- -84.23582958218344
-- -167.35780831101766
-- -191.1077910787939
-- -112.51782966658953
-- -157.5923619634534
-- -88.92691745071829
-- -143.34048412853903
-- -84.56169786453201
-- -87.94842923788495
-- -130.01615815791274
-- -68.12015090403574
-- -161.75758261934803
-- -116.00810406593533
-- -89.82812787449495
-- -2.022554635745049
-- -11.76170352666318
-- -32.255452835749836
-- -115.1246554278457
-- -71.49232748361257
-- -124.00230967207509
-- -70.89645000454215
-- -102.95482128272164
-- -50.82159027419259
-- -33.06904048823107
-- -22.63036689793711
-- 2.9025713302973144
-- -124.74230868900925
-- -8.922991349024134
-- -53.6855755869238
-- -41.667422918658055
-- -59.574977423264315
-- -52.24133195122149
-- -14.9753707692793
-- 18.371216641533415
-- 40.448751873301134
-- -30.07601475057829
-- -8.638289462281344
-- -19.103673741593955
-- -58.989160693329964
-- 38.81395101189456
-- 64.31413648932175
-- 61.814901826391015
-- -22.73329548690856
-- 92.4764821585138
-- -30.905180607424644
-- 95.19599454483829
-- -59.41134246808385
-- -76.17332243157632
-- 49.14415703073
-- -63.74836549475062
-- -65.66980125862429
-- -46.31125437510522
-- -15.5854018545872
-- -68.1014814060287
-- -32.71914549612089
-- -82.70066988198958
-- 41.37278808561206
-- -35.29310228210178
-- -24.80334069785697
-- -2.501666953743949
-- -81.05624834186443
-- 44.689570141437194
-- -9.444026603431737
-- 28.661934934171853
-- 33.534676508527404
-- -23.002377446693522
-- -57.54964424848572
-- -36.844359921983305
-- -74.2492910493037
-- -22.215655183248508
-- -8.425280695608198
-- -81.06803504992166
-- 47.50293629619174
-- 1.2507596344205354
-- -22.147329795064653
-- 21.817219643649302
-- -2.233890964099581
-- 25.703929585905495
-- -56.973551523556274
-- 72.05132559995151
-- -88.96915425026873
-- 10.404583012909765
-- 19.217131237387072
-- -51.67869170029975
-- -86.73728854036517
-- 8.965281264407682
-- -61.39529818193962
-- -43.19282276378947
-- -49.742938375732855
-- -135.55064286454
-- -69.48539283915129
-- -34.52860770873315
-- -41.45447761465108
-- -45.31136237153822
-- -172.98720619427928
-- -91.62860054228031
-- -118.69990132513988
-- -98.59155740063761
-- -38.615966048441194
-- -59.95574983985258
-- -46.60106300721521
-- -24.44048283174632
-- -93.30967132941213
-- -139.37546334098232
-- -191.03883261368398
-- -96.34676308834342
-- -219.64922426058578
-- -54.31860599999553
-- 41.76489177130727
-- -94.79148089749532
-- -105.1248427909044
-- -179.88647293330678
-- -90.66257184925013
-- -62.56560384391366
-- 6.115980278907556
-- 10.37361018493507
-- -152.85158652125753
-- -69.1644018552841
-- -130.76626993605592
-- -92.52705628205884
-- 4.084238037716294
-- -115.05479159529355
-- -101.78629734791565
-- -111.11818707348276
-- -57.16831584420238
-- -55.389153204954084
-- -10.608819349789805
-- -70.22122456032068
-- -171.76502737028918
-- -32.273121976922596
-- -154.64579911444298
-- -58.88622322185824
-- -128.42693267456954
-- -123.58760787442688
-- -185.1613937082734
-- -123.90573654523244
-- -135.67799993329913
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights.yaml
similarity index 100%
rename from mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2900iters.yaml
rename to mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights.yaml
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
deleted file mode 100644
index 619510115..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_1600iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -66.18292178053807
-- -115.63371289306161
-- -40.08998640911374
-- -261.31000395194786
-- -166.7909838724932
-- -139.54637670314835
-- -208.37719185679137
-- -57.717209802050384
-- -146.99926782334603
-- -94.16984989068037
-- -255.50464359809362
-- -67.9308770378757
-- -353.4014028425089
-- -168.20396034869532
-- -191.70073210680513
-- -36.41025520518381
-- -258.97388466685254
-- -239.09393621075688
-- -215.75428259623223
-- -99.96184009589246
-- -117.46963703545366
-- -16.350110272823727
-- -234.7905478492251
-- -165.3589026036757
-- 0.41946019958205855
-- -115.11125087827226
-- -147.41853651364576
-- -103.74454769846525
-- -140.90619141642424
-- -201.40554357493755
-- -341.71306618260576
-- -47.520601996709544
-- 8.723661160614721
-- -304.5231100867443
-- -12.198570874493013
-- -200.07187414553692
-- -89.68284195206441
-- -250.5605909355231
-- -276.7974106271435
-- -80.99692915874027
-- -136.66654586506084
-- -51.79816062682904
-- -85.62445575252828
-- -262.1957398217536
-- -193.5124451059287
-- -205.26576800249143
-- -138.34121902448936
-- -71.9445712933336
-- -22.608517251998556
-- -94.87504568074831
-- -127.96760156077839
-- -173.48834175964086
-- -196.745733980285
-- -159.36858356085776
-- -80.60888792369248
-- -468.1650837655404
-- -125.51666316581834
-- 10.712475760469315
-- -5.411213154927566
-- 96.82693230399087
-- -41.911224794493464
-- 22.762124542131428
-- -98.85987185081449
-- -28.459453910768794
-- 47.521581958349294
-- -248.36345727055203
-- -167.45029857022115
-- 128.28266410089716
-- 57.27033034845042
-- -58.82963578612552
-- -78.48792428054871
-- 67.26178698214157
-- -50.94569383530171
-- 56.58236082078612
-- -120.18656749761499
-- -100.11721665267609
-- -101.31410997999905
-- -55.53205860919838
-- 100.88786413431473
-- -66.82830491525722
-- 118.40778297185312
-- 68.27470664018782
-- 18.77035395820749
-- 38.56560723006115
-- 108.36119035325538
-- 2.505752009999138
-- 95.48861783590644
-- -86.62410294454978
-- 62.88822437860383
-- 8.45668376037163
-- 74.09636149418711
-- -154.22210970764675
-- -33.253355796991094
-- 247.37717511204832
-- 143.07800461797132
-- -78.2684811293491
-- 51.571400492195494
-- -132.71704558130395
-- -13.784941989330031
-- 146.01977475651552
-- 39.56660609935573
-- 113.70599705854998
-- 77.3503599700983
-- 4.444035442699823
-- 55.18774963618358
-- 42.463739509132644
-- 123.39346732558771
-- -54.62464266152292
-- 187.43447087945583
-- 44.22944044489332
-- 73.45213044828706
-- 20.761036793462168
-- -108.08520198682515
-- 176.7604941815522
-- 51.5867519537819
-- 163.86498409750413
-- 146.4850396263265
-- 203.95973954751176
-- 68.72742030684795
-- 261.21759885320284
-- -7.866632151184333
-- 163.9094272594558
-- 234.0995403976671
-- 118.97240645672969
-- 114.44267554008135
-- 133.6913309297981
-- 269.8763973140816
-- 272.9466815796491
-- 99.81503504177155
-- 296.59531963384484
-- 17.92059475315728
-- -16.90459184109093
-- 179.07991534518297
-- 354.0866295072622
-- 137.07533375520808
-- 237.85127101868108
-- 128.85925371868234
-- -20.31813870048381
-- 60.955340188663904
-- 64.94068162389156
-- -30.98586734122918
-- 316.40864325870433
-- 243.4731765974454
-- 125.7636127901831
-- 174.61246380962058
-- 72.91552370463228
-- 117.74987495056021
-- 89.06776397710752
-- -61.98483934620268
-- 81.44306324559632
-y:
-- -66.18292178053807
-- -115.63371289306161
-- -40.08998640911374
-- -261.31000395194786
-- -166.7909838724932
-- -139.54637670314835
-- -208.37719185679137
-- -57.717209802050384
-- -146.99926782334603
-- -94.16984989068037
-- -255.50464359809362
-- -67.9308770378757
-- -353.4014028425089
-- -168.20396034869532
-- -191.70073210680513
-- -36.41025520518381
-- -258.97388466685254
-- -239.09393621075688
-- -215.75428259623223
-- -99.96184009589246
-- -117.46963703545366
-- -16.350110272823727
-- -234.7905478492251
-- -165.3589026036757
-- 0.41946019958205855
-- -115.11125087827226
-- -147.41853651364576
-- -103.74454769846525
-- -140.90619141642424
-- -201.40554357493755
-- -341.71306618260576
-- -47.520601996709544
-- 8.723661160614721
-- -304.5231100867443
-- -12.198570874493013
-- -200.07187414553692
-- -89.68284195206441
-- -250.5605909355231
-- -276.7974106271435
-- -80.99692915874027
-- -136.66654586506084
-- -51.79816062682904
-- -85.62445575252828
-- -262.1957398217536
-- -193.5124451059287
-- -205.26576800249143
-- -138.34121902448936
-- -71.9445712933336
-- -22.608517251998556
-- -94.87504568074831
-- -127.96760156077839
-- -173.48834175964086
-- -196.745733980285
-- -159.36858356085776
-- -80.60888792369248
-- -468.1650837655404
-- -125.51666316581834
-- 10.712475760469315
-- -5.411213154927566
-- 96.82693230399087
-- -41.911224794493464
-- 22.762124542131428
-- -98.85987185081449
-- -28.459453910768794
-- 47.521581958349294
-- -248.36345727055203
-- -167.45029857022115
-- 128.28266410089716
-- 57.27033034845042
-- -58.82963578612552
-- -78.48792428054871
-- 67.26178698214157
-- -50.94569383530171
-- 56.58236082078612
-- -120.18656749761499
-- -100.11721665267609
-- -101.31410997999905
-- -55.53205860919838
-- 100.88786413431473
-- -66.82830491525722
-- 118.40778297185312
-- 68.27470664018782
-- 18.77035395820749
-- 38.56560723006115
-- 108.36119035325538
-- 2.505752009999138
-- 95.48861783590644
-- -86.62410294454978
-- 62.88822437860383
-- 8.45668376037163
-- 74.09636149418711
-- -154.22210970764675
-- -33.253355796991094
-- 247.37717511204832
-- 143.07800461797132
-- -78.2684811293491
-- 51.571400492195494
-- -132.71704558130395
-- -13.784941989330031
-- 146.01977475651552
-- 39.56660609935573
-- 113.70599705854998
-- 77.3503599700983
-- 4.444035442699823
-- 55.18774963618358
-- 42.463739509132644
-- 123.39346732558771
-- -54.62464266152292
-- 187.43447087945583
-- 44.22944044489332
-- 73.45213044828706
-- 20.761036793462168
-- -108.08520198682515
-- 176.7604941815522
-- 51.5867519537819
-- 163.86498409750413
-- 146.4850396263265
-- 203.95973954751176
-- 68.72742030684795
-- 261.21759885320284
-- -7.866632151184333
-- 163.9094272594558
-- 234.0995403976671
-- 118.97240645672969
-- 114.44267554008135
-- 133.6913309297981
-- 269.8763973140816
-- 272.9466815796491
-- 99.81503504177155
-- 296.59531963384484
-- 17.92059475315728
-- -16.90459184109093
-- 179.07991534518297
-- 354.0866295072622
-- 137.07533375520808
-- 237.85127101868108
-- 128.85925371868234
-- -20.31813870048381
-- 60.955340188663904
-- 64.94068162389156
-- -30.98586734122918
-- 316.40864325870433
-- 243.4731765974454
-- 125.7636127901831
-- 174.61246380962058
-- 72.91552370463228
-- 117.74987495056021
-- 89.06776397710752
-- -61.98483934620268
-- 81.44306324559632
-z:
-- 206.98447500965165
-- 107.8294028130727
-- 127.70627060298409
-- 141.06488888291858
-- -15.767558936324034
-- 104.98876408345981
-- 5.876224779897572
-- -156.24625539731528
-- -2.138097040417299
-- 127.13165841939589
-- 76.15459030446152
-- 50.62623211621471
-- 9.119504854677466
-- 161.5643353673217
-- -57.99700273904999
-- -32.27230503978612
-- -83.14731785803305
-- -15.175583056023067
-- 138.97949739911908
-- 41.44931524359774
-- -104.48621776700901
-- -184.41298252085062
-- 158.59376931867337
-- 245.7816880131291
-- 108.7612260328278
-- 58.93201120538486
-- 192.36588072885687
-- -56.5774733003704
-- 104.61593660612346
-- 43.29415695030378
-- 255.3923695434989
-- 132.63037432824035
-- 136.27528838503292
-- 135.1001577502892
-- 195.8501320161921
-- 132.10821361172376
-- 203.36045633476928
-- 167.100557680537
-- 324.5367362072095
-- 118.97279391613657
-- 70.48910814642515
-- 79.43194310400982
-- 120.72703615900545
-- 161.0617461663875
-- 187.02078690130878
-- -39.09418401746164
-- 201.29432195272426
-- 408.1755312763556
-- 334.64368728285683
-- 448.61478103938055
-- 185.06747764565623
-- 285.3909299656516
-- 304.80006984524545
-- 339.07366901303755
-- 277.1096903094583
-- 305.7648810698007
-- 312.8797610167065
-- 297.10552884618846
-- 429.46195026307305
-- 339.901727499861
-- 342.67179256842775
-- 415.8940569913967
-- 329.1228191731202
-- 260.3604320498785
-- 312.0300605531544
-- 77.40047377404535
-- 175.2247417607782
-- 290.631126753723
-- 153.06476762959315
-- 169.29995545563162
-- 418.0612051484701
-- 251.0493006899274
-- 357.5688644492346
-- 280.32211179723
-- 82.14656190493739
-- 503.0296295803606
-- 496.86752838427554
-- 429.7246679629917
-- 194.17299708811746
-- 243.71482842988723
-- 227.14613734687023
-- 227.4711869426846
-- 258.99755010599637
-- 110.97686599389304
-- 185.5664968429427
-- 420.31118153511665
-- 231.8822607074187
-- 309.81649311171697
-- 175.2254131846928
-- 179.2425988729483
-- 136.3979622202208
-- 99.79411510666263
-- 93.48973929992295
-- 109.74382336899727
-- 153.2673809862572
-- 33.70494904998594
-- 88.8129610229828
-- 122.22715562477637
-- 222.00679498214728
-- 230.69968267904997
-- 116.29246653078391
-- 246.87224523938258
-- 158.56768699833208
-- 9.116712893784213
-- 26.284223011217062
-- 175.98358193409112
-- 254.6037023245449
-- 163.13151676047016
-- 187.81788206849762
-- 64.42658973303092
-- 54.22239279380908
-- 148.48615044959578
-- -7.525251620456199
-- -17.583321211299438
-- 29.239331317104764
-- 60.17734941116717
-- -1.6246593546865153
-- 252.72611240296132
-- 76.80778332476146
-- -35.10657574865937
-- 0.15266081527046924
-- 151.53744299599742
-- 140.21158189869433
-- 308.587107767958
-- 180.10815121835486
-- 202.5833415734632
-- 268.8110941119751
-- 33.00458462815347
-- 247.14823748146384
-- 57.486615393927025
-- 306.63228759618227
-- 122.54878915217395
-- 44.552638000239895
-- 205.18443291082852
-- 270.7512434552188
-- -99.16085824948928
-- 182.44998131671272
-- 131.24849717166114
-- 135.6012176624329
-- 41.77685022596228
-- 71.26092745432041
-- 244.9924636858453
-- 170.2397777788086
-- 28.33663036933796
-- 123.17452269069157
-- 118.51957486669377
-- -19.85332562927001
-- 23.50119531752805
-- 54.07588641447143
-- -91.07200624607479
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
deleted file mode 100644
index ebb2aaa7b..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_2100iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- 43.8689624517487
-- -169.6007983643741
-- 30.065101369218713
-- -213.80194456204015
-- -177.86818599720698
-- -89.47729250601859
-- -237.26126603270572
-- -28.90895178468717
-- -241.97378181205764
-- 28.751410237221332
-- -279.8852663973567
-- -34.97992192624139
-- -364.0200198040815
-- -146.56388627682088
-- -235.51843671259178
-- -67.9826828007947
-- -300.9679613175251
-- -266.01731940042015
-- -160.40686421128942
-- -31.917032713536337
-- -228.17916917278487
-- -67.52265418325968
-- -279.93543505386316
-- -99.28769208154215
-- -41.73964330325352
-- -14.03180281400123
-- -164.09225196361115
-- -95.27620682960934
-- -169.03212195117646
-- -243.22225481386909
-- -438.3874977112839
-- -137.38476119639145
-- 50.83280989998028
-- -385.52775451727695
-- -115.00777420022197
-- -137.58527756048275
-- -158.91692533593877
-- -176.8491552727091
-- -322.2574538030059
-- -137.64920778885318
-- -89.03812059637349
-- -216.73854792889284
-- -123.12538039931468
-- -222.69966954734372
-- -132.56932758571173
-- -204.80667799423975
-- -206.15111386654274
-- -8.105410990393208
-- 69.05680558576898
-- -217.54000016851208
-- -105.40398832162322
-- -168.66374801089185
-- -181.10928685990706
-- -162.38012010804493
-- -157.18749108736537
-- -497.1532400025114
-- -81.47159264942029
-- 40.60934477919244
-- 72.52028937802565
-- 211.11407870514296
-- 31.925044889424697
-- -15.448604613922663
-- -108.71351956568382
-- -5.745009893067267
-- 87.6675158159406
-- -296.0279182582153
-- -233.9195781182937
-- 150.14900502415634
-- 50.047713504120075
-- -65.13712699110657
-- -63.29650156639849
-- 146.30639158700484
-- -7.420242089908876
-- 124.38604030184669
-- -225.0918022855327
-- -11.955499092061057
-- -54.97338692339286
-- 50.32031575510728
-- 112.82062511013245
-- -129.72867213328516
-- 136.89085303094504
-- 131.93641239214193
-- 32.05205211041663
-- 154.59836057722606
-- 175.67604024475781
-- -44.92837657248064
-- 81.00467628120519
-- -74.62431053956641
-- 27.537976110327044
-- 42.12871767611807
-- 44.09646445293466
-- 34.3061140292123
-- -79.67991805057657
-- 361.0179974424243
-- 206.94658977362258
-- -62.57961916655771
-- 56.35284096585629
-- -37.90639366438754
-- -34.92683123949176
-- 175.92536801389954
-- -22.202732748075075
-- 157.76726874910136
-- 156.7588267025609
-- 90.1317401095603
-- 168.47447020413233
-- 83.5602166846247
-- 165.36594566744475
-- 45.16869272257553
-- 155.02180181871185
-- 14.638975583831082
-- 94.51397248320666
-- 10.23259861337732
-- -74.05775962174776
-- 138.5976404331386
-- 35.92974930158186
-- 214.45641325560595
-- 52.395356034875604
-- 181.97934360442878
-- 158.5100111613891
-- 281.54223381715923
-- -54.62059130154339
-- 138.26745602168143
-- 360.8153239584046
-- 132.6488916217121
-- 211.33798435015814
-- 177.43241192224525
-- 303.91377149100214
-- 335.3659015411149
-- 112.4052168895307
-- 211.45162497934768
-- 26.902440056531173
-- 12.824929259779719
-- 148.78153487538145
-- 348.51675617912366
-- 80.84786703617041
-- 284.36659295579847
-- 132.89099651423604
-- 47.59218118212255
-- 123.03706687396641
-- -31.555980494697533
-- -13.830137103484992
-- 361.01370506210765
-- 271.5910348078262
-- 130.29943901622258
-- 201.50305302411329
-- 107.94428248568532
-- 148.4194735001841
-- 13.640057642855442
-- -49.82500640498612
-- 51.723330602487785
-y:
-- 43.8689624517487
-- -169.6007983643741
-- 30.065101369218713
-- -213.80194456204015
-- -177.86818599720698
-- -89.47729250601859
-- -237.26126603270572
-- -28.90895178468717
-- -241.97378181205764
-- 28.751410237221332
-- -279.8852663973567
-- -34.97992192624139
-- -364.0200198040815
-- -146.56388627682088
-- -235.51843671259178
-- -67.9826828007947
-- -300.9679613175251
-- -266.01731940042015
-- -160.40686421128942
-- -31.917032713536337
-- -228.17916917278487
-- -67.52265418325968
-- -279.93543505386316
-- -99.28769208154215
-- -41.73964330325352
-- -14.03180281400123
-- -164.09225196361115
-- -95.27620682960934
-- -169.03212195117646
-- -243.22225481386909
-- -438.3874977112839
-- -137.38476119639145
-- 50.83280989998028
-- -385.52775451727695
-- -115.00777420022197
-- -137.58527756048275
-- -158.91692533593877
-- -176.8491552727091
-- -322.2574538030059
-- -137.64920778885318
-- -89.03812059637349
-- -216.73854792889284
-- -123.12538039931468
-- -222.69966954734372
-- -132.56932758571173
-- -204.80667799423975
-- -206.15111386654274
-- -8.105410990393208
-- 69.05680558576898
-- -217.54000016851208
-- -105.40398832162322
-- -168.66374801089185
-- -181.10928685990706
-- -162.38012010804493
-- -157.18749108736537
-- -497.1532400025114
-- -81.47159264942029
-- 40.60934477919244
-- 72.52028937802565
-- 211.11407870514296
-- 31.925044889424697
-- -15.448604613922663
-- -108.71351956568382
-- -5.745009893067267
-- 87.6675158159406
-- -296.0279182582153
-- -233.9195781182937
-- 150.14900502415634
-- 50.047713504120075
-- -65.13712699110657
-- -63.29650156639849
-- 146.30639158700484
-- -7.420242089908876
-- 124.38604030184669
-- -225.0918022855327
-- -11.955499092061057
-- -54.97338692339286
-- 50.32031575510728
-- 112.82062511013245
-- -129.72867213328516
-- 136.89085303094504
-- 131.93641239214193
-- 32.05205211041663
-- 154.59836057722606
-- 175.67604024475781
-- -44.92837657248064
-- 81.00467628120519
-- -74.62431053956641
-- 27.537976110327044
-- 42.12871767611807
-- 44.09646445293466
-- 34.3061140292123
-- -79.67991805057657
-- 361.0179974424243
-- 206.94658977362258
-- -62.57961916655771
-- 56.35284096585629
-- -37.90639366438754
-- -34.92683123949176
-- 175.92536801389954
-- -22.202732748075075
-- 157.76726874910136
-- 156.7588267025609
-- 90.1317401095603
-- 168.47447020413233
-- 83.5602166846247
-- 165.36594566744475
-- 45.16869272257553
-- 155.02180181871185
-- 14.638975583831082
-- 94.51397248320666
-- 10.23259861337732
-- -74.05775962174776
-- 138.5976404331386
-- 35.92974930158186
-- 214.45641325560595
-- 52.395356034875604
-- 181.97934360442878
-- 158.5100111613891
-- 281.54223381715923
-- -54.62059130154339
-- 138.26745602168143
-- 360.8153239584046
-- 132.6488916217121
-- 211.33798435015814
-- 177.43241192224525
-- 303.91377149100214
-- 335.3659015411149
-- 112.4052168895307
-- 211.45162497934768
-- 26.902440056531173
-- 12.824929259779719
-- 148.78153487538145
-- 348.51675617912366
-- 80.84786703617041
-- 284.36659295579847
-- 132.89099651423604
-- 47.59218118212255
-- 123.03706687396641
-- -31.555980494697533
-- -13.830137103484992
-- 361.01370506210765
-- 271.5910348078262
-- 130.29943901622258
-- 201.50305302411329
-- 107.94428248568532
-- 148.4194735001841
-- 13.640057642855442
-- -49.82500640498612
-- 51.723330602487785
-z:
-- 158.02213462044517
-- 124.21389942705694
-- 133.42160238652866
-- 158.42868516085107
-- -21.605406151859455
-- 32.169365188894844
-- -22.124665667361427
-- -284.3370553173801
-- -49.996420145893985
-- 45.18025799900077
-- 70.83272531068998
-- -30.190086637370335
-- 83.32096337971734
-- 167.53181703318776
-- -56.91344159005577
-- -16.880691170276958
-- -31.641226612394295
-- -63.77736018646692
-- 155.3404382904853
-- 53.1641961123058
-- -136.70884568378537
-- -142.14313622713573
-- 54.81400336265734
-- 166.03318193784835
-- 167.03672828780103
-- 12.758532063157585
-- 262.88270654379556
-- -110.29633686990788
-- 117.26884845255185
-- -61.95240388905076
-- 226.37842481589607
-- 222.09402805357576
-- 69.76039970545094
-- 136.86308681331556
-- 137.98500997477788
-- 247.44695640637488
-- 199.8026983217359
-- 314.96927404655963
-- 337.6905947836172
-- 133.27985509039476
-- 147.8521629286081
-- 66.47742441820357
-- 147.76351959903582
-- 190.29003926246511
-- 27.071471212349145
-- -76.8549182147727
-- 286.69501455745836
-- 428.2931492491618
-- 455.61401541707056
-- 521.6731182773849
-- 109.90737231406615
-- 194.39719341953102
-- 339.82207247025315
-- 396.69340854103774
-- 329.3215784934387
-- 299.6469290240797
-- 376.28771103406586
-- 322.5336318152405
-- 525.1208758110637
-- 359.3174665937067
-- 398.69234294972256
-- 482.49576100907564
-- 415.0046037146592
-- 297.69696975229544
-- 361.95274697190786
-- 86.12408361100988
-- 248.16979285039596
-- 294.56545489386446
-- 195.02181086263064
-- 173.81987631166245
-- 485.50692106767747
-- 347.0394720125073
-- 465.59691510544513
-- 340.82681953920854
-- 147.1759691610624
-- 540.8715496577842
-- 577.5678960225167
-- 366.936611212867
-- 202.8999194336259
-- 300.499562679048
-- 297.7028917520312
-- 313.89302244023196
-- 334.60008641069373
-- 107.74468204194072
-- 235.0327715466816
-- 549.289811008155
-- 271.174000270851
-- 294.943792117891
-- 158.55887022795858
-- 160.42922276113836
-- 238.40341330861514
-- 134.30051227200818
-- 20.276647871798527
-- 79.73564021369596
-- 108.04656684276075
-- 96.98113557799664
-- -9.613618858526026
-- 231.7294599200759
-- 286.24011800776145
-- 258.9524745272242
-- 138.96205439286143
-- 230.5178459277302
-- 129.21981541108664
-- 34.94829644138465
-- -47.36503948695072
-- 258.47393124139313
-- 288.0921191562058
-- 99.35596410276497
-- 199.08849267441343
-- 5.962739108937392
-- 57.277582923395016
-- 198.9737245497959
-- -29.250459745576855
-- -35.371226355851526
-- -50.93861828903449
-- -0.5622942134144645
-- 51.2659317519989
-- 163.80067980518817
-- 100.69134696855404
-- 41.28379929101864
-- 44.394393991420785
-- 74.10606750833206
-- 210.39197419780737
-- 257.7009656039906
-- 143.52042324845172
-- 157.95281778355042
-- 371.0355274713962
-- 117.10681489657816
-- 183.5689087876269
-- 113.25094688461957
-- 321.50019921313026
-- 135.28688690325083
-- 109.65516438008626
-- 309.8506415075863
-- 297.8014922160356
-- -125.94596534196043
-- 227.3820448805529
-- 81.85741963127256
-- 168.77238219848283
-- 106.49759132079056
-- 77.40044523712592
-- 278.81113256155186
-- 186.02603672590124
-- 29.11334048494735
-- 74.6223815549481
-- 69.06428438760543
-- -26.82167216354576
-- -28.709326664936647
-- 69.74118428651663
-- -7.377070293044171
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
deleted file mode 100644
index 11b7bae7a..000000000
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/config/trajectory_weights/learned_smoothed_obstacle_avoiding_weights_800iters.yaml
+++ /dev/null
@@ -1,903 +0,0 @@
-x:
-- -29.375551359078376
-- -90.47443500639149
-- -91.71188550404756
-- -189.32305379289966
-- -235.83766720876812
-- -169.72239313636325
-- -149.43256088322323
-- -80.20600645446291
-- -118.55368553718105
-- 42.81215178550607
-- -265.9957636585939
-- -141.95701797617795
-- -266.46167767655487
-- -204.51014475871997
-- -189.80222697311726
-- -38.05749645329762
-- -223.70928226903334
-- -288.62220421304863
-- -220.73144672582546
-- -166.93529723196053
-- -87.8046234522134
-- -122.26709774619059
-- -208.24287361309038
-- -236.1220882073268
-- -122.64068051628453
-- -75.01958163184389
-- -204.96615380578407
-- -23.09215254192919
-- -191.92413980451423
-- -175.38877684876053
-- -280.78418583823486
-- -144.7014390367135
-- -103.03003029337698
-- -299.44372380872426
-- -102.19985354546496
-- -205.1251444690762
-- -209.604136417223
-- -271.64598338749227
-- -349.0777046931662
-- -140.45821201826254
-- -132.12210271681744
-- -89.04875206339462
-- -162.95490217138203
-- -209.54656911129615
-- -237.14361983118667
-- -266.20685193865637
-- -112.69616837548827
-- -168.68025744837794
-- -82.49433733438627
-- -115.23856334171428
-- -225.20573825262352
-- -177.25605785639775
-- -272.9606105692432
-- -209.58804425623748
-- -89.74444994088834
-- -422.6129708309914
-- -149.80385648598852
-- -76.34902279256217
-- -35.59435466166301
-- -28.379183376609024
-- -26.040840385426367
-- -23.411215764621712
-- -246.28832759393669
-- -149.42109564818247
-- 19.638873302406513
-- -247.7719621044008
-- -148.67588307949012
-- -6.819115922872016
-- -42.10929537610118
-- -86.51608561145373
-- 38.745190729243845
-- 73.44615661851579
-- -43.57206718249474
-- 59.22210318409091
-- -120.49920562371088
-- -72.20181047344533
-- -108.08988417914402
-- -75.7450387838097
-- -21.076801297481534
-- -132.65414296232717
-- 48.21738504635631
-- -12.08739295872503
-- -16.230166084301146
-- 9.739221457638676
-- 74.15950234946249
-- -43.841233195862
-- 80.17355341740009
-- -106.34374023750392
-- -21.883107257731865
-- -97.08353131548583
-- 41.03629608726767
-- -101.10630613756221
-- -83.942009736159
-- 192.5212933831941
-- 104.57687178131494
-- -35.98987118013184
-- 17.173769075202546
-- -120.23176589621691
-- -60.84712779673432
-- 1.9327942318091853
-- 29.001885322844586
-- 125.08479299949121
-- 82.45353895679158
-- -15.42840105515149
-- 27.276169375364905
-- 30.07990368176639
-- 172.91643634668338
-- -7.482340203943173
-- 157.59112514236313
-- -137.13366773892676
-- 95.53954085681873
-- 19.49350757622742
-- -57.713229800924424
-- 83.02661866355157
-- 85.08031959692558
-- 157.06008111632303
-- 109.24421484963756
-- 159.04732972517277
-- 90.46862148072736
-- 247.73417368056147
-- -32.26406535057381
-- 142.23886194195728
-- 182.27466879228888
-- 27.461550877478768
-- 99.02342386475823
-- 110.61768010646794
-- 192.14809905399466
-- 196.1612891742741
-- 65.68248160996461
-- 185.67802983123008
-- 99.19332389347727
-- 87.21381730414205
-- 150.52999560895714
-- 316.6079779476388
-- 248.3318997862404
-- 108.64439049194944
-- 69.89827365386222
-- -6.690611980882549
-- 26.166971113579585
-- 115.72582217280392
-- -6.327346694173989
-- 238.32953086103095
-- 293.5383988954415
-- 181.85731011937267
-- 66.33790674489546
-- 112.88452797859479
-- 14.829331008214357
-- 73.80993361645798
-- -58.840870314538606
-- 92.69210473123687
-y:
-- -29.375551359078376
-- -90.47443500639149
-- -91.71188550404756
-- -189.32305379289966
-- -235.83766720876812
-- -169.72239313636325
-- -149.43256088322323
-- -80.20600645446291
-- -118.55368553718105
-- 42.81215178550607
-- -265.9957636585939
-- -141.95701797617795
-- -266.46167767655487
-- -204.51014475871997
-- -189.80222697311726
-- -38.05749645329762
-- -223.70928226903334
-- -288.62220421304863
-- -220.73144672582546
-- -166.93529723196053
-- -87.8046234522134
-- -122.26709774619059
-- -208.24287361309038
-- -236.1220882073268
-- -122.64068051628453
-- -75.01958163184389
-- -204.96615380578407
-- -23.09215254192919
-- -191.92413980451423
-- -175.38877684876053
-- -280.78418583823486
-- -144.7014390367135
-- -103.03003029337698
-- -299.44372380872426
-- -102.19985354546496
-- -205.1251444690762
-- -209.604136417223
-- -271.64598338749227
-- -349.0777046931662
-- -140.45821201826254
-- -132.12210271681744
-- -89.04875206339462
-- -162.95490217138203
-- -209.54656911129615
-- -237.14361983118667
-- -266.20685193865637
-- -112.69616837548827
-- -168.68025744837794
-- -82.49433733438627
-- -115.23856334171428
-- -225.20573825262352
-- -177.25605785639775
-- -272.9606105692432
-- -209.58804425623748
-- -89.74444994088834
-- -422.6129708309914
-- -149.80385648598852
-- -76.34902279256217
-- -35.59435466166301
-- -28.379183376609024
-- -26.040840385426367
-- -23.411215764621712
-- -246.28832759393669
-- -149.42109564818247
-- 19.638873302406513
-- -247.7719621044008
-- -148.67588307949012
-- -6.819115922872016
-- -42.10929537610118
-- -86.51608561145373
-- 38.745190729243845
-- 73.44615661851579
-- -43.57206718249474
-- 59.22210318409091
-- -120.49920562371088
-- -72.20181047344533
-- -108.08988417914402
-- -75.7450387838097
-- -21.076801297481534
-- -132.65414296232717
-- 48.21738504635631
-- -12.08739295872503
-- -16.230166084301146
-- 9.739221457638676
-- 74.15950234946249
-- -43.841233195862
-- 80.17355341740009
-- -106.34374023750392
-- -21.883107257731865
-- -97.08353131548583
-- 41.03629608726767
-- -101.10630613756221
-- -83.942009736159
-- 192.5212933831941
-- 104.57687178131494
-- -35.98987118013184
-- 17.173769075202546
-- -120.23176589621691
-- -60.84712779673432
-- 1.9327942318091853
-- 29.001885322844586
-- 125.08479299949121
-- 82.45353895679158
-- -15.42840105515149
-- 27.276169375364905
-- 30.07990368176639
-- 172.91643634668338
-- -7.482340203943173
-- 157.59112514236313
-- -137.13366773892676
-- 95.53954085681873
-- 19.49350757622742
-- -57.713229800924424
-- 83.02661866355157
-- 85.08031959692558
-- 157.06008111632303
-- 109.24421484963756
-- 159.04732972517277
-- 90.46862148072736
-- 247.73417368056147
-- -32.26406535057381
-- 142.23886194195728
-- 182.27466879228888
-- 27.461550877478768
-- 99.02342386475823
-- 110.61768010646794
-- 192.14809905399466
-- 196.1612891742741
-- 65.68248160996461
-- 185.67802983123008
-- 99.19332389347727
-- 87.21381730414205
-- 150.52999560895714
-- 316.6079779476388
-- 248.3318997862404
-- 108.64439049194944
-- 69.89827365386222
-- -6.690611980882549
-- 26.166971113579585
-- 115.72582217280392
-- -6.327346694173989
-- 238.32953086103095
-- 293.5383988954415
-- 181.85731011937267
-- 66.33790674489546
-- 112.88452797859479
-- 14.829331008214357
-- 73.80993361645798
-- -58.840870314538606
-- 92.69210473123687
-z:
-- 211.2449638813512
-- 144.12643508434022
-- 70.65084772360636
-- 266.22259058351193
-- 79.58098722163406
-- 67.41137999087195
-- 53.22696167140256
-- -155.62230486888396
-- -20.637501526335246
-- 47.29218781689232
-- 137.74499579919055
-- 29.737631923863358
-- 22.265927207340653
-- 134.96071582169628
-- 12.867836720057458
-- 31.746080143319816
-- -61.25340292907621
-- 16.095025201104544
-- 146.4098832489993
-- 40.848615079904334
-- -50.14723555612424
-- -138.43221895490484
-- 153.59666936708396
-- 204.93592122102345
-- 148.309348728097
-- 132.40599809620417
-- 96.32049687526244
-- 14.391642011065146
-- 126.11541675235092
-- 98.49868770999424
-- 227.42842519198672
-- 128.45790771023644
-- 111.23880059118348
-- 137.71166622530953
-- 202.20346380741577
-- 74.30635938312481
-- 211.12787550053028
-- 179.57731430777284
-- 232.0547769473891
-- 106.31587572443996
-- 56.91595541471525
-- 164.6658069280793
-- 105.45001283843736
-- 121.9916460272607
-- 166.56172537885004
-- 43.101383902483924
-- 177.9179920479384
-- 350.4344815921115
-- 273.82271685178284
-- 345.753927164644
-- 205.74782564547337
-- 95.8509054019219
-- 349.4294217304583
-- 292.26509190007056
-- 202.08072722544105
-- 279.18674353150703
-- 307.05731255330056
-- 256.0348143653611
-- 352.8725202764028
-- 342.3067330714769
-- 329.85645000425404
-- 331.2448078046214
-- 261.8592784241704
-- 223.12260579613678
-- 229.7406559910389
-- 106.71386980499686
-- 225.70066674342786
-- 232.960637316935
-- 125.3723581420407
-- 190.07607611067053
-- 384.80580145715317
-- 211.87101947223573
-- 388.5322145790263
-- 251.75754271449767
-- 163.80291231609192
-- 403.4985442367527
-- 407.9914837009331
-- 348.4985939500316
-- 217.94056440429313
-- 186.13163981629907
-- 214.8689491665111
-- 187.73133261536157
-- 152.4888749587075
-- 129.9939226790654
-- 185.17672711886533
-- 420.48693538484144
-- 210.68106693570417
-- 216.2355938232026
-- 151.18380118094063
-- 177.5903345689017
-- 118.42270264251937
-- 134.75396155303804
-- 119.2231629624467
-- 120.5135786586908
-- 99.0081517922155
-- 44.340956839597816
-- 88.5756716853682
-- 146.37757087986753
-- 191.7854607520014
-- 116.84831300929238
-- 102.1535340161195
-- 183.49767166335988
-- 110.26972529822831
-- 31.855645025076026
-- 104.47232402972293
-- 105.16661554418253
-- 186.7668231442927
-- 107.25810511264469
-- 149.91517473659937
-- 67.54777348159766
-- 112.98835500793635
-- 160.24636088290956
-- -9.135489008559233
-- 76.2158660273898
-- 75.39618399235141
-- 115.81860406030827
-- -94.71479356756392
-- 180.49611085352274
-- 124.42826449811837
-- -52.797664491975596
-- 77.4224115843028
-- 99.21542756265629
-- 104.7404776249072
-- 163.9225564943106
-- 128.76984457451476
-- 164.02881762648596
-- 222.10664278936633
-- 54.19271402906178
-- 206.07507379901463
-- 113.56828585758434
-- 297.4088031293355
-- 138.71506972040604
-- 11.133786666699164
-- 245.97664524914433
-- 254.27210332312245
-- -14.460600520764679
-- 104.53854381236779
-- 82.05872014286024
-- 42.73723160438324
-- 29.0544071318862
-- 36.90402358428645
-- 161.02262155535433
-- 120.86832719606659
-- -16.647004317176723
-- 134.56200411183218
-- 80.3362373679378
-- -70.14863660353376
-- 38.133373393847464
-- 133.55911624440668
-- -75.3184987249469
-pitch: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-roll: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]
-yaw: [-156.65718192265612,
- -156.82512813476856,
- -157.03476785128979,
- -157.2807477120766,
- -157.55369295460645,
- -157.84461759860156,
- -158.14778763059294,
- -158.459737944698,
- -158.77686003690096,
- -159.09532479965736,
- -159.41325989374954,
- -159.73163715474024,
- -160.0521375845057,
- -160.3745530176387,
- -160.69700732848457,
- -161.0185104942475,
- -161.3402653767862,
- -161.66397674111627,
- -161.98953153448417,
- -162.315201866925,
- -162.64003295077077,
- -162.96511555690367,
- -163.29205326830424,
- -163.62078022025358,
- -163.9496971026068,
- -164.27788257902984,
- -164.60632279261657,
- -164.93652376742696,
- -165.26846111422094,
- -165.60065680035294,
- -165.93222401150882,
- -166.26405206032558,
- -166.5975532366255,
- -166.9327393757321,
- -167.2682466574162,
- -167.60322357479959,
- -167.93847000443222,
- -168.27530835178302,
- -168.61378183900877,
- -168.95263403217712,
- -169.29104925446183,
- -169.62974494472823,
- -169.9699574813401,
- -170.31175703012775,
- -170.6539879618333,
- -170.99587071205337,
- -171.33804689251744,
- -171.68167070253227,
- -172.02683518428148,
- -172.37247918025702,
- -172.71785930237908,
- -173.06354756695924,
- -173.4106198177865,
- -173.75918826314913,
- -174.10828013599635,
- -174.45718809095524,
- -174.8064204116356,
- -175.1569783712803,
- -175.50898997238295,
- -175.86156501042154,
- -176.21403187168687,
- -176.56684061134436,
- -176.9209216656706,
- -177.276415779213,
- -177.63250973601328,
- -177.9885671847553,
- -178.34498510912093,
- -178.7026267789968,
- -179.0616429301755,
- -179.42129201479344,
- -179.7809723347144,
- -180.14103262348885,
- -180.50227258176474,
- -180.86485046896803,
- -181.2280913368986,
- -181.59142740879287,
- -181.95516366594228,
- -182.3200397542152,
- -182.6862192544363,
- -183.0530889992954,
- -183.42011429540196,
- -183.78756055865932,
- -184.15611080378343,
- -184.525931978699,
- -184.8964681246396,
- -185.2672167028437,
- -185.638407452449,
- -186.01067008275334,
- -186.3841731854121,
- -186.75841368027824,
- -187.132920178221,
- -187.50789034493104,
- -187.88390380611247,
- -188.26112928818085,
- -188.6391124973969,
- -189.01741212654568,
- -189.39619709895,
- -189.7760000696095,
- -190.15698858912273,
- -190.53875329031354,
- -190.92088183004392,
- -191.30351746122224,
- -191.68714886802226,
- -192.07194129758645,
- -192.4575266759198,
- -192.8435204676572,
- -193.23004308121793,
- -193.617542113636,
- -194.0061795490326,
- -194.39562519327265,
- -194.78552113473754,
- -195.17596753027587,
- -195.56737365493905,
- -195.95989742408074,
- -196.35324332333818,
- -196.74707886293638,
- -197.14148632095296,
- -197.53683929553736,
- -197.933290967728,
- -198.33057750888972,
- -198.7283906402854,
- -199.12679692660544,
- -199.52613681329163,
- -199.92655820874447,
- -200.32782617456402,
- -200.72965543146975,
- -201.13209880120448,
- -201.53546597968077,
- -201.93989917925003,
- -202.34518974707686,
- -202.751074198292,
- -203.1575933993827,
- -203.56502857939287,
- -203.97351593447695,
- -204.38287067560304,
- -204.7928499203278,
- -205.20348419671345,
- -205.6150284301484,
- -206.0276125727257,
- -206.44107345232243,
- -206.85518761577225,
- -207.26997671022124,
- -207.68567140275596,
- -208.10239525551458,
- -208.5200046331361,
- -208.93829436247796,
- -209.35727851912242,
- -209.77716544140384,
- -210.19807222793216,
- -210.61987285855759]

From 9acbea23e23e81d6ec5f3f4dea742c241805b38d Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 12 Mar 2020 16:59:06 +0100
Subject: [PATCH 019/264] [hand-over-action] Add boolean release detection
 parameter, and some code clean-up

---
 .../ros/action/HandOver.action                |   1 +
 .../ros/launch/hand_over.launch               |   2 +-
 .../ros/scripts/hand_over_action_client_test  |   5 +-
 .../src/mdr_hand_over_action/action_states.py | 129 ++++++------------
 4 files changed, 47 insertions(+), 90 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
index 3db6f3316..96fbb520b 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
@@ -1,6 +1,7 @@
 # goal
 string posture_type
 bool obstacle
+bool release_detection
 bool context_aware
 ---
 # result
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index cf0fc390a..4144631a1 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -5,7 +5,7 @@
         <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
         <param name="init_config_name" value="neutral" />
         <param name="hand_over_dmp" value="grasp.yaml" />
-        <param name="hand_over_position_policy_parameters_file" value="800.pkl" />
+        <param name="hand_over_position_policy_parameters_file" value="learned_position_policy_parameters.pkl" />
         <param name="hand_over_policy_config_dir" value="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
         <param name="hand_over_dmp_weights_dir" value="$(find mdr_hand_over_action)/config/trajectory_weights/" />
         <param name="dmp_tau" value="30" />
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
index fd8394728..ab29e0609 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
@@ -11,7 +11,8 @@ from mdr_hand_over_action.msg import HandOverAction, HandOverGoal
 if __name__ == '__main__':
     posture_contexts = ['standing', 'seated', 'lying']
     obstacle_contexts = [0, 1]                                 # 0: no obstacle, 1: obstacle
-    
+    release_detection = [0, 1]
+
     parser = argparse.ArgumentParser(description='help text')
     parser.add_argument("--context", "-c", help="list of context parameters. E.g.: ['sitting', 0]") 
     args = parser.parse_args()
@@ -32,6 +33,7 @@ if __name__ == '__main__':
     else:
         posture_context = context_params[0]
         obstacle_context = bool(context_params[1])
+        release_detection = 1
 
         rospy.init_node('hand_over_action_client_test')
 
@@ -43,6 +45,7 @@ if __name__ == '__main__':
         rospy.loginfo('Setting goal for a %s person, with %s obstacle in-between...' % (posture_context, 'an' if obstacle_context else 'no'))
         goal.posture_type = posture_context
         goal.obstacle = obstacle_context
+        goal.release_detection = release_detection
 
         timeout = 15.0
         rospy.loginfo('Sending action lib goal to hand_over_server')
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 8c0bdae7f..8ef0e77fe 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -26,7 +26,7 @@ def __init__(self, timeout=120.0,
                  move_arm_server='move_arm_server',
                  init_config_name = 'neutral',
                  hand_over_policy_config_dir='',
-                 hand_over_position_policy_parameters_file = '800_py2.pkl',
+                 hand_over_position_policy_parameters_file = 'learned_position_policy_parameters.pkl',
                  hand_over_dmp_weights_dir='',
                  hand_over_dmp = 'grasp.yaml',
                  dmp_tau = 30.,
@@ -47,18 +47,14 @@ def __init__(self, timeout=120.0,
 
         self.hand_over_dmp = hand_over_dmp
         self.dmp_tau = dmp_tau
-        # self.hand_over_position_policy_parameters_file = hand_over_position_policy_parameters_file
-        # self.hand_over_position_policy_parameters_file = 'contextHOs_160iters-Sigma_restarts_at_10s, eps:1, N:40, Con:x-diag, Goals:(Se:[0.5, 0.078, 0.7], Ly:[0.7, 0.078, 0.7], St:[0.4, 0.078, 1.0]).pkl'
-        self.hand_over_position_policy_parameters_file = join(self.hand_over_policy_config_dir, '160_py2.pkl')
+        self.hand_over_position_policy_parameters_file = join(self.hand_over_policy_config_dir, hand_over_position_policy_parameters_file)
         self.hand_over_position_policy_parameters = self.load_policy_params_from_file(self.hand_over_position_policy_parameters_file)
 
         self.tf_listener = tf.TransformListener()
         self.force_sensor_topic = force_sensor_topic
 
         self.latest_force_measurement_x = 0.
-        self.latest_force_measurement_z = 0.
         self.cumsum_x = 0
-        self.cumsum_z = 0
         self.force_detection_threshold = 1.
         self.object_reception_detected = False
 
@@ -74,23 +70,19 @@ def init(self):
         return FTSMTransitions.INITIALISED
 
     def running(self):
-        posture_type = self.goal.posture_type
-        obstacle = self.goal.obstacle
-        context_aware = self.goal.context_aware
-
         hand_over_pose = PoseStamped()
         hand_over_pose.header.frame_id = 'base_link'
         hand_over_pose.header.stamp = rospy.Time.now()
-        if context_aware:
+        if self.goal.context_aware:
             # > Pick context-dependent hand-over position:
             policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
             policy_parameter_A = self.hand_over_position_policy_parameters[0][1]
 
-            if posture_type == 'standing':
+            if self.goal.posture_type == 'standing':
                 context_vector = np.array([0.0, 0.0, 0.4])
-            elif posture_type == 'seated':
+            elif self.goal.posture_type == 'seated':
                 context_vector = np.array([0.5, 0.0, 0.0])
-            elif posture_type == 'lying':
+            elif self.goal.posture_type == 'lying':
                 context_vector = np.array([0.0, 0.7, 0.0])
         
             # Sample upper-level policy (with no exploration) for hand-over position;
@@ -114,87 +106,60 @@ def running(self):
         pose_base_link = self.tf_listener.transformPose('base_link', hand_over_pose)
 
         # > Pick context_dependent hand-over trajectory shape:
-        if not obstacle:
+        if not self.goal.obstacle:
             trajectory_weights_filename = 'grasp.yaml'
         else:
+            ## Uncomment one of the following if testing other learned parameters is desired:
+            
             ## Initial learned trajectory:
             # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml'
             
-            ## Smoothed trajectories:
-            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2900iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_2100iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_1600iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights_800iters.yaml'
-
-            ## Smoothed trajectories, with corrected end_points:
-            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2900iters.yaml'
-            trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_2100iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_1600iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights_800iters.yaml'
-
-            ## Underhand trajectories:
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_7700iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_6900iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_6100iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_5300iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_4500iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_3700iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_2900iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_2100iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_1600iters.yaml'
-            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights_800iters.yaml'
+            ## Smoothed learned trajectory:
+            # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights.yaml'
 
+            ## Learned underhand trajectory:
+            # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights.yaml'
+
+            ## Smoothed learned trajectory, with corrected end_points:
+            trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights.yaml'
 
         self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename)
 
         ## TODO: determine whether aligning base is necessary for hand_over action here:
         ## ...
 
-        # # Go to pregrasp low position:
-        # rospy.loginfo('[hand_over] Moving back to pregrasp low position...')
-        # self.__move_arm(MoveArmGoal.NAMED_TARGET, 'pregrasp_low')
-
-        # # Initialize by grasping a provided object:
-        # rospy.loginfo('[hand_over] Closing the gripper')
-        # self.gripper.close() 
-
         # Move to chosen hand_over position, along appropriate trajectory:
         rospy.loginfo('[hand_over] Handing object over...')
         self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
 
-        # Naive object release strategy:
-        # # --------------------------------------------------------------
-        # # Wait for person to hold object:
-        # rospy.loginfo('[hand_over] Waiting before releasing object...')
-        # rospy.sleep(5.)
-
-        # # Release the object:
-        # rospy.loginfo('[hand_over] Opening the gripper...')
-        # self.gripper.open()
-        # #  --------------------------------------------------------------
-
-        # Force sensing object release strategy:
-        # --------------------------------------------------------------
-        # Wait for person to pull object:
-        rospy.sleep(1.)
-        rospy.loginfo('[hand_over] Waiting for object to be received...')
-        rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
-        self.object_reception_detected = False
-        self.cumsum_x = 0.
-        self.cumsum_z = 0.
-
-        start_time = rospy.get_time()
-        while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected:
-            self.detect_object_reception()
-            rospy.sleep(0.1)
+        if not release_detection:
+            # Naive object release strategy:
+            rospy.loginfo('[hand_over] Waiting before releasing object...')
+            rospy.sleep(5.)
 
-        # If a pull is detected, release the object:
-        if self.object_reception_detected:        
-            rospy.loginfo('[hand_over] Opening the gripper to release object...')
+            # Release the object:
+            rospy.loginfo('[hand_over] Opening the gripper...')
             self.gripper.open()
+
         else:
-            rospy.loginfo('[hand_over] Keeping object since no reception was detected')
-        # --------------------------------------------------------------        
+            # Force sensing object release strategy:
+            rospy.sleep(1.)
+            rospy.loginfo('[hand_over] Waiting for object to be received...')
+            rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
+            self.object_reception_detected = False
+            self.cumsum_x = 0.
+
+            start_time = rospy.get_time()
+            while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected:
+                self.detect_object_reception()
+                rospy.sleep(0.1)
+
+            # If a pull is detected, release the object:
+            if self.object_reception_detected:        
+                rospy.loginfo('[hand_over] Opening the gripper to release object...')
+                self.gripper.open()
+            else:
+                rospy.loginfo('[hand_over] Keeping object since no reception was detected')
 
         # Return to a neutral arm position:
         rospy.loginfo('[hand_over] Moving back to neutral position...')
@@ -250,23 +215,11 @@ def detect_object_reception(self):
         pdf_1 = norm(mu_1, std)
 
         self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
-        self.cumsum_z += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
 
         print(self.cumsum_x)
         if self.cumsum_x > self.force_detection_threshold:
-        # if self.cumsum_x > self.force_detection_threshold or self.cumsum_z > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
         
     def force_sensor_cb(self, force_sensor_msg):
         self.latest_force_measurement_x = force_sensor_msg.wrench.force.x
-        self.latest_force_measurement_z = force_sensor_msg.wrench.force.z
-
-        # print('[hand_over DEBUG] Force Measurements:')
-        # print('[hand_over DEBUG] Current cumsum in x:', self.cumsum_x)
-        # print('[hand_over DEBUG] Current cumsum in z:', self.cumsum_z)
-        # print("*********************")
-        # print('[hand_over DEBUG] Current Force Measurements, x:', force_sensor_msg.wrench.force.x)
-        # print('[hand_over DEBUG] Current Force Measurements, y:', force_sensor_msg.wrench.force.y)
-        # print('[hand_over DEBUG] Current Force Measurements, z:', force_sensor_msg.wrench.force.z)
-        # print("\n\n")

From 050c41538d958eb7d4eadfe204ca754e166c863e Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 12 Mar 2020 17:11:49 +0100
Subject: [PATCH 020/264] [hand-over-action] Fix incorrect release_detection
 variable naming

---
 .../ros/src/mdr_hand_over_action/action_states.py               | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 8ef0e77fe..6481984ad 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -132,7 +132,7 @@ def running(self):
         rospy.loginfo('[hand_over] Handing object over...')
         self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
 
-        if not release_detection:
+        if not self.goal.release_detection:
             # Naive object release strategy:
             rospy.loginfo('[hand_over] Waiting before releasing object...')
             rospy.sleep(5.)

From b66882f31c03452fd7ce35f3a3703851a1aaa046 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 12 Mar 2020 17:19:07 +0100
Subject: [PATCH 021/264] [hand-over-action] Fix incorrect result when object
 is released without detection

---
 .../ros/src/mdr_hand_over_action/action_states.py               | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 6481984ad..df49ba144 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -141,6 +141,8 @@ def running(self):
             rospy.loginfo('[hand_over] Opening the gripper...')
             self.gripper.open()
 
+            # Since no detection is used here, the object is released and we always set the reception to True.
+            self.object_reception_detected = True
         else:
             # Force sensing object release strategy:
             rospy.sleep(1.)

From ccbff68013d3cc4df62aacc5e8dd4a158116b7d4 Mon Sep 17 00:00:00 2001
From: Lucy <robotics@mail.inf.h-brs.de>
Date: Thu, 12 Mar 2020 19:04:54 +0100
Subject: [PATCH 022/264] [demo_context_aware_hand_over] Included the newly
 added 'release_detection' action parameter

---
 .../config/context_aware_hand_over_sm.yaml                     | 3 +++
 .../scenario_states/hand_over.py                               | 3 ++-
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
index 2d1ccec59..799419de6 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -59,3 +59,6 @@ state_descriptions:
             - argument:
                 name: context_aware
                 value: True
+            - argument:
+                name: release_detection
+                value: True
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 525c2c57b..4952c589f 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -15,6 +15,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.obstacle_present = kwargs.get('obstacle_present', False)
         self.timeout = kwargs.get('timeout', 120.)
         self.context_aware = kwargs.get('context_aware', True)
+        self.release_detection = kwargs.get('release_detection', True)
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.retry_count = 0
@@ -26,10 +27,10 @@ def execute(self, userdata):
         goal.posture_type = userdata.posture
         goal.obstacle = self.obstacle_present
         goal.context_aware = self.context_aware
+        goal.release_detection = self.release_detection
 
         # calling the actionlib server and waiting for the execution to end
         rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))
-	    # Disabled temporarily for hand-over experiments:
         self.say('Handing item over to a {0} person'.format(userdata.posture))
         self.client.send_goal(goal)
         self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))

From 7546bcbc72f369b4d2d25f00c13e065a6649572a Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 27 Mar 2020 16:09:31 +0100
Subject: [PATCH 023/264] [hand_over_action] Switch to robot-independent topics
 and packages

Requested review changes
---
 .../mdr_hand_over_action/ros/launch/hand_over.launch          | 2 +-
 .../mdr_hand_over_action/ros/scripts/hand_over_action         | 2 +-
 .../ros/src/mdr_hand_over_action/action_states.py             | 4 ++--
 .../mdr_find_people/ros/launch/find_people.launch             | 2 +-
 .../mdr_find_people/ros/src/mdr_find_people/action_states.py  | 2 +-
 5 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index 4144631a1..e5e39ad2c 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -2,7 +2,7 @@
 <launch>
     <node pkg="mdr_hand_over_action" type="hand_over_action" name="hand_over_server" output="screen">
         <param name="move_arm_server" value="move_arm_server" />
-        <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
+        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
         <param name="init_config_name" value="neutral" />
         <param name="hand_over_dmp" value="grasp.yaml" />
         <param name="hand_over_position_policy_parameters_file" value="learned_position_policy_parameters.pkl" />
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
index 81c0c88d5..46d63e3ec 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
@@ -11,7 +11,7 @@ class HandOverServer(object):
     The server expects the following parameters to be made available on the ROS parameter server:
     '''
     def __init__(self):
-        gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mas_hsr_gripper_controller')
+        gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mdr_gripper_controller')
         move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
         init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
         hand_over_dmp = rospy.get_param('~hand_over_dmp', '')
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index df49ba144..4236d1ec0 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -21,8 +21,8 @@
 
 class HandOverSM(ActionSMBase):
     def __init__(self, timeout=120.0,
-                 gripper_controller_pkg_name='mas_hsr_gripper_controller',
-                 force_sensor_topic='/hsrb/wrist_wrench/raw',
+                 gripper_controller_pkg_name='mdr_gripper_controller',
+                 force_sensor_topic='/wrench/raw',
                  move_arm_server='move_arm_server',
                  init_config_name = 'neutral',
                  hand_over_policy_config_dir='',
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
index 03befefc6..4039374f1 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="pointcloud_topic" default="/hsrb/head_rgbd_sensor/depth_registered/rectified_points"/>
+    <arg name="pointcloud_topic" default="/rectified_points"/>
 
     <node pkg="mdr_find_people" type="find_people_server" name="find_people_server" output="screen" >
         <param name="pointcloud_topic" type="string" value="$(arg pointcloud_topic)" />
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index a3f63b0d3..b9759a7bc 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -48,7 +48,7 @@ def __init__(self):
                              output_keys=['find_people_result', 'error_message'])
 
         self._listener = tf.TransformListener()
-        self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/hsrb/head_rgbd_sensor/depth_registered/rectified_points')
+        self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
 
 
     def execute(self, userdata):

From 575f8b7b809d6f90da5d4becc8b6bf91d56a1e38 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Wed, 1 Apr 2020 16:15:57 +0200
Subject: [PATCH 024/264] [hand_over_action] Update README with more detailed
 description

---
 .../mdr_hand_over_action/README.md            | 105 +++++++++++++++++-
 1 file changed, 103 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
index daca0dc62..3e525cb94 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
@@ -1,3 +1,104 @@
-# mdr_handle_open_action
+# mdr_hand_over_action
 
-An action for handing a grasped object over to a person.
\ No newline at end of file
+An action for handing a grasped object over to a person. It is intended to be adaptive to different contexts of the task, such as the receiver's posture, the presence of an intermediate obstacle, receiver attributes, etc.
+
+Currently, the action is only adaptive to postural context, through a hand-over position selection policy acquired using reinforcement learning.
+
+The context-aware object hand-over demo scenario demonstrates the intended workflow of the action, including:
+* Detecting a person
+* Determining their posture
+* Handing a grasped object over
+
+## Action definition
+
+### Goal:
+
+* ``string posture_type``: The receiver's posture: standing up, seated, or lying down
+* ``bool obstacle``: Indicates whether an obstacle is present between the robot and the receiver
+* ``bool release_detection``: Determines whether a force sensor-based object reception detection strategy is used
+* ``bool context_aware``: Determines whether knowledge about context is utilized to adapt execution
+
+### Result:
+
+* ``bool success``
+
+### Feedback:
+
+* ``string current_state``
+* ``string message``
+
+## Directory structure
+
+```
+mdr_pickup_action
+|    CMakeLists.txt
+|    package.xml
+|    setup.py
+|    README.md
+|____config
+|    |____learned_policy_parameters
+|    |    |_____learned_position_policy_parameters.pkl
+|    |    |
+|    |____trajectory_weights
+|         |     grasp.yaml
+|         |     learned_obstacle_avoiding_weights.yaml
+|         |     learned_smoothed_corrected_obstacle_avoiding_weights.yaml
+|         |     learned_smoothed_corrected_underhand_weights.yaml
+|         |_____learned_smoothed_obstacle_avoiding_weights
+|____ros
+     |____action
+     |    |_____HandOver.action
+     |    |
+     |____launch
+     |    |_____hand_over.launch
+     |    |
+     |____scripts
+     |    |     hand_over_action
+     |    |_____hand_over_client_test
+     |    |
+     |____src
+          |____mdr_hand_over_action
+               |    __init__.py
+               |____action_states.py
+```
+
+## Launch file parameters
+
+### Action server
+
+The following parameters may be passed when launching the action server:
+* ``move_arm_server``: Name of the `move_arm` action server (default: 'move_arm_server')
+* ``gripper_controller_pkg_name``: Name of a package that implements functionalities for controlling a robot's gripper (default: 'mdr_gripper_controller')
+* ``init_config_name``: Name of the initial arm configuration for object hand-overs (default: 'neutral')
+* ``hand_over_dmp``:  Path to a YAML file containing the weights of a dynamic motion primitive used for object hand-overs (default: 'grasp.yaml')
+* ``hand_over_position_policy_parameters_file``:  Name of a YAML file containing the parameters of a learned context-dependent hand-over position policy (default: 'learned_position_policy_parameters.pkl')
+* ``hand_over_policy_config_dir``: Path to a directory containing learned hand-over position policy parameters (default: '')
+* ``hand_over_dmp_weights_dir``: Path to a directory containing learned hand-over trajectory shape policy parameters (default: '')
+* ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 30)
+
+## Action execution summary
+
+It is assumed that the robot starts with the object to be handed over already in its grasp. The action proceeds through the following steps:
+1. The end-effector position at which the object will be handed over is first determined:
+  - if the ``context_aware`` goal parameter is set to ``True``, a position is sampled from learned policy, given the value of ``posture_type``,
+  - otherwise, a pre-set, context-independent position is used.
+2. The arm is moved to the ``neutral`` configuration.
+3. The arm is then moved such that the end-effector ends in the position chosen in step 1 (currently the trajectory to be followed is set to a learned DMP trajectory).
+4. If the ``release_detection`` action parameter is set to ``True``, the robot utilizes the wrist force sensor to determine when the person is pulling the object from its grasp with the intent of receiving it, using a signal change detection algorithm, and then releases the object. Otherwise, the robot waits for a short time and simply releases the object.
+5. The arm is moved back to the ``neutral`` configuration.
+
+## Dependencies
+
+* ``smach``
+* ``smach_ros``
+* ``tf``
+* ``actionlib``
+* ``geometry_msgs``
+* ``mdr_move_arm_action``
+* ``mdr_move_base_action``
+
+## Example usage
+
+1. Run the ``move_arm`` action server: ``roslaunch mdr_move_arm_action move_arm.launch``
+2. Run the action server: ``roslaunch mdr_hand_over hand_over.launch``
+3. Run the client example: ``rosrun mdr_hand_over_action hand_over_action_client_test ['standing', 0, 1]``, providing a list of the action's parameters (context, obstacle, release_detection) as arguments
\ No newline at end of file

From 83c890748c8a9d7acbc3dc1402092aa5b892ac79 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 27 Mar 2020 15:32:25 +0100
Subject: [PATCH 025/264] [move_arm_action] Add online DMP trajectory
 generation

---
 .../ros/src/mdr_move_arm_action/dmp.py        | 70 +++++++++++++------
 1 file changed, 48 insertions(+), 22 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 5071eb877..ff0b7a70b 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -6,6 +6,9 @@
 import tf
 from ros_dmp.roll_dmp import RollDmp
 
+# +++
+import yaml
+
 
 class DMPExecutor(object):
     def __init__(self, dmp_name, tau):
@@ -69,11 +72,7 @@ def move_to(self, goal):
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
             initial_pos = np.zeros(3)
 
-        path = self.generate_trajectory(initial_pos, goal)
-        path_odom = np.array([self.transform_pose(position, self.base_link_frame_name, self.odom_frame_name)
-                             for position in path])
-        self.publish_path(path_odom)
-        self.follow_path(path_odom)
+        self.follow_path(initial_pos, goal)
 
     def generate_trajectory(self, initial_pos, goal):
         '''Returns a 2D numpy array representing a path of [x, y, z] points
@@ -102,7 +101,7 @@ def generate_trajectory(self, initial_pos, goal):
                          for state in cartesian_trajectory.cartesian_state])
         return path
 
-    def follow_path(self, path):
+    def follow_path(self, goal):
         '''Moves a manipulator so that it follows the given path. If whole body
         motion is enabled and some points on the path lie outside the reachable
         workspace, the base is moved accordingly as well. The path is followed
@@ -136,15 +135,18 @@ def follow_path(self, path):
         path: np.array -- a 2D array of points (each row represents a point)
 
         '''
-        # sanity check - we only consider the path if it contains any points
-        if path is None or path.shape[0] == 0:
-            rospy.logerr('[move_arm/dmp/follow_path] No points in path; aborting execution')
-            return
-
         rospy.loginfo('[move_arm/dmp/follow_path] Executing motion')
         trans, _ = self.get_transform(self.odom_frame_name, self.palm_link_name, rospy.Time(0))
         current_pos = np.array([trans[0], trans[1], trans[2]])
-        distance_to_goal = np.linalg.norm((path[-1] - current_pos))
+        distance_to_goal = np.linalg.norm((goal - current_pos))
+
+        # +++
+        initial_pose = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0., 0., 0.])
+        goal_pose = np.array([goal[0], goal[1], goal[2], 0., 0., 0.])
+
+        current_path = initial_pose[0:3]
+
+        self.dmp = self.instantiate_dmp(initial_pose, goal_pose)
 
         self.motion_completed = False
         self.motion_cancelled = False
@@ -158,21 +160,16 @@ def follow_path(self, path):
 
             # if the end effector has reached the goal (within the allowed
             # tolerance threshold), we stop the motion
-            distance_to_goal = np.linalg.norm((path[-1] - current_pos))
+            distance_to_goal = np.linalg.norm((goal - current_pos))
             if distance_to_goal <= self.goal_tolerance:
                 self.motion_completed = True
                 break
 
-            path_point_distances = [np.linalg.norm(p - current_pos) for p in path]
-            min_dist_idx = np.argmin(path_point_distances)
-
-            next_goal_idx = -1
-            if min_dist_idx == path.shape[0] - 1:
-                next_goal_idx = min_dist_idx
-            else:
-                next_goal_idx = min_dist_idx + 1
+            # +++
+            next_pos, _, _ = self.dmp.step(tau=self.tau)
+            current_path = np.vstack((current_path, next_pos))
 
-            vel = self.feedforward_gain * (path[next_goal_idx] - path[min_dist_idx]) + self.feedback_gain * (path[next_goal_idx] - current_pos)
+            vel = self.feedback_gain * (next_pos - current_pos)
 
             # we limit the speed if it is above the allowed limit
             velocity_norm = np.linalg.norm(vel)
@@ -219,6 +216,9 @@ def follow_path(self, path):
             self.vel_publisher_arm.publish(twist_arm)
             cmd_count += 1
 
+            # +++
+            self.publish_path(current_path)
+
         # stop arm and base motion after converging
         twist_arm = TwistStamped()
         twist_arm.header.seq = cmd_count
@@ -301,3 +301,29 @@ def sigma_values_cb(self, msg):
 
         '''
         self.min_sigma_value = min(msg.data)
+
+    def instantiate_dmp(self, initial_pose, goal_pose):
+        ''' ...
+        ...
+        ...
+        '''
+        with open(self.dmp_name) as f:
+            dmp_weights_dict = yaml.load(f)
+
+        n_dmps, n_bfs = dmp_weights_dict.shape[0], dmp_weights_dict.shape[1]
+
+        dmp_weights = np.zeros((n_dmps, n_bfs))
+        dmp_weights[0, :] = dmp_weights_dict['x']
+        dmp_weights[1, :] = dmp_weights_dict['y']
+        dmp_weights[2, :] = dmp_weights_dict['z']
+        dmp_weights[3, :] = dmp_weights_dict['roll']
+        dmp_weights[4, :] = dmp_weights_dict['pitch']
+        dmp_weights[5, :] = dmp_weights_dict['yaw']
+
+        return pydmps.dmp_discrete.DMPs_discrete(n_dmps=n_dmps, 
+                                                 n_bfs=n_bfs,
+                                                 dt=self.time_step,
+                                                 y0=initial_pose,
+                                                 goal=goal_pose,
+                                                 ay=None, 
+                                                 w=dmp_weights)

From bfbbb0bac5229c82d2d7f3e02545e3f18bf6ad2a Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Tue, 28 Apr 2020 17:33:37 +0200
Subject: [PATCH 026/264] [move_arm_action] Fix minor errors (tested on HSR)

---
 .../ros/src/mdr_move_arm_action/dmp.py             | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index ff0b7a70b..5f6af3629 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -5,8 +5,8 @@
 from nav_msgs.msg import Path
 import tf
 from ros_dmp.roll_dmp import RollDmp
+import pydmps
 
-# +++
 import yaml
 
 
@@ -101,7 +101,7 @@ def generate_trajectory(self, initial_pos, goal):
                          for state in cartesian_trajectory.cartesian_state])
         return path
 
-    def follow_path(self, goal):
+    def follow_path(self, initial_pos,  goal):
         '''Moves a manipulator so that it follows the given path. If whole body
         motion is enabled and some points on the path lie outside the reachable
         workspace, the base is moved accordingly as well. The path is followed
@@ -140,11 +140,10 @@ def follow_path(self, goal):
         current_pos = np.array([trans[0], trans[1], trans[2]])
         distance_to_goal = np.linalg.norm((goal - current_pos))
 
-        # +++
         initial_pose = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0., 0., 0.])
         goal_pose = np.array([goal[0], goal[1], goal[2], 0., 0., 0.])
 
-        current_path = initial_pose[0:3]
+        current_path = initial_pose[:3]
 
         self.dmp = self.instantiate_dmp(initial_pose, goal_pose)
 
@@ -165,8 +164,8 @@ def follow_path(self, goal):
                 self.motion_completed = True
                 break
 
-            # +++
-            next_pos, _, _ = self.dmp.step(tau=self.tau)
+            next_pose, _, _ = self.dmp.step(tau=self.tau)
+            next_pos = next_pose[:3]
             current_path = np.vstack((current_path, next_pos))
 
             vel = self.feedback_gain * (next_pos - current_pos)
@@ -216,7 +215,6 @@ def follow_path(self, goal):
             self.vel_publisher_arm.publish(twist_arm)
             cmd_count += 1
 
-            # +++
             self.publish_path(current_path)
 
         # stop arm and base motion after converging
@@ -310,7 +308,7 @@ def instantiate_dmp(self, initial_pose, goal_pose):
         with open(self.dmp_name) as f:
             dmp_weights_dict = yaml.load(f)
 
-        n_dmps, n_bfs = dmp_weights_dict.shape[0], dmp_weights_dict.shape[1]
+        n_dmps, n_bfs = len(dmp_weights_dict), len(dmp_weights_dict['x'])
 
         dmp_weights = np.zeros((n_dmps, n_bfs))
         dmp_weights[0, :] = dmp_weights_dict['x']

From a9205907477c4e2738f96acd86c72cf9289024ee Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Tue, 28 Apr 2020 18:08:30 +0200
Subject: [PATCH 027/264] [move_arm_action] Add missing function docstring

---
 .../ros/src/mdr_move_arm_action/dmp.py                   | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 5f6af3629..754d258e1 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -301,9 +301,12 @@ def sigma_values_cb(self, msg):
         self.min_sigma_value = min(msg.data)
 
     def instantiate_dmp(self, initial_pose, goal_pose):
-        ''' ...
-        ...
-        ...
+        '''Instantiates a DMP object from learned weights, given 
+        the initial and final poses.
+
+        Keyword arguments:
+        initial_pose: numpy.ndarray -- initial pose coordinates of end-effector
+        goal_pose: numpy.ndarray -- target pose coordinates of end-effector
         '''
         with open(self.dmp_name) as f:
             dmp_weights_dict = yaml.load(f)

From 157d1e228fbe7613551fc3c94ece3a4c3a7a51a8 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Tue, 28 Apr 2020 18:09:32 +0200
Subject: [PATCH 028/264] [move_arm_action] Correct references to np array
 variables in docstrings

---
 .../ros/src/mdr_move_arm_action/dmp.py                 | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 754d258e1..f6c894291 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -56,7 +56,7 @@ def move_to(self, goal):
         as encoded by self.dmp_name.
 
         Keyword arguments:
-        goal: np.array -- end effector goal position in the base link frame
+        goal: numpy.ndarray -- end effector goal position in the base link frame
 
         '''
         initial_pos = None
@@ -81,8 +81,8 @@ def generate_trajectory(self, initial_pos, goal):
         on the path.
 
         Keyword arguments:
-        initial_pos: np.array -- initial position for the motion
-        goal: np.array -- goal position
+        initial_pos: numpy.ndarray -- initial position for the motion
+        goal: numpy.ndarray -- goal position
 
         '''
         initial_pose = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0., 0., 0.])
@@ -132,7 +132,7 @@ def follow_path(self, initial_pos,  goal):
           minimum sigma value (determined experimentally)
 
         Keyword arguments:
-        path: np.array -- a 2D array of points (each row represents a point)
+        path: numpy.ndarray -- a 2D array of points (each row represents a point)
 
         '''
         rospy.loginfo('[move_arm/dmp/follow_path] Executing motion')
@@ -276,7 +276,7 @@ def publish_path(self, path):
         '''Publishes the given path to the topic specified by self.dmp_executor_path_topic.
 
         Keyword arguments:
-        path: np.array -- a 2D array of points in which each row represents a position
+        path: numpy.ndarray -- a 2D array of points in which each row represents a position
 
         '''
         path_msg = Path()

From d0d406fa228b65ab79390dc2715abe77d2b62cc4 Mon Sep 17 00:00:00 2001
From: Santosh Thoduka <santosh.thoduka@gmail.com>
Date: Wed, 27 May 2020 12:14:06 +0200
Subject: [PATCH 029/264] [hand_over_action] move arm to neutral position in
 init state (otherwise the execution of the DMP is weird); fix readme

---
 .../mdr_manipulation_actions/mdr_hand_over_action/README.md   | 4 ++--
 .../ros/src/mdr_hand_over_action/action_states.py             | 4 ++++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
index 3e525cb94..a595762dc 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
@@ -100,5 +100,5 @@ It is assumed that the robot starts with the object to be handed over already in
 ## Example usage
 
 1. Run the ``move_arm`` action server: ``roslaunch mdr_move_arm_action move_arm.launch``
-2. Run the action server: ``roslaunch mdr_hand_over hand_over.launch``
-3. Run the client example: ``rosrun mdr_hand_over_action hand_over_action_client_test ['standing', 0, 1]``, providing a list of the action's parameters (context, obstacle, release_detection) as arguments
\ No newline at end of file
+2. Run the action server: ``roslaunch mdr_hand_over_action hand_over.launch``
+3. Run the client example: ``rosrun mdr_hand_over_action hand_over_action_client_test ['standing', 0, 1]``, providing a list of the action's parameters (context, obstacle, release_detection) as arguments
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 4236d1ec0..462912373 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -67,6 +67,10 @@ def init(self):
             rospy.logerr('[hand_over] %s', str(exc))
             return FTSMTransitions.INIT_FAILED
 
+        # Start with arm in neutral position:
+        rospy.loginfo('[hand_over] Moving arm to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
+
         return FTSMTransitions.INITIALISED
 
     def running(self):

From e5e95c6b8889f61baf93cbbf66991f750e2066e1 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 29 Dec 2019 14:20:30 +0100
Subject: [PATCH 030/264] [handle_open_action] Modified the action to include
 handle detection

---
 .../mdr_handle_open_action/README.md          | 30 +++++++++-
 .../ros/action/HandleOpen.action              |  9 ++-
 .../ros/launch/handle_open_action.launch      |  1 +
 .../ros/scripts/handle_open_action            |  3 +
 .../scripts/handle_open_action_client_test    | 51 +---------------
 .../mdr_handle_open_action/action_states.py   | 58 +++++++++++++++++--
 6 files changed, 94 insertions(+), 58 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/README.md
index d78816b46..01edae3a7 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/README.md
@@ -6,8 +6,15 @@ An action for manipulating handles.
 
 ### Goal:
 
-* ``string handle_type``: Type of the handle (currently unused)
-* ``geometry_msgs/PoseStamped handle_pose``: Pose of the handle
+* ``string handle_type``: Optional type of the handle that needs to be opened
+
+The following constants are also defined in the action goal:
+* ``string PULL=pull_knob``
+* ``string LEVER=lever_knob``
+* ``string ROUND=round_knob``
+* ``string UNKNOWN=unknown``
+
+Note that these constants match the handle classes defined at https://github.com/b-it-bots/mas_models/blob/master/perception_models/detectors/classes_handles.yaml
 
 ### Result:
 
@@ -25,12 +32,25 @@ An action for manipulating handles.
 * ``move_arm_server``: Name of the move_arm action server (default: 'move_arm_server')
 * ``move_base_server``: Name of the move_base action server (default: 'move_base_server')
 * ``move_forward_server``: Name of the move_forward action server (default: 'move_forward_server')
+* ``detect_handle_server``: Name of a detect_handle action server (default: 'detect_handle_server')
 * ``force_sensor_topic``: Name of topic for wrist force sensor measurements (default: 'force_sensor_topic')
 * ``pregrasp_config_name``: Name of the pregrasp configuration (default: 'pregrasp_config_name')
 * ``final_config_name``: Name of the final configuration (default: 'final_config_name')
 * ``handle_open_dmp``: Path to a YAML file containing the weights of a dynamic motion primitive used for opening handles (default: '')
 * ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 30)
 
+## Action execution summary
+
+1. A handle is detected (including the type of handle and its pose)
+2. Depending on the handle type, a different opening policy is followed:
+    * a pull knob is the easiest to open since in that case the robot simply grasps the handle and moves backwards
+    * a round knob is similar to a pull knob, but the robot moves sideways as well
+    * for a lever knob, force feedback from the handle is used until the lever is pushed downwards and then the base moves to open the door
+
+In all three cases, the pose at which the handle is grasped is sampled from a learned distribution as described below.
+
+If `handle_type` is not `unknown`, the detected handle type is checked against the given type; if these don't match, an action failure is reported.
+
 ## Dependencies
 
 * ``numpy``
@@ -41,6 +61,8 @@ An action for manipulating handles.
 * ``tf``
 * ``actionlib``
 * ``geometry_msgs``
+* ``mas_perception_msgs``
+* ``mas_perception_libs``
 * ``mas_execution``
 * ``mdr_rosplan_interface``
 * ``mdr_move_arm_action``
@@ -54,3 +76,7 @@ The package additionally includes some data (under `learning_data`) that was col
 There are two sets of poses there:
 * `sample_grasp_poses_uniform`: Poses sampled from a uniform distribution
 * `sample_grasp_poses_learned`: Poses sampled from the learned success distribution
+
+## Handle detection
+
+For the purpose of online handle detection, the action expects an action server of type `mas_perception_msgs/DetectObjectsAction` to be active. An example launch file for starting such a server is given at https://github.com/b-it-bots/mas_perception_libs/blob/devel/ros/launch/object_detection.launch. Note that a handle detection model can be found at https://github.com/b-it-bots/mas_models/tree/master/perception_models/detectors
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/action/HandleOpen.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/action/HandleOpen.action
index d5c9dbad1..57fddcceb 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/action/HandleOpen.action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/action/HandleOpen.action
@@ -1,6 +1,11 @@
-# goal
+string PULL=pull_knob
+string LEVER=lever_knob
+string ROUND=round_knob
+string UNKNOWN=unknown
+
+# optional handle type for cross-checking
+# the results of the online handle detection
 string handle_type
-geometry_msgs/PoseStamped handle_pose
 ---
 # result
 bool success
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
index 4670f12cb..4946e6a9b 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
@@ -5,6 +5,7 @@
         <param name="move_arm_server" value="move_arm_server" />
         <param name="move_base_server" value="move_base_server" />
         <param name="move_forward_server" value="move_forward_server" />
+        <param name="detect_handle_server" value="detect_handle_server" />
         <param name="force_sensor_topic" value="/wrist_force/raw"/>
         <param name="pregrasp_config_name" value="pregrasp_low" />
         <param name="final_config_name" value="pregrasp" />
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action
index 96922d2d3..b3fec85b0 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action
@@ -15,6 +15,7 @@ class HandleOpenServer(object):
     * move_arm_server: Name of the move_arm action server (default: 'move_arm_server')
     * move_base_server: Name of the move_base action server (default: 'move_base_server')
     * move_forward_server: Name of the move_forward action server (default: 'move_forward_server')
+    * detect_handle_server: Name of a detect_handle action server (default: 'detect_handle_server')
     * force_sensor_topic: Name of topic for wrist force sensor measurements
                           (default: 'force_sensor_topic')
     * pregrasp_config_name: Name of the pregrasp configuration (default: 'pregrasp_config_name')
@@ -30,6 +31,7 @@ class HandleOpenServer(object):
         move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
         move_base_server = rospy.get_param('~move_base_server', 'move_base_server')
         move_forward_server = rospy.get_param('~move_forward_server', 'move_forward_server')
+        detect_handle_server = rospy.get_param('~detect_handle_server', 'detect_handle_server')
         force_sensor_topic = rospy.get_param('~force_sensor_topic', 'force_sensor_topic')
         pregrasp_config_name = rospy.get_param('~pregrasp_config_name', 'pregrasp_config_name')
         final_config_name = rospy.get_param('~final_config_name', 'final_config_name')
@@ -42,6 +44,7 @@ class HandleOpenServer(object):
                                       move_base_server=move_base_server,
                                       force_sensor_topic=force_sensor_topic,
                                       move_forward_server=move_forward_server,
+                                      detect_handle_server=detect_handle_server,
                                       pregrasp_config_name=pregrasp_config_name,
                                       final_config_name=final_config_name,
                                       handle_open_dmp=handle_open_dmp,
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action_client_test
index c5a9991aa..45e442f63 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/scripts/handle_open_action_client_test
@@ -16,57 +16,8 @@ if __name__ == '__main__':
     client = actionlib.SimpleActionClient('/handle_open_server', HandleOpenAction)
     client.wait_for_server()
 
-    random = False
-
     goal = HandleOpenGoal()
-
-    goal.handle_pose.header.frame_id = 'base_link'
-    goal.handle_pose.header.stamp = rospy.Time.now()
-
-    if random:
-        # Sampling Random Grasp Positions From a Uniform Distribution:
-
-        z_lim = [0.62, 0.72]
-        x_lim = [0.68, 0.71]
-        y_lim = [0.00, 0.20]
-
-        sample_grasp_pose = [np.random.uniform(low=x_lim[0], high=x_lim[1]),
-                             np.random.uniform(low=y_lim[0], high=y_lim[1]),
-                             np.random.uniform(low=z_lim[0], high=z_lim[1])]
-
-        # Saving the sampled position in a file:
-        # save_samples_to_file('sample_grasp_poses')
-
-    else:
-        # Sampling Grasp Positions From Learned Gaussian Distribution:
-
-        ## TODO: Sample and store positions, and use to continuously update parameters (lifelong learning)
-        # At the moment, use the values learned from the last experiment (13/09/19)
-        posterior_mu_estimate = [0.70064241, 0.10315619, 0.6579796]
-        posterior_sigma_estimate = [4.48271554e-05, 2.67117757e-03, 4.27263449e-04]
-
-        sample_grasp_pose = [np.random.multivariate_normal(np.array([posterior_mu_estimate[0]]),
-                                                           np.array([posterior_sigma_estimate[0]])[np.newaxis])[0],
-                             np.random.multivariate_normal(np.array([posterior_mu_estimate[1]]),
-                                                             np.array([posterior_sigma_estimate[1]])[np.newaxis])[0],
-                             np.random.multivariate_normal(np.array([posterior_mu_estimate[2]]),
-                                                             np.array([posterior_sigma_estimate[2]])[np.newaxis])[0]]
-
-        # Saving the positions sampled from the new distribution in a file:
-        # save_samples_to_file('new_sample_grasp_poses')
-
-    rospy.loginfo('Sampled Grasp Position: ')
-    print(sample_grasp_pose)
-    print()
-
-    goal.handle_pose.pose.position.x = sample_grasp_pose[0]
-    goal.handle_pose.pose.position.y = sample_grasp_pose[1]
-    goal.handle_pose.pose.position.z = sample_grasp_pose[2]
-
-    goal.handle_pose.pose.orientation.x = 0.000
-    goal.handle_pose.pose.orientation.y = 0.000
-    goal.handle_pose.pose.orientation.z = 0.000
-    goal.handle_pose.pose.orientation.w = 1.000
+    goal.handle_type = HandleOpenGoal.UNKNOWN
 
     timeout = 15.0
     rospy.loginfo('Sending action lib goal to handle_open_server')
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
index f02ce9158..99f3024ac 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
@@ -1,4 +1,3 @@
-#!/usr/bin/python
 from importlib import import_module
 import numpy as np
 from scipy.stats import norm
@@ -9,13 +8,14 @@
 import tf
 import actionlib
 from geometry_msgs.msg import PoseStamped, WrenchStamped
+from mas_perception_msgs.msg import DetectObjectsAction, DetectObjectsGoal
 
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal
 from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
 from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
-from mdr_handle_open_action.msg import HandleOpenResult
+from mdr_handle_open_action.msg import HandleOpenGoal, HandleOpenResult
 
 class HandleOpenSM(ActionSMBase):
     def __init__(self, timeout=120.0,
@@ -23,6 +23,7 @@ def __init__(self, timeout=120.0,
                  move_arm_server='move_arm_server',
                  move_base_server='move_base_server',
                  move_forward_server='move_forward_server',
+                 detect_handle_server='detect_handle_server',
                  force_sensor_topic='/force_wrist/raw',
                  pregrasp_config_name='pregrasp_low',
                  final_config_name='pregrasp',
@@ -40,6 +41,7 @@ def __init__(self, timeout=120.0,
         self.move_arm_server = move_arm_server
         self.move_base_server = move_base_server
         self.move_forward_server = move_forward_server
+        self.detect_handle_server = detect_handle_server
         self.force_sensor_topic = force_sensor_topic
         self.pregrasp_config_name = pregrasp_config_name
         self.final_config_name = final_config_name
@@ -62,7 +64,9 @@ def __init__(self, timeout=120.0,
         self.failure_data = []
 
         self.move_arm_client = None
+        self.move_base_client = None
         self.move_forward_client = None
+        self.detect_handle_client = None
 
     def init(self):
         try:
@@ -88,11 +92,39 @@ def init(self):
             rospy.logerr('[handle_open] %s', str(exc))
             return FTSMTransitions.INIT_FAILED
 
+        try:
+            self.detect_handle_client = actionlib.SimpleActionClient(self.detect_handle_server, DetectObjectsAction)
+            rospy.loginfo('[pickup] Waiting for %s server', self.detect_handle_server)
+            self.detect_handle_client.wait_for_server()
+        except Exception as exc:
+            rospy.logerr('[handle_open] %s', str(exc))
+            return FTSMTransitions.INIT_FAILED
+
         return FTSMTransitions.INITIALISED
 
     def running(self):
-        pose = self.goal.handle_pose
-        pose.header.stamp = rospy.Time(0)
+        rospy.loginfo('[handle_open] Detecting handle')
+        self.detect_handle_client.send_goal(DetectObjectsGoal())
+        self.detect_handle_client.wait_for_result(timeout=self.timeout)
+        detection_result = self.detect_handle_client.get_result()
+
+        # if a handle could not be detected, the action is aborted
+        if not detection_result.objects.objects:
+            rospy.loginfo('[handle_open] Could not detect handle')
+            self.result = self.set_result(False)
+            return FTSMTransitions.DONE
+
+        rospy.loginfo('[handle_open] Detected %d handles; taking closest handle',
+                      len(detected_handles.objects.objects))
+        handle = self.__get_closest_handle(detection_result.objects.objects)
+        if self.goal.handle_type and self.goal.handle_type != HandleOpenGoal.UNKNOWN:
+            if self.goal_handle_type != handle.category:
+                rospy.loginfo('[handle_open] Type of detected handle does not match expected type %s',
+                              self.goal.handle_type)
+                self.result = self.set_result(False)
+                return FTSMTransitions.DONE
+
+        handle.pose.header.stamp = rospy.Time(0)
         pose_base_link = self.tf_listener.transformPose('base_link', pose)
 
         if self.base_elbow_offset > 0:
@@ -154,6 +186,24 @@ def running(self):
         self.__move_arm(MoveArmGoal.NAMED_TARGET, self.final_config_name)
         return FTSMTransitions.DONE
 
+    def __get_closest_handle(self, detected_handles):
+        '''Returns the handle closest to the robot.
+
+        Keyword arguments:
+        detected_handles: Sequence[mas_perception_msgs.msg.Object]
+
+        '''
+        if len(detected_handles) == 1:
+            return detected_handles[0]
+
+        handle_distances = []
+        for handle in detected_handles:
+            pose_base_link = self.tf_listener.transformPose('base_link', handle.pose)
+            handle_distances.append(np.linalg.norm([pose_base_link.pose.position.x,
+                                                    pose_base_link.pose.position.y,
+                                                    pose_base_link.pose.position.z]))
+        return detected_handles[np.argmin(handle_distances)]
+
     def __prepare_handle_grasp(self, pose_base_link):
         rospy.loginfo('[handle_open] Moving to a pregrasp configuration...')
         self.__move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name)

From 22302ecce1369de8f519c35c7b48e22cbfb3f04d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 29 Dec 2019 14:50:36 +0100
Subject: [PATCH 031/264] [handle_open_action/action_states] Error fix
 (undefined variable)

---
 .../ros/src/mdr_handle_open_action/action_states.py             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
index 99f3024ac..830c81a21 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
@@ -115,7 +115,7 @@ def running(self):
             return FTSMTransitions.DONE
 
         rospy.loginfo('[handle_open] Detected %d handles; taking closest handle',
-                      len(detected_handles.objects.objects))
+                      len(detection_result.objects.objects))
         handle = self.__get_closest_handle(detection_result.objects.objects)
         if self.goal.handle_type and self.goal.handle_type != HandleOpenGoal.UNKNOWN:
             if self.goal_handle_type != handle.category:

From 7ec2fe5ee7a7c53008237dacc08d5af56a5e922d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 4 Jan 2020 12:32:45 +0100
Subject: [PATCH 032/264] [handle_open_action] Error fix in wait_for_result
 call

---
 .../ros/src/mdr_handle_open_action/action_states.py             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
index 830c81a21..3c368cd4e 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
@@ -105,7 +105,7 @@ def init(self):
     def running(self):
         rospy.loginfo('[handle_open] Detecting handle')
         self.detect_handle_client.send_goal(DetectObjectsGoal())
-        self.detect_handle_client.wait_for_result(timeout=self.timeout)
+        self.detect_handle_client.wait_for_result(timeout=rospy.Duration.from_sec(self.timeout))
         detection_result = self.detect_handle_client.get_result()
 
         # if a handle could not be detected, the action is aborted

From d83f835be8dafcad3b2c859a42ce35f4516b5002 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 7 Jan 2020 22:10:17 +0100
Subject: [PATCH 033/264] [handle_open_action] Fixed a pose transform call with
 a non-existing object

---
 .../ros/src/mdr_handle_open_action/action_states.py             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
index 3c368cd4e..573d2bf1c 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/src/mdr_handle_open_action/action_states.py
@@ -125,7 +125,7 @@ def running(self):
                 return FTSMTransitions.DONE
 
         handle.pose.header.stamp = rospy.Time(0)
-        pose_base_link = self.tf_listener.transformPose('base_link', pose)
+        pose_base_link = self.tf_listener.transformPose('base_link', handle.pose)
 
         if self.base_elbow_offset > 0:
             self.__align_base_with_pose(pose_base_link)

From 644d651e3b3de89a9720f0412d7058dd65563d49 Mon Sep 17 00:00:00 2001
From: Dharmin <dharmingeo@gmail.com>
Date: Sat, 4 Jul 2020 13:30:18 +0200
Subject: [PATCH 034/264] [scenarios] Added skeleton for mdr_demo_lab_manager

---
 .../mdr_demo_lab_manager/CMakeLists.txt       | 31 +++++++++++++++
 .../config/lab_manager_sm.yaml                |  7 ++++
 .../mdr_demo_lab_manager/package.xml          | 38 +++++++++++++++++++
 .../ros/launch/lab_manager.launch             | 16 ++++++++
 4 files changed, 92 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
new file mode 100644
index 000000000..c7325ecfc
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_demo_lab_manager)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  mdr_move_arm_action
+  mdr_move_base_action
+  mdr_detect_person
+  mas_execution_manager
+  mdr_navigation_behaviours
+  mdr_perception_behaviours
+  mdr_manipulation_behaviours
+  mas_knowledge_base
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mdr_move_arm_action
+    mdr_move_base_action
+    mdr_detect_person
+    mas_execution_manager
+    mdr_navigation_behaviours
+    mdr_perception_behaviours
+    mdr_manipulation_behaviours
+    mas_knowledge_base
+)
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
new file mode 100644
index 000000000..4a0154d87
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -0,0 +1,7 @@
+# Description: Defines a state machine for a scenario in which a robot monitors people who enter a lab and guides them to a free sitting spot.
+# Author: Alex Mitrevski
+# Email: aleksandar.mitrevski@h-brs.de
+sm_id: mdr_demo_lab_manager
+states: []
+outcomes: [DONE, FAILED]
+state_descriptions: []
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/package.xml
new file mode 100644
index 000000000..68c99c242
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/package.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_demo_lab_manager</name>
+  <version>1.0.0</version>
+  <description>
+    A package for a scenario in which a robot monitors people who enter a lab
+    and guides them to a free sitting spot.
+  </description>
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <author email="dharmingeo@gmail.com">Dharmin Bakaraniya</author>
+  <author email="ahmed.abdelrahman@inf.h-brs.de">Ahmed Faisal Abdelrahman</author>
+  <author email="samuel.parra@inf.h-brs.de">Samuel Parra</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>mdr_move_arm_action</build_depend>
+  <build_depend>mdr_move_base_action</build_depend>
+  <build_depend>mdr_detect_person</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+  <build_depend>mdr_navigation_behaviours</build_depend>
+  <build_depend>mdr_perception_behaviours</build_depend>
+  <build_depend>mdr_manipulation_behaviours</build_depend>
+  <build_depend>mas_knowledge_base</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>mdr_move_arm_action</run_depend>
+  <run_depend>mdr_move_base_action</run_depend>
+  <run_depend>mdr_detect_person</run_depend>
+  <run_depend>mas_execution_manager</run_depend>
+  <run_depend>mdr_navigation_behaviours</run_depend>
+  <run_depend>mdr_perception_behaviours</run_depend>
+  <run_depend>mdr_manipulation_behaviours</run_depend>
+  <run_depend>mas_knowledge_base</run_depend>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
new file mode 100644
index 000000000..d3f5244d7
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+
+    <!-- Actions -->
+    <include file="$(find mdr_move_arm_action)/ros/launch/move_arm.launch" />
+
+    <include file="$(find mdr_move_base_action)/ros/launch/move_base_action.launch" />
+    <include file="$(find mdr_move_base_action)/ros/launch/move_base_client.launch" />
+
+    <!-- State machine -->
+    <node pkg="mas_execution_manager" type="state_machine_creator" name="lab_manager" output="screen">
+        <param name="sm_config_file" type="str"
+               value="$(find mdr_demo_lab_manager)/config/lab_manager_sm.yaml" />
+    </node>
+</launch>

From 6382045bfa175ee66485151892d90b10c58e9a40 Mon Sep 17 00:00:00 2001
From: Dharmin <dharmingeo@gmail.com>
Date: Sat, 4 Jul 2020 14:29:22 +0200
Subject: [PATCH 035/264] [mdr_demo_lab_manager] Added states for lab manager
 (first draft)

---
 .../config/lab_manager_sm.yaml                | 83 ++++++++++++++++++-
 1 file changed, 82 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 4a0154d87..6fe5cbbd7 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -2,6 +2,87 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_lab_manager
-states: []
+states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, ASK_NAME, TURN_PERSON_AWAY, GUIDE_TO_EMPTY_SPOT]
 outcomes: [DONE, FAILED]
 state_descriptions: []
+    - state:
+        name: GO_TO_MONITORING_POSE
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: MONITOR_DOOR
+            - transition:
+                name: failed
+                state: GO_TO_MONITORING_POSE
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: destination_locations
+                value: [monitoring_pose]
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: MONITOR_DOOR
+        state_module_name: mdr_perception_behaviours.monitor_door
+        state_class_name: MonitorDoor
+        transitions:
+            - transition:
+                name: no_empty_spot
+                state: TURN_PERSON_AWAY
+            - transition:
+                name: new_person
+                state: ASK_NAME
+            - transition:
+                name: already_logged_person
+                state: MONITOR_DOOR
+            - transition:
+                name: known_person
+                state: MONITOR_DOOR
+        arguments: []
+    - state:
+        name: TURN_PERSON_AWAY
+        state_module_name: mdr_perception_behaviours.turn_person_away
+        state_class_name: TurnPersonAway
+        transitions:
+            - transition:
+                name: already_logged_person
+                state: MONITOR_DOOR
+        arguments: []
+    - state:
+        name: ASK_NAME
+        state_module_name: mdr_perception_behaviours.ask_name
+        state_class_name: AskName
+        transitions:
+            - transition:
+                name: succeeded
+                state: GUIDE_TO_EMPTY_SPOT
+            - transition:
+                name: failed
+                state: ASK_NAME
+        arguments: []
+    - state:
+        name: GUIDE_TO_EMPTY_SPOT
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_MONITORING_POSE
+            - transition:
+                name: failed
+                state: GO_TO_MONITORING_POSE
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: destination_locations
+                value: [spot_1, spot_2, spot_3, spot_4, spot_5, spot_6, spot_7, spot_8]
+            - argument:
+                name: number_of_retries
+                value: 3

From 363ef495c27aecb96f69f68da70ab1ffe5dd0d83 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 4 Jul 2020 16:32:05 +0200
Subject: [PATCH 036/264] Add debug flag for calls to say()

---
 .../ros/src/mdr_perception_behaviours/find_people.py         | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
index bb55bab2a..056e3ad2a 100644
--- a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
+++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
@@ -12,6 +12,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'find_people')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.debug = kwargs.get('debug', 0)
         self.retry_count = 0
         self.timeout = 120.
 
@@ -30,14 +31,14 @@ def execute(self, userdata):
 
         if self.succeeded:
             rospy.loginfo('[find_people] Successfully found people')
-            self.say('Successfully found people')
+            if self.debug: self.say('Successfully found people')
             return 'succeeded'
 
         rospy.loginfo('Could not find people')
         self.say('Could not find people')
         if self.retry_count == self.number_of_retries:
             rospy.loginfo('[find_people] Failed to find people')
-            self.say('Aborting operation')
+            if self.debug: self.say('Aborting operation')
             return 'failed_after_retrying'
         rospy.loginfo('[find_people] Retrying to find people')
         self.retry_count += 1

From 2ca1b9349a5221887966c00499d4b2997145f275 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 4 Jul 2020 16:55:37 +0200
Subject: [PATCH 037/264] Add simple say sentence HRI behaviour

---
 .../src/mdr_hri_behaviours/say_sentence.py    | 22 +++++++++++++++++++
 1 file changed, 22 insertions(+)
 create mode 100644 mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
new file mode 100644
index 000000000..e6ba3727a
--- /dev/null
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
@@ -0,0 +1,22 @@
+import time
+import rospy
+import random
+
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class SaySentence(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'say_sentence',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'say_something')
+        self.sentences = list(kwargs.get('sentences', list()))
+
+    def execute(self, userdata):
+        sentence = random.choice(self.sentences)
+
+        rospy.loginfo('Saying: %s' % sentence)
+        self.say(sentence)
+
+        return 'succeeded'

From fb170a8eb371f3c7d69011b9dc9463af2fec232e Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 4 Jul 2020 17:17:34 +0200
Subject: [PATCH 038/264] Remove server launch files; add find people client
 launch file

---
 .../mdr_demo_lab_manager/ros/launch/lab_manager.launch       | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
index d3f5244d7..4f70fc968 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
@@ -3,11 +3,10 @@
     <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
 
     <!-- Actions -->
-    <include file="$(find mdr_move_arm_action)/ros/launch/move_arm.launch" />
-
-    <include file="$(find mdr_move_base_action)/ros/launch/move_base_action.launch" />
     <include file="$(find mdr_move_base_action)/ros/launch/move_base_client.launch" />
 
+    <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
+
     <!-- State machine -->
     <node pkg="mas_execution_manager" type="state_machine_creator" name="lab_manager" output="screen">
         <param name="sm_config_file" type="str"

From 50ddb73cc825bb072d6d11ffc68985cfe3262dd5 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 4 Jul 2020 17:19:43 +0200
Subject: [PATCH 039/264] [demo_lab_manager] Modify monitor door state and add
 verify person state

---
 .../config/lab_manager_sm.yaml                | 35 +++++++++++++++----
 1 file changed, 28 insertions(+), 7 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 6fe5cbbd7..8a8a9654f 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -2,7 +2,7 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_lab_manager
-states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, ASK_NAME, TURN_PERSON_AWAY, GUIDE_TO_EMPTY_SPOT]
+states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, TURN_PERSON_AWAY, GUIDE_TO_EMPTY_SPOT]
 outcomes: [DONE, FAILED]
 state_descriptions: []
     - state:
@@ -28,8 +28,26 @@ state_descriptions: []
                 value: 3
     - state:
         name: MONITOR_DOOR
-        state_module_name: mdr_perception_behaviours.monitor_door
-        state_class_name: MonitorDoor
+        state_module_name: mdr_perception_behaviours.find_people
+        state_class_name: FindPeople
+        transitions:
+            - transition:
+                name: succeeded
+                state: VERIFY_PERSON
+            - transition:
+                name: failed
+                state: MONITOR_DOOR
+            - transition:
+                name: failed_after_retrying
+                state: MONITOR_DOOR
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 0
+    - state:
+        name: VERIFY_PERSON
+        state_module_name: mdr_perception_behaviours.verify_person
+        state_class_name: VerifyPerson
         transitions:
             - transition:
                 name: no_empty_spot
@@ -46,13 +64,16 @@ state_descriptions: []
         arguments: []
     - state:
         name: TURN_PERSON_AWAY
-        state_module_name: mdr_perception_behaviours.turn_person_away
-        state_class_name: TurnPersonAway
+        state_module_name: mdr_hri_behaviours.say_something
+        state_class_name: SaySomething
         transitions:
             - transition:
-                name: already_logged_person
+                name: succeeded
                 state: MONITOR_DOOR
-        arguments: []
+        arguments:
+            - argument:
+                name: sentences
+                value: ['The lab is full. Please come back later.']
     - state:
         name: ASK_NAME
         state_module_name: mdr_perception_behaviours.ask_name

From b8b89bda8fb052e308b7a0fa53c426f54ad10e60 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 16:31:51 +0200
Subject: [PATCH 040/264] [demo_lab_manager] Added stub verify_person
 implementation

---
 .../config/lab_manager_sm.yaml                |  2 +-
 .../mdr_demo_lab_manager/ros/src/__init__.py  |  0
 .../ros/src/scenario_states/__init__.py       |  0
 .../ros/src/scenario_states/verify_person.py  | 24 +++++++++++++++++++
 4 files changed, 25 insertions(+), 1 deletion(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 8a8a9654f..904d67069 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -46,7 +46,7 @@ state_descriptions: []
                 value: 0
     - state:
         name: VERIFY_PERSON
-        state_module_name: mdr_perception_behaviours.verify_person
+        state_module_name: mdr_demo_lab_manager.scenario_states.verify_person
         state_class_name: VerifyPerson
         transitions:
             - transition:
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py
new file mode 100644
index 000000000..4a5aba424
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py
@@ -0,0 +1,24 @@
+import rospy
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class VerifyPerson(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'verify_person',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['no_empty_spot', 'new_person',
+                                             'already_logged_person',
+                                             'known_person'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'verify_person')
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.total_locations = kwargs.get('total_locations', 0)
+        self.retry_count = 0
+        self.timeout = 120.
+
+    def execute(self, userdata):
+        rospy.loginfo('[verify_person] Verifying person')
+
+        occupied_spots = self.kb_interface.get_all_attributes('occupied_location')
+        if len(occupied_spots) == self.total_locations:
+            return 'no_empty_spot'
+        return 'new_person'

From 7c0788e269d07a8671d2bf1e1d00d671dd1bc3da Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 16:46:29 +0200
Subject: [PATCH 041/264] [demo_lab_manager] Added a setup.py and fixed the
 structure of the python package

---
 .../ros/src/{ => mdr_demo_lab_manager}/__init__.py    |  0
 .../scenario_states/__init__.py                       |  0
 .../scenario_states/verify_person.py                  |  0
 .../mdr_demos/mdr_demo_lab_manager/setup.py           | 11 +++++++++++
 4 files changed, 11 insertions(+)
 rename mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/{ => mdr_demo_lab_manager}/__init__.py (100%)
 rename mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/{ => mdr_demo_lab_manager}/scenario_states/__init__.py (100%)
 rename mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/{ => mdr_demo_lab_manager}/scenario_states/verify_person.py (100%)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/__init__.py
similarity index 100%
rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/__init__.py
rename to mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/__init__.py
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/__init__.py
similarity index 100%
rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/__init__.py
rename to mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/__init__.py
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
similarity index 100%
rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/scenario_states/verify_person.py
rename to mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/setup.py
new file mode 100644
index 000000000..5b5d6ee7c
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_demo_lab_manager'],
+    package_dir={'mdr_demo_lab_manager': 'ros/src/mdr_demo_lab_manager'}
+)
+
+setup(**d)

From 4b98789cb402a7ea92d15eb0e177bd8746703174 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 17:07:01 +0200
Subject: [PATCH 042/264] [demo_lab_manager] Forgot to set up the Python
 package in the CMakeLists

---
 .../mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
index c7325ecfc..24570054c 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt
@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
   mas_knowledge_base
 )
 
+catkin_python_setup()
+
 catkin_package(
   CATKIN_DEPENDS
     rospy

From 17de9fe906b35c1d7caef1716e3a008fb834d822 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 17 Jul 2020 17:14:31 +0200
Subject: [PATCH 043/264] [hri_behaviours] Fix incorrect scenario name

---
 .../ros/src/mdr_hri_behaviours/say_sentence.py                  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
index e6ba3727a..ffb36cad4 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
@@ -10,7 +10,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded'])
         self.sm_id = kwargs.get('sm_id', '')
-        self.state_name = kwargs.get('state_name', 'say_something')
+        self.state_name = kwargs.get('state_name', 'say_sentence')
         self.sentences = list(kwargs.get('sentences', list()))
 
     def execute(self, userdata):

From 83a9cbe32fd8365ee48816314a9bd9946c713395 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 17 Jul 2020 17:16:03 +0200
Subject: [PATCH 044/264] [demo_lab_manager] Fix incorrect state name

---
 .../mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 904d67069..ae6edbcf5 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -64,8 +64,8 @@ state_descriptions: []
         arguments: []
     - state:
         name: TURN_PERSON_AWAY
-        state_module_name: mdr_hri_behaviours.say_something
-        state_class_name: SaySomething
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
         transitions:
             - transition:
                 name: succeeded

From 41741c826c1dd5112e279a76bf839274ddfa9d49 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 19:47:58 +0200
Subject: [PATCH 045/264] [hri_behaviours] Added a state for asking for a name

---
 .../ros/src/mdr_hri_behaviours/ask_name.py    | 39 +++++++++++++++++++
 1 file changed, 39 insertions(+)
 create mode 100644 mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py
new file mode 100644
index 000000000..ee6e0db8b
--- /dev/null
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py
@@ -0,0 +1,39 @@
+import rospy
+import actionlib
+from actionlib_msgs.msg import GoalStatus
+
+from mdr_listen_action.msg import ListenAction, ListenGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class AskName(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'ask_name',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'ask_name')
+        self.timeout = 10.
+
+        # wait for listen action server
+        self.listen_client = actionlib.SimpleActionClient('listen_server', ListenAction)
+        listen_wait_result = self.listen_client.wait_for_server(timeout=rospy.Duration(self.timeout))
+        if not listen_wait_result:
+            raise RuntimeError('Failed to wait for "listen_server" action')
+
+    def execute(self, userdata):
+        sentence = 'Please tell me your first name'
+        rospy.loginfo('Saying {0}'.format(sentence))
+        self.say(sentence)
+
+        goal = ListenGoal()
+        self.listen_client.send_goal(goal)
+        self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        listen_state = self.listen_client.get_state()
+        listen_result = self.listen_client.get_result()
+
+        if listen_state == GoalStatus.SUCCEEDED:
+            rospy.loginfo('Understood: {}'.format(listen_result.message))
+            userdata.person_name = listen_result.message
+            return 'succeeded'
+        rospy.logerr('Could not get input, listen action returned None')
+        return 'failed'

From f11eaff62b1e81ac38aca24ff4d3772e1dd7927e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 20:00:50 +0200
Subject: [PATCH 046/264] [demo_lab_manager] Verify person can now send the
 robot to a random location

This is added just for testing purposes
---
 .../mdr_demo_lab_manager/config/lab_manager_sm.yaml  | 12 ++++++------
 .../scenario_states/verify_person.py                 |  5 +++++
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index ae6edbcf5..3b8cbd490 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -4,7 +4,7 @@
 sm_id: mdr_demo_lab_manager
 states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, TURN_PERSON_AWAY, GUIDE_TO_EMPTY_SPOT]
 outcomes: [DONE, FAILED]
-state_descriptions: []
+state_descriptions:
     - state:
         name: GO_TO_MONITORING_POSE
         state_module_name: mdr_navigation_behaviours.move_base
@@ -61,7 +61,10 @@ state_descriptions: []
             - transition:
                 name: known_person
                 state: MONITOR_DOOR
-        arguments: []
+        arguments:
+            - argument:
+                name: total_locations
+                value: 7
     - state:
         name: TURN_PERSON_AWAY
         state_module_name: mdr_hri_behaviours.say_sentence
@@ -76,7 +79,7 @@ state_descriptions: []
                 value: ['The lab is full. Please come back later.']
     - state:
         name: ASK_NAME
-        state_module_name: mdr_perception_behaviours.ask_name
+        state_module_name: mdr_hri_behaviours.ask_name
         state_class_name: AskName
         transitions:
             - transition:
@@ -101,9 +104,6 @@ state_descriptions: []
                 name: failed_after_retrying
                 state: FAILED
         arguments:
-            - argument:
-                name: destination_locations
-                value: [spot_1, spot_2, spot_3, spot_4, spot_5, spot_6, spot_7, spot_8]
             - argument:
                 name: number_of_retries
                 value: 3
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 4a5aba424..7d2cdad3e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -1,3 +1,5 @@
+import random
+
 import rospy
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
@@ -21,4 +23,7 @@ def execute(self, userdata):
         occupied_spots = self.kb_interface.get_all_attributes('occupied_location')
         if len(occupied_spots) == self.total_locations:
             return 'no_empty_spot'
+
+        spot = random.randint(1, self.total_locations)
+        userdata.destination_locations = ['spot_{0}'.format(spot)]
         return 'new_person'

From a3e32a58d69c3a3b01345e6dd7f532ba4aa05d5c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 20:20:20 +0200
Subject: [PATCH 047/264] [navigation_behaviours/move_base] Using
 destination_locations from userdata if not provided as an arg

---
 .../ros/src/mdr_navigation_behaviours/move_base.py         | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
index d248a1f27..3a193fb0d 100644
--- a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
+++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
@@ -12,7 +12,8 @@ class MoveBase(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'move_base',
                                    save_sm_state=save_sm_state,
-                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'])
+                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'],
+                                   input_keys=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'move_base')
         self.move_base_server = kwargs.get('move_base_server', 'move_base_server')
@@ -28,6 +29,10 @@ def __init__(self, save_sm_state=False, **kwargs):
 
     def execute(self, userdata):
         original_location = self.kb_interface.get_robot_location(self.robot_name)
+        if len(self.destination_locations) == 0:
+            self.destination_locations = userdata.destination_locations
+            rospy.loginfo("Using userdata's destination_locations {0}".format(self.destination_locations))
+
         for destination_location in self.destination_locations:
             dispatch_msg = self.get_dispatch_msg(original_location,
                                                  destination_location)

From bbd89675cbc9366dea1c889d4608b21e60defa86 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 20:21:19 +0200
Subject: [PATCH 048/264] [demo_lab_manager] Fixed missing userdata key
 declaration

---
 .../src/mdr_demo_lab_manager/scenario_states/verify_person.py  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 7d2cdad3e..dc4a13178 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -9,7 +9,8 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    save_sm_state=save_sm_state,
                                    outcomes=['no_empty_spot', 'new_person',
                                              'already_logged_person',
-                                             'known_person'])
+                                             'known_person'],
+                                   output_keys=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'verify_person')
         self.number_of_retries = kwargs.get('number_of_retries', 0)

From 2dfdb2c905e2362f65f3c1ffa6be40e1b79334f1 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 20:26:09 +0200
Subject: [PATCH 049/264] [hri_behaviours/ask_name] Added missing userdata key
 declaration

---
 .../mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py
index ee6e0db8b..b06c50ec7 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py
@@ -9,7 +9,8 @@ class AskName(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'ask_name',
                                    save_sm_state=save_sm_state,
-                                   outcomes=['succeeded', 'failed'])
+                                   outcomes=['succeeded', 'failed'],
+                                   output_keys=['person_name'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'ask_name')
         self.timeout = 10.

From 7714e06062f8b7c4e0c22de286820f1052b34150 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 17 Jul 2020 20:32:18 +0200
Subject: [PATCH 050/264] [find_people_action/client] Fixed check for
 undetected people

---
 .../mdr_find_people/ros/scripts/find_people_client              | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
index bdab77008..b26476797 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
@@ -29,7 +29,7 @@ class FindPeopleClient(ActionClientBase):
         client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout)))
         result = client.get_result()
 
-        if result and result.person_list:
+        if result and result.person_list and result.person_list.persons:
             rospy.loginfo('[FIND_PEOPLE] Updating the knowledge base')
             self.update_knowledge_base(result.person_list)
             self.send_action_feedback(True)

From 83d25cf700e6937fc17bead7748127d1da9337e1 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Wed, 22 Jul 2020 19:37:59 +0200
Subject: [PATCH 051/264] Replace speech state with the google forms based user
 data query

---
 .../config/lab_manager_sm.yaml                | 21 +++++-
 .../scenario_states/get_user_data.py          | 70 +++++++++++++++++++
 2 files changed, 88 insertions(+), 3 deletions(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 3b8cbd490..38ea6a41b 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -79,8 +79,8 @@ state_descriptions:
                 value: ['The lab is full. Please come back later.']
     - state:
         name: ASK_NAME
-        state_module_name: mdr_hri_behaviours.ask_name
-        state_class_name: AskName
+        state_module_name: mdr_demo_lab_manager.scenario_states.get_user_data
+        state_class_name: GetUserData
         transitions:
             - transition:
                 name: succeeded
@@ -88,7 +88,22 @@ state_descriptions:
             - transition:
                 name: failed
                 state: ASK_NAME
-        arguments: []
+            - transition:
+                name: failed_after_retrying
+                state: GUIDE_TO_EMPTY_SPOT
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: sheet_id
+                value: 1P2awsaugrcFmm2MXKEqznnMDVIS3LUSq6NeT8rJhxKU
+            - argument:
+                name: worksheet_name
+                value: responses
+            - argument:
+                name: loop_rate_s
+                value: 2.0
     - state:
         name: GUIDE_TO_EMPTY_SPOT
         state_module_name: mdr_navigation_behaviours.move_base
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
new file mode 100644
index 000000000..f91a13ead
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -0,0 +1,70 @@
+import requests
+import pandas as pd
+import numpy as np
+import time
+
+import rospy
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class GetUserData(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'get_user_data',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed', 
+                                             'failed_after_retrying'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'get_user_data')
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+        self.timeout = kwargs.get('timeout', 120.)
+
+        self._sheet_id = kwargs.get('sheet_id', '')
+        self._worksheet_name = kwargs.get('worksheet_name', 'responses')
+        
+        self._num_known_entries = None
+        self._loop_rate_s = kwargs.get('loop_rate_s', 2.)
+
+        data = self._load_spreadsheet()
+        if data:
+            self._num_known_entries = data.shape[0]
+            rospy.loginfo("[get_user_data] Initialized the user data query with {0} prior known users".format(self._num_known_entries))
+        else:
+            rospy.logerr("[get_user_data] Could not initialize!")
+
+    def execute(self, userdata):
+        if self._num_known_entries is None:
+            rospy.logerr("User data was not initialized. Cannot process user input!")
+            return "succeeded"
+
+        rospy.loginfo("[get_user_data] waiting for user data")
+        start_time = time.time()
+        while True and time.time() - start_time < self.timeout:
+            data = self._load_spreadsheet()
+            num_of_new_entries = data.shape[0] - self._num_known_entries
+            if num_of_new_entries > 0:
+                self._num_known_entries = data.shape[0]
+                new_entry = data[-1, 1:3]
+                rospy.loginfo("[get_user_data] Found a new entry!\n\tName: {0}\n\tEmail: {1}".format(new_entry[0], new_entry[1]))
+                # TODO Write the user data to knowledge base
+                return "succeeded"
+            else:
+                time.sleep(self._loop_rate_s)
+        rospy.logerr("[get_user_data] Query timed out!")
+
+        if self.retry_count == self.number_of_retries:
+            self.say("Sorry, I could get our name. I shall proceed without it.")
+            self.retry_count = 0
+            return "failed_after_retrying"
+
+        self.say("Could you please enter your data? I still have not received it.")
+        self.retry_count += 1
+        return "failed"
+
+    def _load_spreadsheet(self):
+        # Construct the URL
+        URL = 'https://docs.google.com/spreadsheets/d/{0}/gviz/tq?tqx=out:csv&sheet={1}'.format(
+                self._sheet_id, self._worksheet_name)
+
+        # Fetch and process the csv data
+        data = pd.read_csv(URL, sep=",").to_numpy()
+        return data

From 7acc2c5a85413d97074418f41c14c7d8e0dc219e Mon Sep 17 00:00:00 2001
From: Lucy <robotics@mail.inf.h-brs.de>
Date: Sat, 25 Jul 2020 15:54:35 +0200
Subject: [PATCH 052/264] [demo_lab_manager/scenario_states/verify_person]
 Adding the occupancy checking implemented by Dharmin yesterday

---
 .../scenario_states/verify_person.py            | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index dc4a13178..11b12fe6f 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -2,6 +2,8 @@
 
 import rospy
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
+from rosplan_knowledge_msgs.msg import DomainFormula
+from diagnostic_msgs.msg import KeyValue
 
 class VerifyPerson(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
@@ -17,14 +19,23 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.total_locations = kwargs.get('total_locations', 0)
         self.retry_count = 0
         self.timeout = 120.
+        occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
+        if occupied_locations is None:
+            self.kb_interface.insert_obj_instance('occupied_locations', DomainFormula())
 
     def execute(self, userdata):
         rospy.loginfo('[verify_person] Verifying person')
 
-        occupied_spots = self.kb_interface.get_all_attributes('occupied_location')
-        if len(occupied_spots) == self.total_locations:
+        occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
+        occ_spots = [kv.key for kv in occupied_locations.typed_parameters]
+        if len(occupied_locations.typed_parameters) == self.total_locations:
             return 'no_empty_spot'
 
-        spot = random.randint(1, self.total_locations)
+        spots = [str(i+1) for i in range(self.total_locations)]
+        for occ_spot in occ_spots:
+            spots.remove(occ_spot)
+        spot = random.choice(spots)
         userdata.destination_locations = ['spot_{0}'.format(spot)]
+        occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='true'))
+        self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
         return 'new_person'

From 336e41fa87448ee5885ff7c1f97bd6044be39c9f Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Sat, 25 Jul 2020 16:41:48 +0200
Subject: [PATCH 053/264] [find_people_action] Modify to integrate face image
 extraction and adding to Person msg

---
 .../ros/src/mdr_find_people/action_states.py  | 25 ++++++++++++++++++-
 1 file changed, 24 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 9a7c4fc23..f016c91c8 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -2,9 +2,11 @@
 import rospy
 import smach
 import sympy
+import numpy as np
 
 import tf
-from sensor_msgs.msg import PointCloud2
+import face_recognition
+from sensor_msgs.msg import PointCloud2, Image
 from geometry_msgs.msg import Point, Pose, PoseStamped
 from mdr_find_people.msg import FindPeopleResult
 from mas_perception_msgs.msg import Person, PersonList
@@ -39,10 +41,21 @@ def execute(self, userdata):
         cv_image = cloud_msg_to_cv_image(cloud_msg)
         bridge = CvBridge()
         images = []
+        face_images = []
         for i, bb2d in enumerate(bb2ds):
             cropped_cv = crop_image(cv_image, bb2d)
             cropped_img_msg = bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
+            
+            rospy.loginfo('[find_people] Attempting to extract face of person {}'.format(i+1))
+            cropped_face_img = self._extract_face_image(cropped_cv)
+            if cropped_face_img is not None:
+                cropped_face_img_msg = bridge.cv2_to_imgmsg(cropped_face_img,
+                                                            encoding='passthrough')
+            else:
+                cropped_face_img_msg = Image()
+            
             images.append(cropped_img_msg)
+            face_images.append(cropped_face_img_msg)
 
         # Create the action result message
         pl = []
@@ -54,6 +67,7 @@ def execute(self, userdata):
             map_pose = self._listener.transformPose('/map', poses[i])
             p.pose = map_pose
             p.body_image = images[i]
+            p.face.image = face_images[i]
 
             pl.append(p)
 
@@ -65,3 +79,12 @@ def execute(self, userdata):
 
         userdata['find_people_result'] = result
         return 'succeeded'
+
+    def _extract_face_image(self, image_array):
+        try:
+            top, right, bottom, left = face_recognition.face_locations(image_array)[0]
+            rospy.loginfo('[find_people] Successfully extracted face from person image.')
+            return image_array[top:bottom, left:right]
+        except IndexError:
+            rospy.logwarn('[find_people] Failed to extract face from person image!')
+            return None

From a38ad92151fe2bfc57e66dc95655b9a6ad0ac97a Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 25 Jul 2020 16:46:53 +0200
Subject: [PATCH 054/264] [verify_person] Placeholder code for searching in
 known faces

---
 .../config/lab_manager_sm.yaml                    |  3 +++
 .../scenario_states/get_user_data.py              |  4 ++++
 .../scenario_states/verify_person.py              | 15 +++++++++++++++
 3 files changed, 22 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 38ea6a41b..f30c2eaf6 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -65,6 +65,9 @@ state_descriptions:
             - argument:
                 name: total_locations
                 value: 7
+            - argument:
+                name: threshold
+                value: 1
     - state:
         name: TURN_PERSON_AWAY
         state_module_name: mdr_hri_behaviours.say_sentence
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index f91a13ead..34936c88c 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -4,6 +4,7 @@
 import time
 
 import rospy
+from mas_perception_msgs.msg import Person
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
 class GetUserData(ScenarioStateBase):
@@ -46,6 +47,9 @@ def execute(self, userdata):
                 new_entry = data[-1, 1:3]
                 rospy.loginfo("[get_user_data] Found a new entry!\n\tName: {0}\n\tEmail: {1}".format(new_entry[0], new_entry[1]))
                 # TODO Write the user data to knowledge base
+                person_0 = self.kb_interface.get_obj_instance('person_0', Person._type)
+                self.kb_interface.insert_obj_instance(new_entry[0], person_0)
+                self.kn_interface.remove_obj_instance('person_0', Person._type)
                 return "succeeded"
             else:
                 time.sleep(self._loop_rate_s)
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 11b12fe6f..433d57d57 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -1,8 +1,10 @@
 import random
 
 import rospy
+import numpy as np
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 from rosplan_knowledge_msgs.msg import DomainFormula
+from mas_perception_msgs.msg import Person
 from diagnostic_msgs.msg import KeyValue
 
 class VerifyPerson(ScenarioStateBase):
@@ -17,6 +19,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.state_name = kwargs.get('state_name', 'verify_person')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.total_locations = kwargs.get('total_locations', 0)
+        self.threshold = kwargs.get('threshold', 0)
         self.retry_count = 0
         self.timeout = 120.
         occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
@@ -26,11 +29,23 @@ def __init__(self, save_sm_state=False, **kwargs):
     def execute(self, userdata):
         rospy.loginfo('[verify_person] Verifying person')
 
+        # Look for occupied spots
         occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
         occ_spots = [kv.key for kv in occupied_locations.typed_parameters]
         if len(occupied_locations.typed_parameters) == self.total_locations:
             return 'no_empty_spot'
 
+        # Try to match face to current people in the lab
+        known_faces = self.kb_interface.get_obj_of_type(Person._type)
+        unknown_face = self.kb_interface.get_obj_instance('person_0', Person._type)
+
+        for known_face in known_faces:
+            distance = known_face.face - unknown_face.face
+            if np.linalg.norm(distance) < self.threshold: 
+                self.say("Welcome back {0}".format(known_face.name))
+                return 'known_person'
+
+        # No matching face, treat as new person
         spots = [str(i+1) for i in range(self.total_locations)]
         for occ_spot in occ_spots:
             spots.remove(occ_spot)

From caccf3b405634477a608908e6954b5ddbdf5b00a Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 7 Aug 2020 15:36:49 +0200
Subject: [PATCH 055/264] [get_user_data] Ask the person to scan the QR code
 printed in the wall

---
 .../src/mdr_demo_lab_manager/scenario_states/get_user_data.py  | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index 34936c88c..ecad31199 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -37,6 +37,9 @@ def execute(self, userdata):
             rospy.logerr("User data was not initialized. Cannot process user input!")
             return "succeeded"
 
+        self.say("Hi, seems like you are visiting the lab for the first time. "
+                "Please enter your details by scanning the QR code displayed on the wall to your right.")
+
         rospy.loginfo("[get_user_data] waiting for user data")
         start_time = time.time()
         while True and time.time() - start_time < self.timeout:

From 2c922b8c66abf4dffcedf57379838bc693855eae Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 11:19:05 +0200
Subject: [PATCH 056/264] [find_people/action_states] The 'views' fields of the
 Person and Face msgs are now used

---
 .../ros/src/mdr_find_people/action_states.py     | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index f016c91c8..d06e40cfd 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -9,7 +9,7 @@
 from sensor_msgs.msg import PointCloud2, Image
 from geometry_msgs.msg import Point, Pose, PoseStamped
 from mdr_find_people.msg import FindPeopleResult
-from mas_perception_msgs.msg import Person, PersonList
+from mas_perception_msgs.msg import Person, PersonList, ObjectView
 from mas_perception_libs import ImageDetectionKey
 from mas_perception_libs.visualization import crop_image
 from mas_perception_libs.utils import cloud_msg_to_cv_image
@@ -45,7 +45,7 @@ def execute(self, userdata):
         for i, bb2d in enumerate(bb2ds):
             cropped_cv = crop_image(cv_image, bb2d)
             cropped_img_msg = bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
-            
+
             rospy.loginfo('[find_people] Attempting to extract face of person {}'.format(i+1))
             cropped_face_img = self._extract_face_image(cropped_cv)
             if cropped_face_img is not None:
@@ -53,7 +53,7 @@ def execute(self, userdata):
                                                             encoding='passthrough')
             else:
                 cropped_face_img_msg = Image()
-            
+
             images.append(cropped_img_msg)
             face_images.append(cropped_face_img_msg)
 
@@ -66,8 +66,14 @@ def execute(self, userdata):
 
             map_pose = self._listener.transformPose('/map', poses[i])
             p.pose = map_pose
-            p.body_image = images[i]
-            p.face.image = face_images[i]
+
+            person_view = ObjectView()
+            person_view.image = images[i]
+            p.views.append(person_view)
+
+            face_view = ObjectView()
+            face_view.image = face_images[i]
+            p.face.views.append(face_view)
 
             pl.append(p)
 

From fa7eb65d5a80f63044d0eb7dc343095f63c29f3b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 11:26:49 +0200
Subject: [PATCH 057/264] [find_my_mates/describe_person] Used the 'views'
 field of the Person msg

---
 .../src/mdr_find_my_mates/scenario_states/describe_person.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_find_my_mates/ros/src/mdr_find_my_mates/scenario_states/describe_person.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_find_my_mates/ros/src/mdr_find_my_mates/scenario_states/describe_person.py
index 5fce8a288..992b1fd2d 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_find_my_mates/ros/src/mdr_find_my_mates/scenario_states/describe_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_find_my_mates/ros/src/mdr_find_my_mates/scenario_states/describe_person.py
@@ -27,7 +27,7 @@ def execute(self, userdata):
         person_msg = self.kb_interface.get_obj_instance(userdata.person_name, Person)
         person_info_msg = self.kb_interface.get_obj_instance(userdata.person_name, PersonInfo)
         try:
-            cv_image = self.bridge.imgmsg_to_cv2(person_msg.rgb_image, "bgr8")
+            cv_image = self.bridge.imgmsg_to_cv2(person_msg.views[0].image, "bgr8")
         except CvBridgeError as e:
             rospy.logerr(e.msg)
 

From 3769efc67b0e2311a6457ab54eb1907c5970ec75 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 11:39:59 +0200
Subject: [PATCH 058/264] [demo_lab_manager/verify_person] The 'views' field of
 the Face msg is now used

---
 .../scenario_states/verify_person.py                | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 433d57d57..747522917 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -36,13 +36,14 @@ def execute(self, userdata):
             return 'no_empty_spot'
 
         # Try to match face to current people in the lab
-        known_faces = self.kb_interface.get_obj_of_type(Person._type)
-        unknown_face = self.kb_interface.get_obj_instance('person_0', Person._type)
+        known_people = self.kb_interface.get_obj_of_type(Person._type)
+        unknown_person = self.kb_interface.get_obj_instance('person_0', Person._type)
 
-        for known_face in known_faces:
-            distance = known_face.face - unknown_face.face
-            if np.linalg.norm(distance) < self.threshold: 
-                self.say("Welcome back {0}".format(known_face.name))
+        for known_person in known_people:
+            distance = np.linalg.norm(known_person.face.views[0].embedding.embedding - \
+                                      unknown_person.face.views[0].embedding.embedding)
+            if np.linalg.norm(distance) < self.threshold:
+                self.say("Welcome back {0}".format(known_person.name))
                 return 'known_person'
 
         # No matching face, treat as new person

From 47d282cfbf490aa19699ee733f530b42c7156a2f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 11:43:38 +0200
Subject: [PATCH 059/264] [find_people/action_states] Removed unused imports;
 fixed a relative import

---
 .../mdr_find_people/ros/src/mdr_find_people/action_states.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index d06e40cfd..1919b32bb 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -1,20 +1,17 @@
-import math
 import rospy
 import smach
 import sympy
-import numpy as np
 
 import tf
 import face_recognition
 from sensor_msgs.msg import PointCloud2, Image
-from geometry_msgs.msg import Point, Pose, PoseStamped
 from mdr_find_people.msg import FindPeopleResult
 from mas_perception_msgs.msg import Person, PersonList, ObjectView
 from mas_perception_libs import ImageDetectionKey
 from mas_perception_libs.visualization import crop_image
 from mas_perception_libs.utils import cloud_msg_to_cv_image
 from cv_bridge import CvBridge
-from find_people import FindPeople
+from mdf_find_people.find_people import FindPeople
 
 
 class FindPeopleState(smach.State):

From cdc8c3bbf30adeaae0c203eb764d58a56b690f71 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 11:48:23 +0200
Subject: [PATCH 060/264] [demo_lab_manager/verify_person] Converted the
 embeddings to np arrays before use

---
 .../mdr_demo_lab_manager/scenario_states/verify_person.py    | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 747522917..939a0a6d0 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -39,9 +39,10 @@ def execute(self, userdata):
         known_people = self.kb_interface.get_obj_of_type(Person._type)
         unknown_person = self.kb_interface.get_obj_instance('person_0', Person._type)
 
+        unknown_person_embedding = np.array(unknown_person.face.views[0].embedding.embedding)
         for known_person in known_people:
-            distance = np.linalg.norm(known_person.face.views[0].embedding.embedding - \
-                                      unknown_person.face.views[0].embedding.embedding)
+            distance = np.linalg.norm(np.array(known_person.face.views[0].embedding.embedding) - \
+                                      unknown_person_embedding)
             if np.linalg.norm(distance) < self.threshold:
                 self.say("Welcome back {0}".format(known_person.name))
                 return 'known_person'

From 76c3cf391ace4040c92ab015c13502eabd23fd71 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 17:06:02 +0200
Subject: [PATCH 061/264] [find_people] Face embeddings can now be also
 computed

Provided that a path to an embedding model is specified
---
 .../ros/launch/find_people.launch             |  2 ++
 .../ros/src/mdr_find_people/action_states.py  | 20 ++++++++++++++++++-
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
index 8b30a8ee5..0f3acf8a2 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
@@ -1,8 +1,10 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="pointcloud_topic" default="/mdr_perception/rectified_points"/>
+    <arg name="face_embedding_model_path" default="/home/robot/models/face_embedding_model.pt"/>
 
     <node pkg="mdr_find_people" type="find_people_server" name="find_people_server" output="screen" >
         <param name="pointcloud_topic" type="string" value="$(arg pointcloud_topic)" />
+        <param name="face_embedding_model_path" type="string" value="$(arg face_embedding_model_path)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 1919b32bb..44b462917 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -1,6 +1,6 @@
 import rospy
 import smach
-import sympy
+import torch
 
 import tf
 import face_recognition
@@ -13,6 +13,8 @@
 from cv_bridge import CvBridge
 from mdf_find_people.find_people import FindPeople
 
+from dataset_interface.siamese_net.model import SiameseNetwork
+from dataset_interface.siamese_net.utils import get_grayscale_image_tensor
 
 class FindPeopleState(smach.State):
     def __init__(self):
@@ -23,7 +25,16 @@ def __init__(self):
 
         self._listener = tf.TransformListener()
         self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
+        self.face_embedding_model_path = rospy.get_param("~face_embedding_model_path", '')
 
+        self.face_embedding_model = None
+        if self.face_embedding_model_path:
+            self.face_embedding_model = SiameseNetwork()
+            self.face_embedding_model.load_state_dict(torch.load(self.face_embedding_model_path))
+            self.face_embedding_model.eval()
+
+            device = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu')
+            self.face_embedding_model.to(device)
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FIND_PEOPLE')
@@ -70,6 +81,13 @@ def execute(self, userdata):
 
             face_view = ObjectView()
             face_view.image = face_images[i]
+
+            if self.face_embedding_model is not None:
+                face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
+                grayscale_img = get_grayscale_image_tensor(face_cv2)
+                embedding = self.face_embedding_model.forward_once(grayscale_img)
+                face_view.embedding.embedding = embedding.tolist()
+
             p.face.views.append(face_view)
 
             pl.append(p)

From ded4e8af452d59252a67410204283ea7aba88b9f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 17:20:13 +0200
Subject: [PATCH 062/264] [find_people] Fixed wrong import

---
 .../mdr_find_people/ros/src/mdr_find_people/action_states.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 44b462917..3690f83c7 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -11,7 +11,7 @@
 from mas_perception_libs.visualization import crop_image
 from mas_perception_libs.utils import cloud_msg_to_cv_image
 from cv_bridge import CvBridge
-from mdf_find_people.find_people import FindPeople
+from mdr_find_people.find_people import FindPeople
 
 from dataset_interface.siamese_net.model import SiameseNetwork
 from dataset_interface.siamese_net.utils import get_grayscale_image_tensor

From c5da6d27f9d61b840d76c7ad62fc067244ffe2d5 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Fri, 14 Aug 2020 17:52:55 +0200
Subject: [PATCH 063/264] [verify_person] free up a space

---
 .../config/lab_manager_sm.yaml                |  3 ++
 .../scenario_states/get_user_data.py          |  9 ++++-
 .../scenario_states/verify_person.py          | 37 +++++++++++++++++--
 3 files changed, 43 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index f30c2eaf6..5a71a4ad9 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -68,6 +68,9 @@ state_descriptions:
             - argument:
                 name: threshold
                 value: 1
+            - argument:
+                name: timeout
+                value: 10.
     - state:
         name: TURN_PERSON_AWAY
         state_module_name: mdr_hri_behaviours.say_sentence
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index ecad31199..3c6b8ec42 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -12,7 +12,8 @@ def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'get_user_data',
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'failed', 
-                                             'failed_after_retrying'])
+                                             'failed_after_retrying'],
+                                   input_key=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'get_user_data')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
@@ -52,7 +53,11 @@ def execute(self, userdata):
                 # TODO Write the user data to knowledge base
                 person_0 = self.kb_interface.get_obj_instance('person_0', Person._type)
                 self.kb_interface.insert_obj_instance(new_entry[0], person_0)
-                self.kn_interface.remove_obj_instance('person_0', Person._type)
+                self.kb_interface.remove_obj_instance('person_0', Person._type)
+                # update the occupied spots
+                spot = userdata.destination_locations
+                occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value=new_entry[0]))
+                self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
                 return "succeeded"
             else:
                 time.sleep(self._loop_rate_s)
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 939a0a6d0..cd8ae21a6 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -21,11 +21,18 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.total_locations = kwargs.get('total_locations', 0)
         self.threshold = kwargs.get('threshold', 0)
         self.retry_count = 0
-        self.timeout = 120.
+        self.timeout = kwargs.get('timeout', 10.)
         occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
         if occupied_locations is None:
             self.kb_interface.insert_obj_instance('occupied_locations', DomainFormula())
 
+        # wait for listen action server
+        self.listen_client = actionlib.SimpleActionClient('listen_server', ListenAction)
+        listen_wait_result = self.listen_client.wait_for_server(timeout=rospy.Duration(self.timeout))
+        
+        if not listen_wait_result:
+            raise RuntimeError('Failed to wait for "listen_server" action')
+
     def execute(self, userdata):
         rospy.loginfo('[verify_person] Verifying person')
 
@@ -43,8 +50,30 @@ def execute(self, userdata):
         for known_person in known_people:
             distance = np.linalg.norm(np.array(known_person.face.views[0].embedding.embedding) - \
                                       unknown_person_embedding)
+
             if np.linalg.norm(distance) < self.threshold:
-                self.say("Welcome back {0}".format(known_person.name))
+                self.say("Hello {0}".format(known_person.name))
+                self.say("If you would like to free up your spot, please say goodbye.")
+                # wait for goodbye
+                goal = ListenGoal()
+
+                self.listen_client.send_goal(goal)
+                self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+                listen_state = self.listen_client.get_state()
+                listen_result = self.listen_client.get_result()
+
+                if listen_state == GoalStatus.SUCCEEDED and "bye" in listen_result.message:
+                    self.say("Goodbye! {0}. stay safe!".format(known_person.name))
+                    spot = known_person
+                    
+                    for occ_spot in occupied_locations.typed_parameters:
+                        if occ_spot.value == known_person.name:
+                            occupied_locations.typed_parameters.remove(occ_spot)
+
+                    self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+                    self.kb_interface.remove_obj_instance(known_person.name, Person._type)
+                  
+                # otherwise return to monitor door
                 return 'known_person'
 
         # No matching face, treat as new person
@@ -53,6 +82,6 @@ def execute(self, userdata):
             spots.remove(occ_spot)
         spot = random.choice(spots)
         userdata.destination_locations = ['spot_{0}'.format(spot)]
-        occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='true'))
-        self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+        # occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='true'))
+        # self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
         return 'new_person'

From 9a4ccb984e6c2e0cc9742ccbb8d3c6d2218996ff Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 14 Aug 2020 19:09:49 +0200
Subject: [PATCH 064/264] [find_people/action_states] Fixed the use of the
 embedding model

---
 .../ros/src/mdr_find_people/action_states.py    | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 3690f83c7..2979230c1 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -2,6 +2,8 @@
 import smach
 import torch
 
+from PIL import Image as PILImage
+import cv2
 import tf
 import face_recognition
 from sensor_msgs.msg import PointCloud2, Image
@@ -14,7 +16,7 @@
 from mdr_find_people.find_people import FindPeople
 
 from dataset_interface.siamese_net.model import SiameseNetwork
-from dataset_interface.siamese_net.utils import get_grayscale_image_tensor
+from dataset_interface.siamese_net.utils import get_transforms
 
 class FindPeopleState(smach.State):
     def __init__(self):
@@ -83,13 +85,18 @@ def execute(self, userdata):
             face_view.image = face_images[i]
 
             if self.face_embedding_model is not None:
+                if not face_view.image.data:
+                    continue
+
                 face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
-                grayscale_img = get_grayscale_image_tensor(face_cv2)
-                embedding = self.face_embedding_model.forward_once(grayscale_img)
-                face_view.embedding.embedding = embedding.tolist()
+                grayscale_img = cv2.cvtColor(face_cv2, cv2.COLOR_BGR2GRAY)
+                img_tensor = get_transforms()(PILImage.fromarray(grayscale_img))
+                img_tensor.unsqueeze_(0)
 
-            p.face.views.append(face_view)
+                embedding = self.face_embedding_model.forward_once(img_tensor)
+                face_view.embedding.embedding = embedding.detach().numpy().squeeze().tolist()
 
+            p.face.views.append(face_view)
             pl.append(p)
 
         # Package that actual PersonList message

From 7b37caebc640e937e39eeed7f7d44a7792b06c3c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 15 Aug 2020 11:13:20 +0200
Subject: [PATCH 065/264] [demo_lab_manager/verify_person] Added missing
 imports

---
 .../scenario_states/verify_person.py              | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index cd8ae21a6..3de10770e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -1,12 +1,17 @@
 import random
+import numpy as np
 
 import rospy
-import numpy as np
-from mas_execution_manager.scenario_state_base import ScenarioStateBase
+import actionlib
+from actionlib_msgs.msg import GoalStatus
+
 from rosplan_knowledge_msgs.msg import DomainFormula
+from mdr_listen_action.msg import ListenAction, ListenGoal
 from mas_perception_msgs.msg import Person
 from diagnostic_msgs.msg import KeyValue
 
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
 class VerifyPerson(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'verify_person',
@@ -29,7 +34,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         # wait for listen action server
         self.listen_client = actionlib.SimpleActionClient('listen_server', ListenAction)
         listen_wait_result = self.listen_client.wait_for_server(timeout=rospy.Duration(self.timeout))
-        
+
         if not listen_wait_result:
             raise RuntimeError('Failed to wait for "listen_server" action')
 
@@ -65,14 +70,14 @@ def execute(self, userdata):
                 if listen_state == GoalStatus.SUCCEEDED and "bye" in listen_result.message:
                     self.say("Goodbye! {0}. stay safe!".format(known_person.name))
                     spot = known_person
-                    
+
                     for occ_spot in occupied_locations.typed_parameters:
                         if occ_spot.value == known_person.name:
                             occupied_locations.typed_parameters.remove(occ_spot)
 
                     self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
                     self.kb_interface.remove_obj_instance(known_person.name, Person._type)
-                  
+
                 # otherwise return to monitor door
                 return 'known_person'
 

From 1892cfe38339bb879950b91fedffc5253d855674 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 16 Aug 2020 18:52:55 +0200
Subject: [PATCH 066/264] [demo_lab_manager/verify_person] Refactored to use
 the 'recognise_person' fn from the KB interface

The function was added with
https://github.com/b-it-bots/mas_knowledge_base/pull/39
---
 .../config/lab_manager_sm.yaml                |  2 +-
 .../scenario_states/verify_person.py          | 60 ++++++++-----------
 2 files changed, 27 insertions(+), 35 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 5a71a4ad9..8e2e6fe6d 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -66,7 +66,7 @@ state_descriptions:
                 name: total_locations
                 value: 7
             - argument:
-                name: threshold
+                name: person_recognition_threshold
                 value: 1
             - argument:
                 name: timeout
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 3de10770e..87943113f 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -24,7 +24,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.state_name = kwargs.get('state_name', 'verify_person')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.total_locations = kwargs.get('total_locations', 0)
-        self.threshold = kwargs.get('threshold', 0)
+        self.person_recognition_threshold = kwargs.get('person_recognition_threshold', 0)
         self.retry_count = 0
         self.timeout = kwargs.get('timeout', 10.)
         occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
@@ -42,44 +42,36 @@ def execute(self, userdata):
         rospy.loginfo('[verify_person] Verifying person')
 
         # Look for occupied spots
-        occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
+        occupied_locations = self.kb_interface.get_obj_instance('occupied_locations',
+                                                                DomainFormula._type)
         occ_spots = [kv.key for kv in occupied_locations.typed_parameters]
         if len(occupied_locations.typed_parameters) == self.total_locations:
             return 'no_empty_spot'
 
         # Try to match face to current people in the lab
-        known_people = self.kb_interface.get_obj_of_type(Person._type)
-        unknown_person = self.kb_interface.get_obj_instance('person_0', Person._type)
-
-        unknown_person_embedding = np.array(unknown_person.face.views[0].embedding.embedding)
-        for known_person in known_people:
-            distance = np.linalg.norm(np.array(known_person.face.views[0].embedding.embedding) - \
-                                      unknown_person_embedding)
-
-            if np.linalg.norm(distance) < self.threshold:
-                self.say("Hello {0}".format(known_person.name))
-                self.say("If you would like to free up your spot, please say goodbye.")
-                # wait for goodbye
-                goal = ListenGoal()
-
-                self.listen_client.send_goal(goal)
-                self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
-                listen_state = self.listen_client.get_state()
-                listen_result = self.listen_client.get_result()
-
-                if listen_state == GoalStatus.SUCCEEDED and "bye" in listen_result.message:
-                    self.say("Goodbye! {0}. stay safe!".format(known_person.name))
-                    spot = known_person
-
-                    for occ_spot in occupied_locations.typed_parameters:
-                        if occ_spot.value == known_person.name:
-                            occupied_locations.typed_parameters.remove(occ_spot)
-
-                    self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
-                    self.kb_interface.remove_obj_instance(known_person.name, Person._type)
-
-                # otherwise return to monitor door
-                return 'known_person'
+        recognised_person = self.kb_interface.recognise_person('person_0',
+                                                               Person._type,
+                                                               self.person_recognition_threshold)
+        if recognised_person is not None:
+            self.say("Hello {0}".format(recognised_person.name))
+            self.say("If you would like to free up your spot, please say goodbye.")
+
+            # wait for goodbye
+            goal = ListenGoal()
+            self.listen_client.send_goal(goal)
+            self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+            listen_state = self.listen_client.get_state()
+            listen_result = self.listen_client.get_result()
+
+            if listen_state == GoalStatus.SUCCEEDED and "bye" in listen_result.message:
+                self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
+                for occ_spot in occupied_locations.typed_parameters:
+                    if occ_spot.value == recognised_person.name:
+                        occupied_locations.typed_parameters.remove(occ_spot)
+                self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+
+            # otherwise return to monitor door
+            return 'known_person'
 
         # No matching face, treat as new person
         spots = [str(i+1) for i in range(self.total_locations)]

From f676af297d013f9cda14566a41f062947c9378c9 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 17 Aug 2020 20:31:21 +0200
Subject: [PATCH 067/264] [scenarios] Added a metapackage mdr_utility_tasks
 for, well, small utility scenarios

---
 .../mdr_utility_tasks/CMakeLists.txt             |  4 ++++
 .../mdr_utility_tasks/package.xml                | 16 ++++++++++++++++
 2 files changed, 20 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/package.xml

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/CMakeLists.txt
new file mode 100644
index 000000000..c186d8e47
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_utility_tasks)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/package.xml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/package.xml
new file mode 100644
index 000000000..5962a60ff
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_utility_tasks/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_utility_tasks</name>
+  <version>1.0.0</version>
+  <description>A metapackage for utility tasks (such as learning data collection or populating a knowledge base)</description>
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

From 1efde56a35d80e5b5e4fe2f538ab95cef82da574 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 17 Aug 2020 20:34:56 +0200
Subject: [PATCH 068/264] [utility_tasks] Added a scenario for collecting
 images of an unknown person

---
 .../mdr_util_register_person/CMakeLists.txt   |  25 +++
 .../config/register_person_sm.yaml            | 176 ++++++++++++++++++
 .../mdr_util_register_person/package.xml      |  27 +++
 .../ros/launch/register_person.launch         |  10 +
 .../src/mdr_util_register_person/__init__.py  |   0
 .../scenario_states/__init__.py               |   0
 .../scenario_states/store_person.py           |  46 +++++
 .../mdr_util_register_person/setup.py         |  11 ++
 8 files changed, 295 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
new file mode 100644
index 000000000..4f28aa652
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_util_register_person)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  mdr_detect_person
+  mas_execution_manager
+  mdr_perception_behaviours
+  mas_knowledge_base
+)
+
+catkin_python_setup()
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mdr_detect_person
+    mas_execution_manager
+    mdr_perception_behaviours
+    mas_knowledge_base
+)
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
new file mode 100644
index 000000000..e83dce92a
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
@@ -0,0 +1,176 @@
+# Description: Defines a state machine for a scenario in which a robot "learns" a person by
+# collecting images of the person and storing those in its permanent knowledge base.
+# Author: Alex Mitrevski
+# Email: aleksandar.mitrevski@h-brs.de
+sm_id: mdr_util_register_person
+states: [GIVE_INSTRUCTIONS, GIVE_FRONT_INSTRUCTIONS, TAKE_FRONT_PICTURE, STORE_FRONT_PICTURE, GIVE_LEFT_INSTRUCTIONS, TAKE_LEFT_PICTURE, STORE_LEFT_PICTURE, GIVE_RIGHT_INSTRUCTIONS, TAKE_RIGHT_PICTURE, STORE_RIGHT_PICTURE, SAY_THANKS]
+outcomes: [DONE, FAILED]
+state_descriptions:
+    - state:
+        name: GIVE_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: GIVE_FRONT_INSTRUCTIONS
+        arguments:
+            - argument:
+                name: sentences
+                value: ['I would like to take a few pictures of you so that I can learn your face.', 'Please stand in front of my camera and follow my instructions.']
+    - state:
+        name: GIVE_FRONT_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: TAKE_FRONT_PICTURE
+        arguments:
+            - argument:
+                name: sentences
+                value: ['Please keep your face straight towards me.']
+    - state:
+        name: TAKE_FRONT_PICTURE
+        state_module_name: mdr_perception_behaviours.find_people
+        state_class_name: FindPeople
+        transitions:
+            - transition:
+                name: succeeded
+                state: STORE_FRONT_PICTURE
+            - transition:
+                name: failed
+                state: TAKE_FRONT_PICTURE
+            - transition:
+                name: failed_after_retrying
+                state: TAKE_FRONT_PICTURE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 0
+    - state:
+        name: STORE_FRONT_PICTURE
+        state_module_name: mdr_util_register_person.scenario_states.store_person
+        state_class_name: StorePerson
+        transitions:
+            - transition:
+                name: succeeded
+                state: GIVE_LEFT_INSTRUCTIONS
+            - transition:
+                name: failed
+                state: FAILED
+            - transition:
+                name: retake_picture
+                state: TAKE_FRONT_PICTURE
+        arguments:
+            - argument:
+                name: person_msg_id
+                value: person_0
+    - state:
+        name: GIVE_LEFT_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: TAKE_LEFT_PICTURE
+        arguments:
+            - argument:
+                name: sentences
+                value: ['Please turn your face so that it's facing a bit to the left.']
+    - state:
+        name: TAKE_LEFT_PICTURE
+        state_module_name: mdr_perception_behaviours.find_people
+        state_class_name: FindPeople
+        transitions:
+            - transition:
+                name: succeeded
+                state: STORE_LEFT_PICTURE
+            - transition:
+                name: failed
+                state: TAKE_LEFT_PICTURE
+            - transition:
+                name: failed_after_retrying
+                state: TAKE_LEFT_PICTURE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 0
+    - state:
+        name: STORE_LEFT_PICTURE
+        state_module_name: mdr_util_register_person.scenario_states.store_person
+        state_class_name: StorePerson
+        transitions:
+            - transition:
+                name: succeeded
+                state: GIVE_RIGHT_INSTRUCTIONS
+            - transition:
+                name: failed
+                state: FAILED
+            - transition:
+                name: retake_picture
+                state: TAKE_LEFT_PICTURE
+        arguments:
+            - argument:
+                name: person_msg_id
+                value: person_0
+    - state:
+        name: GIVE_RIGHT_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: TAKE_RIGHT_PICTURE
+        arguments:
+            - argument:
+                name: sentences
+                value: ['Please turn your face so that it's facing a bit to the right.']
+    - state:
+        name: TAKE_RIGHT_PICTURE
+        state_module_name: mdr_perception_behaviours.find_people
+        state_class_name: FindPeople
+        transitions:
+            - transition:
+                name: succeeded
+                state: STORE_RIGHT_PICTURE
+            - transition:
+                name: failed
+                state: TAKE_RIGHT_PICTURE
+            - transition:
+                name: failed_after_retrying
+                state: TAKE_RIGHT_PICTURE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 0
+    - state:
+        name: STORE_RIGHT_PICTURE
+        state_module_name: mdr_util_register_person.scenario_states.store_person
+        state_class_name: StorePerson
+        transitions:
+            - transition:
+                name: succeeded
+                state: SAY_THANKS
+            - transition:
+                name: failed
+                state: FAILED
+            - transition:
+                name: retake_picture
+                state: TAKE_RIGHT_PICTURE
+        arguments:
+            - argument:
+                name: person_msg_id
+                value: person_0
+    - state:
+        name: SAY_THANKS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: DONE
+        arguments:
+            - argument:
+                name: sentences
+                value: ['I have now learned your face. Thanks for the help!']
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
new file mode 100644
index 000000000..9b21bc28d
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_util_register_person</name>
+  <version>1.0.0</version>
+  <description>
+    A package for a task in which a robot registers a previuosly unknown person
+    by collecting images of them and storing those in the knowledge base.
+  </description>
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>mdr_detect_person</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+  <build_depend>mdr_perception_behaviours</build_depend>
+  <build_depend>mas_knowledge_base</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>mdr_detect_person</run_depend>
+  <run_depend>mas_execution_manager</run_depend>
+  <run_depend>mdr_perception_behaviours</run_depend>
+  <run_depend>mas_knowledge_base</run_depend>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
new file mode 100644
index 000000000..23dc1e5aa
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Actions -->
+    <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
+
+    <!-- State machine -->
+    <node pkg="mas_execution_manager" type="state_machine_creator" name="register_person" output="screen">
+        <param name="sm_config_file" type="str" value="$(find mdr_util_register_person)/config/register_person_sm.yaml" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/__init__.py b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
new file mode 100644
index 000000000..df1f28f55
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
@@ -0,0 +1,46 @@
+from mas_perception_msgs.msg import Person
+
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class StorePerson(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'store_person',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'retake_picture', 'failed'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'store_person')
+        self.person_msg_id = kwargs.get('person_msg_id', 'person_0')
+
+    def execute(self, userdata):
+        person = self.kb_interface.get_obj_instance(self.person_msg_id,
+                                                    Person._type)
+        if person is None:
+            self.say('Sorry, I got confused for some reason, we will have to stop the procedure.')
+            return 'failed'
+
+        if not person.face.views[0].embedding.embedding:
+            self.say('I unfortunately could not see your face.')
+            self.say('Let us try taking the picture again.')
+            return 'retake_picture'
+
+        person_permanent_storage = self.kb_interface.get_obj_instance(self.person_msg_id,
+                                                                      Person._type,
+                                                                      permanent_storage=True)
+
+        # if we already have the person in permanent storage, we add
+        # the most recent view of the face to the list of face views;
+        # if not, we just insert the complete person message
+        # to permanent storage so that we can update it later
+        if person_permanent_storage is not None:
+            person_permanent_storage.face.views.append(person.face.views[0])
+            self.kb_interface.update_obj_instance(self.person_msg_id,
+                                                  person_permanent_storage,
+                                                  permanent_storage=True)
+        else:
+            self.kb_interface.insert_obj_instance(self.person_msg_id,
+                                                  person,
+                                                  permanent_storage=True)
+
+        # we remove the person message from temporary storage
+        self.kb_interface.remove_obj_instance(self.person_msg_id, Person._type)
+        return 'succeeded'
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/setup.py b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/setup.py
new file mode 100644
index 000000000..32cee255c
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_util_register_person'],
+    package_dir={'mdr_util_register_person': 'ros/src/mdr_util_register_person'}
+)
+
+setup(**d)

From 5edd73301acc6944079439cefa1ed691689ffdf0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 07:29:54 +0200
Subject: [PATCH 069/264] [util_register_person] Added a missing dependency

---
 .../mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt   | 2 ++
 .../mdr_utility_tasks/mdr_util_register_person/package.xml      | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
index 4f28aa652..1c3b1cff6 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   mdr_detect_person
   mas_execution_manager
   mdr_perception_behaviours
+  mdr_hri_behaviours
   mas_knowledge_base
 )
 
@@ -17,6 +18,7 @@ catkin_package(
     mdr_detect_person
     mas_execution_manager
     mdr_perception_behaviours
+    mdr_hri_behaviours
     mas_knowledge_base
 )
 
diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
index 9b21bc28d..596fec4c7 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/package.xml
@@ -17,11 +17,13 @@
   <build_depend>mdr_detect_person</build_depend>
   <build_depend>mas_execution_manager</build_depend>
   <build_depend>mdr_perception_behaviours</build_depend>
+  <build_depend>mdr_hri_behaviours</build_depend>
   <build_depend>mas_knowledge_base</build_depend>
 
   <run_depend>rospy</run_depend>
   <run_depend>mdr_detect_person</run_depend>
   <run_depend>mas_execution_manager</run_depend>
   <run_depend>mdr_perception_behaviours</run_depend>
+  <run_depend>mdr_hri_behaviours</run_depend>
   <run_depend>mas_knowledge_base</run_depend>
 </package>

From f67e94b41a428210b052f49a8ff3f18516de38f2 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 11:42:55 +0200
Subject: [PATCH 070/264] [util_register_person/sm] Fixed YAML error (unescaped
 single quote)

---
 .../mdr_util_register_person/config/register_person_sm.yaml   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
index e83dce92a..f9d1783b7 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
@@ -77,7 +77,7 @@ state_descriptions:
         arguments:
             - argument:
                 name: sentences
-                value: ['Please turn your face so that it's facing a bit to the left.']
+                value: ['Please turn your face so that it is facing a bit to the left.']
     - state:
         name: TAKE_LEFT_PICTURE
         state_module_name: mdr_perception_behaviours.find_people
@@ -125,7 +125,7 @@ state_descriptions:
         arguments:
             - argument:
                 name: sentences
-                value: ['Please turn your face so that it's facing a bit to the right.']
+                value: ['Please turn your face so that it is facing a bit to the right.']
     - state:
         name: TAKE_RIGHT_PICTURE
         state_module_name: mdr_perception_behaviours.find_people

From 72d3918d41efd610a8df971ed492a8ba9f08e69a Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 14:01:25 +0200
Subject: [PATCH 071/264] [find_people/action_states] Fixed bug in which
 detected people weren't added to the list if there was no face image

---
 .../mdr_find_people/ros/src/mdr_find_people/action_states.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 2979230c1..ae281b684 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -84,10 +84,7 @@ def execute(self, userdata):
             face_view = ObjectView()
             face_view.image = face_images[i]
 
-            if self.face_embedding_model is not None:
-                if not face_view.image.data:
-                    continue
-
+            if self.face_embedding_model is not None and face_view.image.data:
                 face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
                 grayscale_img = cv2.cvtColor(face_cv2, cv2.COLOR_BGR2GRAY)
                 img_tensor = get_transforms()(PILImage.fromarray(grayscale_img))

From 21b56007feccfd05d2693543d2ebb42be3798a8d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 14:22:02 +0200
Subject: [PATCH 072/264] [util_register_person/store_person] Removing the
 person msg on failure as well

---
 .../mdr_util_register_person/scenario_states/store_person.py    | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
index df1f28f55..a83b8067c 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/src/mdr_util_register_person/scenario_states/store_person.py
@@ -16,11 +16,13 @@ def execute(self, userdata):
                                                     Person._type)
         if person is None:
             self.say('Sorry, I got confused for some reason, we will have to stop the procedure.')
+            self.kb_interface.remove_obj_instance(self.person_msg_id, Person._type)
             return 'failed'
 
         if not person.face.views[0].embedding.embedding:
             self.say('I unfortunately could not see your face.')
             self.say('Let us try taking the picture again.')
+            self.kb_interface.remove_obj_instance(self.person_msg_id, Person._type)
             return 'retake_picture'
 
         person_permanent_storage = self.kb_interface.get_obj_instance(self.person_msg_id,

From 60cbffeb0991847e40bc1ed654512885e36bbb37 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 16:55:11 +0200
Subject: [PATCH 073/264] [util_register_person/register_person.launch]
 Included the rosplan and find people action launchers

---
 .../mdr_util_register_person/ros/launch/register_person.launch | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
index 23dc1e5aa..6aec52eed 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/ros/launch/register_person.launch
@@ -1,6 +1,9 @@
 <?xml version="1.0"?>
 <launch>
+    <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch" />
+
     <!-- Actions -->
+    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
     <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
 
     <!-- State machine -->

From fb3aa0c82fc558b4011af3cca7a640cd35d612de Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 17:18:49 +0200
Subject: [PATCH 074/264] [util_register_person/sm] Corrected instruction
 sentence

To use the SaySentence state correctly, I am now passing just one
sentence to the state instead of two
---
 .../mdr_util_register_person/config/register_person_sm.yaml     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
index f9d1783b7..a02fb7006 100644
--- a/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_utility_tasks/mdr_util_register_person/config/register_person_sm.yaml
@@ -17,7 +17,7 @@ state_descriptions:
         arguments:
             - argument:
                 name: sentences
-                value: ['I would like to take a few pictures of you so that I can learn your face.', 'Please stand in front of my camera and follow my instructions.']
+                value: ['I would like to take a few pictures of you so that I can learn your face. Please stand in front of my camera and follow my instructions.']
     - state:
         name: GIVE_FRONT_INSTRUCTIONS
         state_module_name: mdr_hri_behaviours.say_sentence

From 633fd2ca12df959f89282d776b5c19b5e8cdb611 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 18 Aug 2020 18:47:07 +0200
Subject: [PATCH 075/264] [demo_lab_manager/get_user_data] __init__: input_key
 -> input_keys

---
 .../mdr_demo_lab_manager/scenario_states/get_user_data.py   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index 3c6b8ec42..45a55d26a 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -11,9 +11,9 @@ class GetUserData(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'get_user_data',
                                    save_sm_state=save_sm_state,
-                                   outcomes=['succeeded', 'failed', 
+                                   outcomes=['succeeded', 'failed',
                                              'failed_after_retrying'],
-                                   input_key=['destination_locations'])
+                                   input_keys=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'get_user_data')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
@@ -22,7 +22,7 @@ def __init__(self, save_sm_state=False, **kwargs):
 
         self._sheet_id = kwargs.get('sheet_id', '')
         self._worksheet_name = kwargs.get('worksheet_name', 'responses')
-        
+
         self._num_known_entries = None
         self._loop_rate_s = kwargs.get('loop_rate_s', 2.)
 

From 626215c9db65d42ce0e7aa0ba0a19b567cbb9284 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 12:38:32 +0200
Subject: [PATCH 076/264] [lab_manager] added debig message

---
 .../scenario_states/verify_person.py               | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 87943113f..3981ea45a 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -63,12 +63,14 @@ def execute(self, userdata):
             listen_state = self.listen_client.get_state()
             listen_result = self.listen_client.get_result()
 
-            if listen_state == GoalStatus.SUCCEEDED and "bye" in listen_result.message:
-                self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
-                for occ_spot in occupied_locations.typed_parameters:
-                    if occ_spot.value == recognised_person.name:
-                        occupied_locations.typed_parameters.remove(occ_spot)
-                self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+            if listen_state == GoalStatus.SUCCEEDED:
+                rospy.loginfo("[MESSAGE] {}".format(listen_result.message))
+                if "bye" in listen_result.message:
+                    self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
+                    for occ_spot in occupied_locations.typed_parameters:
+                        if occ_spot.value == recognised_person.name:
+                            occupied_locations.typed_parameters.remove(occ_spot)
+                    self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
 
             # otherwise return to monitor door
             return 'known_person'

From a48ad1356b49a20b8f4cc391bb958ac9d3ae342e Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 12:43:25 +0200
Subject: [PATCH 077/264] [lab_manager/verify_person] added sleep after
 instruction

---
 .../src/mdr_demo_lab_manager/scenario_states/verify_person.py   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 3981ea45a..df99787f2 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -55,7 +55,7 @@ def execute(self, userdata):
         if recognised_person is not None:
             self.say("Hello {0}".format(recognised_person.name))
             self.say("If you would like to free up your spot, please say goodbye.")
-
+            rospy.sleep(3)
             # wait for goodbye
             goal = ListenGoal()
             self.listen_client.send_goal(goal)

From 4d4aec7e1c875453b4b666a525ce808de4f8c90a Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 12:52:54 +0200
Subject: [PATCH 078/264] [lab_manager/verify_person] added list of words that
 sound similar to 'bye'

---
 .../mdr_demo_lab_manager/scenario_states/verify_person.py    | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index df99787f2..87f66744e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -27,6 +27,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.person_recognition_threshold = kwargs.get('person_recognition_threshold', 0)
         self.retry_count = 0
         self.timeout = kwargs.get('timeout', 10.)
+        self.bye = ['bye', 'by', 'buy', 'bi']
         occupied_locations = self.kb_interface.get_obj_instance('occupied_locations', DomainFormula._type)
         if occupied_locations is None:
             self.kb_interface.insert_obj_instance('occupied_locations', DomainFormula())
@@ -55,7 +56,7 @@ def execute(self, userdata):
         if recognised_person is not None:
             self.say("Hello {0}".format(recognised_person.name))
             self.say("If you would like to free up your spot, please say goodbye.")
-            rospy.sleep(3)
+            rospy.sleep(4)
             # wait for goodbye
             goal = ListenGoal()
             self.listen_client.send_goal(goal)
@@ -65,7 +66,7 @@ def execute(self, userdata):
 
             if listen_state == GoalStatus.SUCCEEDED:
                 rospy.loginfo("[MESSAGE] {}".format(listen_result.message))
-                if "bye" in listen_result.message:
+                if any(word in listen_result.message for word in self.bye):
                     self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
                     for occ_spot in occupied_locations.typed_parameters:
                         if occ_spot.value == recognised_person.name:

From 0e2ff0d2c824577aea382799afa566d837960937 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 12:56:15 +0200
Subject: [PATCH 079/264] [lab_manager/verify_person] extended the sleep before
 listening for goodbye

---
 .../src/mdr_demo_lab_manager/scenario_states/verify_person.py   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 87f66744e..235db5fbb 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -56,7 +56,7 @@ def execute(self, userdata):
         if recognised_person is not None:
             self.say("Hello {0}".format(recognised_person.name))
             self.say("If you would like to free up your spot, please say goodbye.")
-            rospy.sleep(4)
+            rospy.sleep(5)
             # wait for goodbye
             goal = ListenGoal()
             self.listen_client.send_goal(goal)

From a4a15bcf6995128f5db5b2a48ae72c3520040307 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 13:25:14 +0200
Subject: [PATCH 080/264] [lab_manager/verify_person] modified the state
 machine and checked for logged users in verify person

---
 .../config/lab_manager_sm.yaml                |  2 +-
 .../scenario_states/verify_person.py          | 49 +++++++++++--------
 2 files changed, 30 insertions(+), 21 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 8e2e6fe6d..e1255f0ad 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -60,7 +60,7 @@ state_descriptions:
                 state: MONITOR_DOOR
             - transition:
                 name: known_person
-                state: MONITOR_DOOR
+                state: GUIDE_TO_EMPTY_SPOT
         arguments:
             - argument:
                 name: total_locations
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 235db5fbb..bc2da1812 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -53,28 +53,37 @@ def execute(self, userdata):
         recognised_person = self.kb_interface.recognise_person('person_0',
                                                                Person._type,
                                                                self.person_recognition_threshold)
-        if recognised_person is not None:
-            self.say("Hello {0}".format(recognised_person.name))
-            self.say("If you would like to free up your spot, please say goodbye.")
-            rospy.sleep(5)
-            # wait for goodbye
-            goal = ListenGoal()
-            self.listen_client.send_goal(goal)
-            self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
-            listen_state = self.listen_client.get_state()
-            listen_result = self.listen_client.get_result()
+        # Check if the person is logged
+        person_logged = False
+        for occ_spot in occupied_locations.typed_parameters:
+                if occ_spot.value == recognised_person.name:
+                    person_logged = True
 
-            if listen_state == GoalStatus.SUCCEEDED:
-                rospy.loginfo("[MESSAGE] {}".format(listen_result.message))
-                if any(word in listen_result.message for word in self.bye):
-                    self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
-                    for occ_spot in occupied_locations.typed_parameters:
-                        if occ_spot.value == recognised_person.name:
-                            occupied_locations.typed_parameters.remove(occ_spot)
-                    self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+        if recognised_person is not None:
+            if person_logged:
+                self.say("Hello {0}".format(recognised_person.name))
+                self.say("If you would like to free up your spot, please say goodbye.")
+                rospy.sleep(5)
+                # wait for goodbye
+                goal = ListenGoal()
+                self.listen_client.send_goal(goal)
+                self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+                listen_state = self.listen_client.get_state()
+                listen_result = self.listen_client.get_result()
 
-            # otherwise return to monitor door
-            return 'known_person'
+                if listen_state == GoalStatus.SUCCEEDED:
+                    rospy.loginfo("[MESSAGE] {}".format(listen_result.message))
+                    if any(word in listen_result.message for word in self.bye):
+                        self.say("Goodbye! {0}. stay safe!".format(recognised_person.name))
+                        for occ_spot in occupied_locations.typed_parameters:
+                            if occ_spot.value == recognised_person.name:
+                                occupied_locations.typed_parameters.remove(occ_spot)
+                        self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+                    # return to monitor door
+                    return "already_logged_person"
+            else:
+                # guide to empty spot
+                return 'known_person'
 
         # No matching face, treat as new person
         spots = [str(i+1) for i in range(self.total_locations)]

From b9e6d17a034917e4e77f46b6a5736dccf4d8d196 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 13:36:26 +0200
Subject: [PATCH 081/264] [lab_manager/verify_person] fixed wrong indentation

---
 .../src/mdr_demo_lab_manager/scenario_states/verify_person.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index bc2da1812..b73fe5c2d 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -56,8 +56,8 @@ def execute(self, userdata):
         # Check if the person is logged
         person_logged = False
         for occ_spot in occupied_locations.typed_parameters:
-                if occ_spot.value == recognised_person.name:
-                    person_logged = True
+            if occ_spot.value == recognised_person.name:
+                person_logged = True
 
         if recognised_person is not None:
             if person_logged:

From f1d8daf2104a4c040cde114305403ea0282fa5d7 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 13:45:50 +0200
Subject: [PATCH 082/264] [find_people] Fixed import for draw_labeled_boxes
 (from mas_perception_libs)

---
 .../mdr_find_people/ros/src/mdr_find_people/find_people.py     | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index ee4485a64..d84e96154 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -9,7 +9,8 @@
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz, draw_labeled_boxes
+                                      crop_cloud_to_xyz
+from mas_perception_libs.visualization import draw_labeled_boxes
 from ssd_keras_ros import SSDKerasObjectDetector
 
 

From f0f8e47178f2ecf59146a5c873dd2d721470008b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 15:45:21 +0200
Subject: [PATCH 083/264] [environments/brsu-c069] Added poses for the lab
 manager scenario

---
 mdr_environments/brsu-c069/navigation_goals.yaml | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/mdr_environments/brsu-c069/navigation_goals.yaml b/mdr_environments/brsu-c069/navigation_goals.yaml
index 219dccccf..7f28b9895 100644
--- a/mdr_environments/brsu-c069/navigation_goals.yaml
+++ b/mdr_environments/brsu-c069/navigation_goals.yaml
@@ -12,3 +12,13 @@ observation_pose: [-0.386269, -0.149052, -0.013196]
 trash_can: [-1.991786, -3.080515, -1.644647]
 shelf_cupboard: [4.372, -3.300, 3.113]
 trash_bin1: [6.55, -3.75, -0.00277]
+
+monitoring_pose: [-1.249, -2.924, -2.888]
+spot_1: [-0.229, 0.517, -2.784]
+spot_2: [-0.263, 0.756, 0.548]
+spot_3: [0.601, -2.074, 0.823]
+spot_4: [4.238, -1.575, 1.468]
+spot_5: [6.388, -0.584, 1.506]
+spot_6: [6.846, -0.826, -1.132]
+spot_7: [8.274, -3.791, 0.778]
+disinfection_table: [7.095, -3.350, 3.132]

From 1bcaee1a39c707adbdd47e73080a8c3f3f9d8696 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 15:53:51 +0200
Subject: [PATCH 084/264] [find_people/action_states] Removed grayscale img
 conversion before calculating a face embedding

---
 .../mdr_find_people/ros/src/mdr_find_people/action_states.py   | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index ae281b684..735acffd4 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -86,8 +86,7 @@ def execute(self, userdata):
 
             if self.face_embedding_model is not None and face_view.image.data:
                 face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
-                grayscale_img = cv2.cvtColor(face_cv2, cv2.COLOR_BGR2GRAY)
-                img_tensor = get_transforms()(PILImage.fromarray(grayscale_img))
+                img_tensor = get_transforms()(PILImage.fromarray(face_cv2))
                 img_tensor.unsqueeze_(0)
 
                 embedding = self.face_embedding_model.forward_once(img_tensor)

From 4ff7da6564e21a8d0d8494cc550a96664a2d0bbc Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 16:06:06 +0200
Subject: [PATCH 085/264] [demo_lab_manager/verify_person] Assigned sitting
 spot also in the case of a known person entering the lab

---
 .../scenario_states/verify_person.py               | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index b73fe5c2d..dc8d96bd4 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -83,14 +83,20 @@ def execute(self, userdata):
                     return "already_logged_person"
             else:
                 # guide to empty spot
+                spot = self.choose_sitting_spot(occ_spots)
+                userdata.destination_locations = ['spot_{0}'.format(spot)]
                 return 'known_person'
 
         # No matching face, treat as new person
-        spots = [str(i+1) for i in range(self.total_locations)]
-        for occ_spot in occ_spots:
-            spots.remove(occ_spot)
-        spot = random.choice(spots)
+        spot = self.choose_sitting_spot(occ_spots)
         userdata.destination_locations = ['spot_{0}'.format(spot)]
         # occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='true'))
         # self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
         return 'new_person'
+
+    def choose_sitting_spot(self, occ_spots):
+        spots = [str(i+1) for i in range(self.total_locations)]
+        for occ_spot in occ_spots:
+            spots.remove(occ_spot)
+        spot = random.choice(spots)
+        return spot

From f9249bc38f65abbdac7193c820c36636ffd79910 Mon Sep 17 00:00:00 2001
From: samuelpg <9706510+samuelpg@users.noreply.github.com>
Date: Sat, 22 Aug 2020 16:09:17 +0200
Subject: [PATCH 086/264] [lab_manager/verify_person] fixed no salutation bug

---
 .../src/mdr_demo_lab_manager/scenario_states/verify_person.py   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index dc8d96bd4..f5c18760a 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -60,8 +60,8 @@ def execute(self, userdata):
                 person_logged = True
 
         if recognised_person is not None:
+            self.say("Hello {0}".format(recognised_person.name))
             if person_logged:
-                self.say("Hello {0}".format(recognised_person.name))
                 self.say("If you would like to free up your spot, please say goodbye.")
                 rospy.sleep(5)
                 # wait for goodbye

From e381ee1888160b8ef13cf466ba0f351efdf872e6 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 16:45:40 +0200
Subject: [PATCH 087/264] [demo_lab_manager] Added a state for sending a
 disinfection cmd

---
 .../config/lab_manager_sm.yaml                | 44 +++++++++++++++++--
 .../scenario_states/perform_disinfection.py   | 25 +++++++++++
 2 files changed, 65 insertions(+), 4 deletions(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index e1255f0ad..e4a135213 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -2,7 +2,7 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_lab_manager
-states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, TURN_PERSON_AWAY, GUIDE_TO_EMPTY_SPOT]
+states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, TURN_PERSON_AWAY, GO_TO_DISINFECTION_SPOT, PERFORM_DISINFECTION, GUIDE_TO_EMPTY_SPOT]
 outcomes: [DONE, FAILED]
 state_descriptions:
     - state:
@@ -60,7 +60,7 @@ state_descriptions:
                 state: MONITOR_DOOR
             - transition:
                 name: known_person
-                state: GUIDE_TO_EMPTY_SPOT
+                state: GO_TO_DISINFECTION_SPOT
         arguments:
             - argument:
                 name: total_locations
@@ -90,13 +90,13 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: GUIDE_TO_EMPTY_SPOT
+                state: GO_TO_DISINFECTION_SPOT
             - transition:
                 name: failed
                 state: ASK_NAME
             - transition:
                 name: failed_after_retrying
-                state: GUIDE_TO_EMPTY_SPOT
+                state: GO_TO_DISINFECTION_SPOT
         arguments:
             - argument:
                 name: number_of_retries
@@ -110,6 +110,42 @@ state_descriptions:
             - argument:
                 name: loop_rate_s
                 value: 2.0
+    - state:
+        name: GO_TO_DISINFECTION_SPOT
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: PERFORM_DISINFECTION
+            - transition:
+                name: failed
+                state: GO_TO_DISINFECTION_SPOT
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: destination_locations
+                value: [disinfection_table]
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: PERFORM_DISINFECTION
+        state_module_name: mdr_demo_lab_manager.scenario_states.perform_disinfection
+        state_class_name: PerformDisinfection
+        transitions:
+            - transition:
+                name: succeeded
+                state: GUIDE_TO_EMPTY_SPOT
+        arguments:
+            - argument:
+                name: disinfection_cmd_topic
+                value: /control_robot/event_in
+            - argument:
+                name: disinfection_duration
+                value: 20.0
     - state:
         name: GUIDE_TO_EMPTY_SPOT
         state_module_name: mdr_navigation_behaviours.move_base
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
new file mode 100644
index 000000000..192ebec32
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
@@ -0,0 +1,25 @@
+import rospy
+from std_msgs.msg import Empty
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class PerformDisinfection(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'perform_disinfection',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'perform_disinfection')
+        self.disinfection_cmd_topic = kwargs.get('disinfection_cmd_topic',
+                                                 '/disinfect_cmd')
+        self.disinfection_duration = kwargs.get('disinfection_duration', 20.)
+
+        self.disinfection_cmd_pub = None
+        try:
+            self.disinfection_cmd_pub = rospy.Publisher(self.disinfection_cmd_topic,
+                                                        Empty, queue_size=1)
+        except:
+            rospy.logerr('[send_disinfection_command] Could not create a disinfection command publisher')
+
+    def execute(self, userdata):
+        self.disinfection_cmd_pub.publish(Empty())
+        rospy.sleep(self.disinfection_duration)

From 0e41b6df32b8caf7d7e4de604bfa25b3ec80633e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 16:55:37 +0200
Subject: [PATCH 088/264] [demo_lab_manager] Recover in case a person is
 detected, but their face is not seen

---
 .../mdr_demo_lab_manager/config/lab_manager_sm.yaml        | 3 +++
 .../mdr_demo_lab_manager/scenario_states/verify_person.py  | 7 ++++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index e4a135213..3293f37ec 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -61,6 +61,9 @@ state_descriptions:
             - transition:
                 name: known_person
                 state: GO_TO_DISINFECTION_SPOT
+            - transition:
+                name: face_not_seen
+                state: MONITOR_DOOR
         arguments:
             - argument:
                 name: total_locations
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index f5c18760a..8f1d0a78e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -18,7 +18,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    save_sm_state=save_sm_state,
                                    outcomes=['no_empty_spot', 'new_person',
                                              'already_logged_person',
-                                             'known_person'],
+                                             'known_person', 'face_not_seen'],
                                    output_keys=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'verify_person')
@@ -49,6 +49,11 @@ def execute(self, userdata):
         if len(occupied_locations.typed_parameters) == self.total_locations:
             return 'no_empty_spot'
 
+        person = self.kb_interface.get_obj_instance('person_0', Person._type)
+        if not person.face.views or not person.face.views[0].image.data:
+            self.say('I could not see your face. Could you please look at me briefly?')
+            return 'face_not_seen'
+
         # Try to match face to current people in the lab
         recognised_person = self.kb_interface.recognise_person('person_0',
                                                                Person._type,

From 0a8ba2263c6a2e8ff9ecce14949fd27cafb02e77 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 17:07:36 +0200
Subject: [PATCH 089/264] [demo_lab_manager/perform_disinfection] Added
 instructions for disinfection and a missing transition

---
 .../scenario_states/perform_disinfection.py                     | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
index 192ebec32..61e0ae0be 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
@@ -22,4 +22,6 @@ def __init__(self, save_sm_state=False, **kwargs):
 
     def execute(self, userdata):
         self.disinfection_cmd_pub.publish(Empty())
+        self.say('Please hold out your hand to my friend Kinova and get some hand sanitiser')
         rospy.sleep(self.disinfection_duration)
+        return 'succeeded'

From f2253618c3aab66f4dff31267b2f04f6eb318c75 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 17:18:20 +0200
Subject: [PATCH 090/264] [demo_lab_manager/config] Increased duration of
 disinfection procedure

---
 .../mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 3293f37ec..b5eedc6bf 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -148,7 +148,7 @@ state_descriptions:
                 value: /control_robot/event_in
             - argument:
                 name: disinfection_duration
-                value: 20.0
+                value: 30.0
     - state:
         name: GUIDE_TO_EMPTY_SPOT
         state_module_name: mdr_navigation_behaviours.move_base

From bd20768994eb6709b0b849fd7c0ab304ca099a49 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 17:23:41 +0200
Subject: [PATCH 091/264] [demo_lab_manager/config] Added more instructions
 states

---
 .../config/lab_manager_sm.yaml                | 46 +++++++++++++++++--
 1 file changed, 41 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index b5eedc6bf..d7aec472e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -2,7 +2,7 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_lab_manager
-states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, TURN_PERSON_AWAY, GO_TO_DISINFECTION_SPOT, PERFORM_DISINFECTION, GUIDE_TO_EMPTY_SPOT]
+states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, GIVE_DISINFECTION_FOLLOWING_INSTRUCTIONS, TURN_PERSON_AWAY, GO_TO_DISINFECTION_SPOT, PERFORM_DISINFECTION, GIVE_ASSIGNED_SPOT_FOLLOWING_INSTRUCTIONS, GUIDE_TO_EMPTY_SPOT, GIVE_LAB_LEAVE_INSTRUCTIONS]
 outcomes: [DONE, FAILED]
 state_descriptions:
     - state:
@@ -60,7 +60,7 @@ state_descriptions:
                 state: MONITOR_DOOR
             - transition:
                 name: known_person
-                state: GO_TO_DISINFECTION_SPOT
+                state: GIVE_DISINFECTION_FOLLOWING_INSTRUCTIONS
             - transition:
                 name: face_not_seen
                 state: MONITOR_DOOR
@@ -99,7 +99,7 @@ state_descriptions:
                 state: ASK_NAME
             - transition:
                 name: failed_after_retrying
-                state: GO_TO_DISINFECTION_SPOT
+                state: GIVE_DISINFECTION_FOLLOWING_INSTRUCTIONS
         arguments:
             - argument:
                 name: number_of_retries
@@ -113,6 +113,18 @@ state_descriptions:
             - argument:
                 name: loop_rate_s
                 value: 2.0
+    - state:
+        name: GIVE_DISINFECTION_FOLLOWING_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_DISINFECTION_SPOT
+        arguments:
+            - argument:
+                name: sentences
+                value: ['Please follow me to the hand sanitiser station.']
     - state:
         name: GO_TO_DISINFECTION_SPOT
         state_module_name: mdr_navigation_behaviours.move_base
@@ -141,7 +153,7 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: GUIDE_TO_EMPTY_SPOT
+                state: GIVE_ASSIGNED_SPOT_FOLLOWING_INSTRUCTIONS
         arguments:
             - argument:
                 name: disinfection_cmd_topic
@@ -149,6 +161,18 @@ state_descriptions:
             - argument:
                 name: disinfection_duration
                 value: 30.0
+    - state:
+        name: GIVE_ASSIGNED_SPOT_FOLLOWING_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: GUIDE_TO_EMPTY_SPOT
+        arguments:
+            - argument:
+                name: sentences
+                value: ['Please follow me to your assigned spot.']
     - state:
         name: GUIDE_TO_EMPTY_SPOT
         state_module_name: mdr_navigation_behaviours.move_base
@@ -156,7 +180,7 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: GO_TO_MONITORING_POSE
+                state: GIVE_LAB_LEAVE_INSTRUCTIONS
             - transition:
                 name: failed
                 state: GO_TO_MONITORING_POSE
@@ -167,3 +191,15 @@ state_descriptions:
             - argument:
                 name: number_of_retries
                 value: 3
+    - state:
+        name: GIVE_LAB_LEAVE_INSTRUCTIONS
+        state_module_name: mdr_hri_behaviours.say_sentence
+        state_class_name: SaySentence
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_MONITORING_POSE
+        arguments:
+            - argument:
+                name: sentences
+                value: ['If you are leaving the lab, please come to me so that I know that your spot is free.']

From 9ca36bbab611627b064bbef8512f322689a46110 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 17:44:26 +0200
Subject: [PATCH 092/264] [hri_behaviours/say_sentence] Added an optional sleep
 argument for long sentences

---
 .../ros/src/mdr_hri_behaviours/say_sentence.py                 | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
index ffb36cad4..2aec9b9b8 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
@@ -12,6 +12,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'say_sentence')
         self.sentences = list(kwargs.get('sentences', list()))
+        self.sleep_time = kwargs.get('sleep_time', -1.)
 
     def execute(self, userdata):
         sentence = random.choice(self.sentences)
@@ -19,4 +20,6 @@ def execute(self, userdata):
         rospy.loginfo('Saying: %s' % sentence)
         self.say(sentence)
 
+        if self.sleep_time > 0:
+            rospy.sleep(self.sleep_time)
         return 'succeeded'

From 520425cf49f18933280036fa2455ef3fdf7b1e1f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 17:45:30 +0200
Subject: [PATCH 093/264] [demo_lab_manager] Adeed sleep in the last
 instruction state; modified disinfection instruction sentence for clarity

---
 .../mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml  | 3 +++
 .../scenario_states/perform_disinfection.py                    | 2 +-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index d7aec472e..4b6fad0bf 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -203,3 +203,6 @@ state_descriptions:
             - argument:
                 name: sentences
                 value: ['If you are leaving the lab, please come to me so that I know that your spot is free.']
+            - argument:
+                name: sleep_time
+                value: 5.0
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
index 61e0ae0be..76eed09e1 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
@@ -22,6 +22,6 @@ def __init__(self, save_sm_state=False, **kwargs):
 
     def execute(self, userdata):
         self.disinfection_cmd_pub.publish(Empty())
-        self.say('Please hold out your hand to my friend Kinova and get some hand sanitiser')
+        self.say('Please hold out your hand to my friend Kinova. You will then get some hand sanitizer.')
         rospy.sleep(self.disinfection_duration)
         return 'succeeded'

From 5e5f08d620276a37706671ecfc79a884398830f5 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 22 Aug 2020 20:53:09 +0200
Subject: [PATCH 094/264] [demo_lab_manager/launch] Included the ROSPlan
 launcher

---
 .../mdr_demo_lab_manager/ros/launch/lab_manager.launch          | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
index 4f70fc968..3399a30ec 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+    <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch" />
 
     <!-- Actions -->
     <include file="$(find mdr_move_base_action)/ros/launch/move_base_client.launch" />
-
     <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
 
     <!-- State machine -->

From 7ddc58ac453ae8bc02e2e5c4a4aa0b3cf62ec0bf Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 27 Aug 2020 13:50:46 +0200
Subject: [PATCH 095/264] [demo_lab_manager/verify_person] Update the KB when a
 person is assigned a spot

---
 .../mdr_demo_lab_manager/scenario_states/verify_person.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 8f1d0a78e..5ffd8d385 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -90,13 +90,17 @@ def execute(self, userdata):
                 # guide to empty spot
                 spot = self.choose_sitting_spot(occ_spots)
                 userdata.destination_locations = ['spot_{0}'.format(spot)]
+                occupied_locations.typed_parameters.append(KeyValue(key=str(spot),
+                                                                    value=recognised_person.name))
+                self.kb_interface.update_obj_instance('occupied_locations',
+                                                      occupied_locations)
                 return 'known_person'
 
         # No matching face, treat as new person
         spot = self.choose_sitting_spot(occ_spots)
         userdata.destination_locations = ['spot_{0}'.format(spot)]
-        # occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='true'))
-        # self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+        occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='unknown'))
+        self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
         return 'new_person'
 
     def choose_sitting_spot(self, occ_spots):

From 1eef2b9e59e673a4c9255556a26c2c0525c01bca Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 27 Aug 2020 14:09:57 +0200
Subject: [PATCH 096/264] [demo_lab_manager/verify_person] Perform check for
 assigned spot only if a person is recognised

---
 .../scenario_states/verify_person.py                  | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 5ffd8d385..8030582a7 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -58,13 +58,14 @@ def execute(self, userdata):
         recognised_person = self.kb_interface.recognise_person('person_0',
                                                                Person._type,
                                                                self.person_recognition_threshold)
-        # Check if the person is logged
-        person_logged = False
-        for occ_spot in occupied_locations.typed_parameters:
-            if occ_spot.value == recognised_person.name:
-                person_logged = True
 
         if recognised_person is not None:
+            # Check if the person is logged
+            person_logged = False
+            for occ_spot in occupied_locations.typed_parameters:
+                if occ_spot.value == recognised_person.name:
+                    person_logged = True
+
             self.say("Hello {0}".format(recognised_person.name))
             if person_logged:
                 self.say("If you would like to free up your spot, please say goodbye.")

From dafb02e0a3b1bd21b9595f1511b21e8ab7525ab5 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 27 Aug 2020 14:22:04 +0200
Subject: [PATCH 097/264] [perception_behaviours/find_people] Only say people
 aren't found in debug mode

---
 .../ros/src/mdr_perception_behaviours/find_people.py          | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
index 056e3ad2a..3c5933ac0 100644
--- a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
+++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
@@ -12,7 +12,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'find_people')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
-        self.debug = kwargs.get('debug', 0)
+        self.debug = kwargs.get('debug', False)
         self.retry_count = 0
         self.timeout = 120.
 
@@ -35,7 +35,7 @@ def execute(self, userdata):
             return 'succeeded'
 
         rospy.loginfo('Could not find people')
-        self.say('Could not find people')
+        if self.debug: self.say('Could not find people')
         if self.retry_count == self.number_of_retries:
             rospy.loginfo('[find_people] Failed to find people')
             if self.debug: self.say('Aborting operation')

From 71708c0c440e36650ce39c720c2ae568957f5546 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 27 Aug 2020 14:24:24 +0200
Subject: [PATCH 098/264] [nav_behaviours/move_base] Added a debug mode for
 saying debugging messages

---
 .../ros/src/mdr_navigation_behaviours/move_base.py         | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
index 3a193fb0d..bef7b955c 100644
--- a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
+++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py
@@ -19,6 +19,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.move_base_server = kwargs.get('move_base_server', 'move_base_server')
         self.destination_locations = list(kwargs.get('destination_locations', list()))
         self.timeout = kwargs.get('timeout', 120.)
+        self.debug = kwargs.get('debug', False)
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.retry_count = 0
@@ -38,7 +39,7 @@ def execute(self, userdata):
                                                  destination_location)
 
             rospy.loginfo('Sending the base to %s' % destination_location)
-            self.say('Going to ' + destination_location)
+            if self.debug: self.say('Going to ' + destination_location)
             self.action_dispatch_pub.publish(dispatch_msg)
 
             self.executing = True
@@ -54,9 +55,9 @@ def execute(self, userdata):
                 original_location = destination_location
             else:
                 rospy.logerr('Could not reach %s' % destination_location)
-                self.say('Could not reach ' + destination_location)
+                if self.debug: self.say('Could not reach ' + destination_location)
                 if self.retry_count == self.number_of_retries:
-                    self.say('Aborting operation')
+                    if self.debug: self.say('Aborting operation')
                     return 'failed_after_retrying'
                 self.retry_count += 1
                 return 'failed'

From a478eed8b59754c486ecb166b8daa3914775841a Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 28 Aug 2020 08:29:39 +0200
Subject: [PATCH 099/264] [demo_lab_manager/get_user_data] Fixed imports and KB
 updates

---
 .../scenario_states/get_user_data.py          | 45 +++++++++++--------
 1 file changed, 27 insertions(+), 18 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index 45a55d26a..ce5ab75d0 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -1,9 +1,9 @@
-import requests
-import pandas as pd
-import numpy as np
 import time
+import pandas as pd
 
 import rospy
+from diagnostic_msgs.msg import KeyValue
+from rosplan_knowledge_msgs.msg import DomainFormula
 from mas_perception_msgs.msg import Person
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
@@ -31,7 +31,7 @@ def __init__(self, save_sm_state=False, **kwargs):
             self._num_known_entries = data.shape[0]
             rospy.loginfo("[get_user_data] Initialized the user data query with {0} prior known users".format(self._num_known_entries))
         else:
-            rospy.logerr("[get_user_data] Could not initialize!")
+            rospy.logerr("[get_user_data] Could not initialize user data sheet!")
 
     def execute(self, userdata):
         if self._num_known_entries is None:
@@ -39,7 +39,7 @@ def execute(self, userdata):
             return "succeeded"
 
         self.say("Hi, seems like you are visiting the lab for the first time. "
-                "Please enter your details by scanning the QR code displayed on the wall to your right.")
+                 "Please enter your details by scanning the QR code displayed on the wall to your right.")
 
         rospy.loginfo("[get_user_data] waiting for user data")
         start_time = time.time()
@@ -49,15 +49,23 @@ def execute(self, userdata):
             if num_of_new_entries > 0:
                 self._num_known_entries = data.shape[0]
                 new_entry = data[-1, 1:3]
-                rospy.loginfo("[get_user_data] Found a new entry!\n\tName: {0}\n\tEmail: {1}".format(new_entry[0], new_entry[1]))
-                # TODO Write the user data to knowledge base
-                person_0 = self.kb_interface.get_obj_instance('person_0', Person._type)
-                self.kb_interface.insert_obj_instance(new_entry[0], person_0)
+                name, email = new_entry[0], new_entry[1]
+                rospy.loginfo("[get_user_data] Found a new entry!\n\tName: {0}\n\tEmail: {1}".format(name, email))
+
+                # we add the new person to permanent storage (one-shot learning of people)
+                # and remove it from temporary storage
+                person_msg = self.kb_interface.get_obj_instance('person_0', Person._type)
+                person_msg.name = name
+                self.kb_interface.insert_obj_instance(name, person_msg, permanent_storage=True)
                 self.kb_interface.remove_obj_instance('person_0', Person._type)
-                # update the occupied spots
+
+                # the occupied spots are updated as well
                 spot = userdata.destination_locations
-                occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value=new_entry[0]))
+                occupied_locations = self.kb_interface.get_obj_instance('occupied_locations',
+                                                                        DomainFormula._type)
+                occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value=name))
                 self.kb_interface.update_obj_instance('occupied_locations', occupied_locations)
+
                 return "succeeded"
             else:
                 time.sleep(self._loop_rate_s)
@@ -73,10 +81,11 @@ def execute(self, userdata):
         return "failed"
 
     def _load_spreadsheet(self):
-        # Construct the URL
-        URL = 'https://docs.google.com/spreadsheets/d/{0}/gviz/tq?tqx=out:csv&sheet={1}'.format(
-                self._sheet_id, self._worksheet_name)
-
-        # Fetch and process the csv data
-        data = pd.read_csv(URL, sep=",").to_numpy()
-        return data
+        URL = 'https://docs.google.com/spreadsheets/d/{0}/gviz/tq?tqx=out:csv&sheet={1}'.format(self._sheet_id,
+                                                                                                self._worksheet_name)
+        try:
+            data = pd.read_csv(URL, sep=",").to_numpy()
+            return data
+        except Exception as exc:
+            rospy.logerr(str(exc))
+            return None

From 5a17ead5ac9bd58d00dfc380c3dbac0371bf6c23 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 28 Aug 2020 09:21:47 +0200
Subject: [PATCH 100/264] Resolved various Codacy issues

---
 .../ros/src/mdr_find_people/action_states.py    | 15 ++-------------
 .../ros/src/mdr_find_people/find_people.py      | 17 ++++++++++++-----
 .../ros/src/mdr_hri_behaviours/say_sentence.py  |  1 -
 .../scenario_states/perform_disinfection.py     |  3 ++-
 .../scenario_states/verify_person.py            |  1 -
 5 files changed, 16 insertions(+), 21 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 735acffd4..0cc9aec02 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -3,9 +3,7 @@
 import torch
 
 from PIL import Image as PILImage
-import cv2
 import tf
-import face_recognition
 from sensor_msgs.msg import PointCloud2, Image
 from mdr_find_people.msg import FindPeopleResult
 from mas_perception_msgs.msg import Person, PersonList, ObjectView
@@ -56,8 +54,8 @@ def execute(self, userdata):
             cropped_cv = crop_image(cv_image, bb2d)
             cropped_img_msg = bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
 
-            rospy.loginfo('[find_people] Attempting to extract face of person {}'.format(i+1))
-            cropped_face_img = self._extract_face_image(cropped_cv)
+            rospy.loginfo('[find_people] Attempting to extract face of person {0}'.format(i+1))
+            cropped_face_img = FindPeople.extract_face_image(cropped_cv)
             if cropped_face_img is not None:
                 cropped_face_img_msg = bridge.cv2_to_imgmsg(cropped_face_img,
                                                             encoding='passthrough')
@@ -103,12 +101,3 @@ def execute(self, userdata):
 
         userdata['find_people_result'] = result
         return 'succeeded'
-
-    def _extract_face_image(self, image_array):
-        try:
-            top, right, bottom, left = face_recognition.face_locations(image_array)[0]
-            rospy.loginfo('[find_people] Successfully extracted face from person image.')
-            return image_array[top:bottom, left:right]
-        except IndexError:
-            rospy.logwarn('[find_people] Failed to extract face from person image!')
-            return None
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index d84e96154..f8f22b751 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -3,7 +3,9 @@
 import os
 import numpy as np
 
+import rospy
 from rospkg import RosPack
+import face_recognition
 
 from std_msgs.msg import Header
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
@@ -15,11 +17,6 @@
 
 
 class FindPeople(object):
-
-    def __init__(self):
-        pass
-
-
     @staticmethod
     def detect(cloud_msg):
         # Transform point cloud to base link
@@ -85,3 +82,13 @@ def render_image_with_detections(cloud_msg, bounding_boxes):
         cv_image = cloud_msg_to_cv_image(cloud_msg)
         image = draw_labeled_boxes(cv_image, bounding_boxes)
         return image
+
+    @staticmethod
+    def extract_face_image(image_array):
+        try:
+            top, right, bottom, left = face_recognition.face_locations(image_array)[0]
+            rospy.loginfo('[find_people] Successfully extracted face from person image.')
+            return image_array[top:bottom, left:right]
+        except IndexError:
+            rospy.logwarn('[find_people] Failed to extract face from person image!')
+            return None
diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
index 2aec9b9b8..ce351b663 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py
@@ -1,4 +1,3 @@
-import time
 import rospy
 import random
 
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
index 76eed09e1..d6e3d1bd4 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/perform_disinfection.py
@@ -17,8 +17,9 @@ def __init__(self, save_sm_state=False, **kwargs):
         try:
             self.disinfection_cmd_pub = rospy.Publisher(self.disinfection_cmd_topic,
                                                         Empty, queue_size=1)
-        except:
+        except Exception as exc:
             rospy.logerr('[send_disinfection_command] Could not create a disinfection command publisher')
+            rospy.logerr(str(exc))
 
     def execute(self, userdata):
         self.disinfection_cmd_pub.publish(Empty())
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 8030582a7..9ee938e62 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -1,5 +1,4 @@
 import random
-import numpy as np
 
 import rospy
 import actionlib

From e1a1a3f6b653d0c622d992ce7b3b78f77a00c393 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 29 Aug 2020 23:14:31 +0200
Subject: [PATCH 101/264] [demo_lab_manager/launch] Included the listen action
 launcher

---
 .../mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
index 3399a30ec..5bc5bec56 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/launch/lab_manager.launch
@@ -4,6 +4,7 @@
     <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch" />
 
     <!-- Actions -->
+    <include file="$(find mdr_listen_action)/ros/launch/listen.launch" />
     <include file="$(find mdr_move_base_action)/ros/launch/move_base_client.launch" />
     <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
 

From dac409108ca3710e842e4a0896c1434a14bdb1c1 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Thu, 3 Sep 2020 15:38:53 +0200
Subject: [PATCH 102/264] Add additional notes for building the packages on the
 robot

---
 README.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/README.md b/README.md
index 1111c1732..3b99c8467 100644
--- a/README.md
+++ b/README.md
@@ -72,11 +72,14 @@ For external users, the following instructions should get you a working system:
     wstool update -t src
     rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
   ```
+  **Note:** If setting up on the robot, skip the `rosdep install` command.
+
 3. Build the code
 
   ```
   catkin build
   ```
+  Make sure that you have sourced `/opt/ros/kinetic/setup.bash` before building the packages.
 
 If you encounter any problems, please check the list of [issues](https://github.com/b-it-bots/mas_domestic_robotics/issues) and open a new one if you don't see a discussion of the problem there.
 

From fb0626c8cd11f7bbfe72788fa3bc83727e1da24d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Sep 2020 10:19:24 +0200
Subject: [PATCH 103/264] [demo_lab_manager/verify_person] Removing the
 person_0 msg from the KB before repeating the monitoring operation

person_0 is the KB ID of the person that we verify; if the message is
not removed and there are multiple messages with the same ID, the oldest
message will always be returned during retrieval, which is undesired
---
 .../mdr_demo_lab_manager/scenario_states/verify_person.py  | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
index 9ee938e62..b745b3a0e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py
@@ -51,6 +51,9 @@ def execute(self, userdata):
         person = self.kb_interface.get_obj_instance('person_0', Person._type)
         if not person.face.views or not person.face.views[0].image.data:
             self.say('I could not see your face. Could you please look at me briefly?')
+
+            # we remove the person message from the knowledge base before transitioning
+            self.kb_interface.remove_obj_instance('person_0', Person._type)
             return 'face_not_seen'
 
         # Try to match face to current people in the lab
@@ -59,6 +62,9 @@ def execute(self, userdata):
                                                                self.person_recognition_threshold)
 
         if recognised_person is not None:
+            # we remove the anonymous person message from the knowledge base
+            self.kb_interface.remove_obj_instance('person_0', Person._type)
+
             # Check if the person is logged
             person_logged = False
             for occ_spot in occupied_locations.typed_parameters:
@@ -97,6 +103,7 @@ def execute(self, userdata):
                 return 'known_person'
 
         # No matching face, treat as new person
+        self.kb_interface.remove_obj_instance('person_0', Person._type)
         spot = self.choose_sitting_spot(occ_spots)
         userdata.destination_locations = ['spot_{0}'.format(spot)]
         occupied_locations.typed_parameters.append(KeyValue(key=str(spot), value='unknown'))

From 353d3aecb1f6e6d2f0eeb0d40e29a1608aa91ba6 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 4 Sep 2020 14:25:11 +0200
Subject: [PATCH 104/264] Add a new rosinstall file with devel versions for
 repositories

---
 mas-domestic-devel.rosinstall | 88 +++++++++++++++++++++++++++++++++++
 1 file changed, 88 insertions(+)
 create mode 100644 mas-domestic-devel.rosinstall

diff --git a/mas-domestic-devel.rosinstall b/mas-domestic-devel.rosinstall
new file mode 100644
index 000000000..0a4be0b99
--- /dev/null
+++ b/mas-domestic-devel.rosinstall
@@ -0,0 +1,88 @@
+- git:
+    local-name: mas_common_robotics
+    uri: https://github.com/b-it-bots/mas_common_robotics.git
+    version: kinetic
+
+- git:
+    local-name: mas_domestic_robotics
+    uri: https://github.com/b-it-bots/mas_domestic_robotics.git
+    version: devel
+
+- git:
+    local-name: mas_navigation_tools
+    uri: https://github.com/b-it-bots/mas_navigation_tools.git
+    version: devel
+
+- git:
+    local-name: mas_perception_libs
+    uri: https://github.com/b-it-bots/mas_perception_libs.git
+    version: devel
+
+- git:
+    local-name: mas_perception_msgs
+    uri: https://github.com/b-it-bots/mas_perception_msgs.git
+    version: devel
+
+- git:
+    local-name: mas_execution_manager
+    uri: https://github.com/b-it-bots/mas_execution_manager.git
+    version: devel
+
+- git:
+    local-name: mas_knowledge_base
+    uri: https://github.com/b-it-bots/mas_knowledge_base.git
+    version: devel
+
+- git:
+    local-name: action-execution
+    uri: https://github.com/b-it-bots/action-execution.git
+    version: devel
+
+- git:
+    local-name: ftsm
+    uri: https://github.com/b-it-bots/ftsm.git
+    version: master
+- git:
+    local-name: topological_map
+    uri: https://github.com/b-it-bots/topological_map.git
+    version: master
+
+- git:
+    local-name: zbar_ros
+    uri: https://github.com/b-it-bots/zbar_ros.git
+    version: kinetic
+
+- git:
+    local-name: dmp
+    uri: https://github.com/b-it-bots/dmp.git
+    version: master
+
+- git:
+    local-name: ROSPlan
+    uri: https://github.com/KCL-Planning/ROSPlan.git
+    version: devel
+
+- git:
+    local-name: mongodb_store
+    uri: https://github.com/b-it-bots/mongodb_store.git
+    version: kinetic-devel
+
+- git:
+    local-name: occupancy_grid_utils
+    uri: https://github.com/b-it-bots/occupancy_grid_utils.git
+    version: indigo-devel
+
+- git:
+    local-name: orocos_kinematics_dynamics
+    uri: https://github.com/b-it-bots/orocos_kinematics_dynamics.git
+    version: master
+
+- git:
+    local-name: kdl_parser
+    uri: https://github.com/ros/kdl_parser.git
+    version: kinetic-devel
+
+- git:
+    local-name: ros_dmp
+    uri: https://github.com/b-it-bots/ros_dmp.git
+    version: master

From 08125182a0d3be59e84c7589d79f10fb71a060a4 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 4 Sep 2020 14:42:57 +0200
Subject: [PATCH 105/264] Use the master branch of b-it-bots fork for ROSPlan

---
 mas-domestic-devel.rosinstall | 4 ++--
 mas-domestic.rosinstall       | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/mas-domestic-devel.rosinstall b/mas-domestic-devel.rosinstall
index 0a4be0b99..e906adf02 100644
--- a/mas-domestic-devel.rosinstall
+++ b/mas-domestic-devel.rosinstall
@@ -59,8 +59,8 @@
 
 - git:
     local-name: ROSPlan
-    uri: https://github.com/KCL-Planning/ROSPlan.git
-    version: devel
+    uri: https://github.com/b-it-bots/ROSPlan.git
+    version: master
 
 - git:
     local-name: mongodb_store
diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall
index bb6a3f0d1..0a416c874 100644
--- a/mas-domestic.rosinstall
+++ b/mas-domestic.rosinstall
@@ -59,7 +59,7 @@
 
 - git:
     local-name: ROSPlan
-    uri: https://github.com/KCL-Planning/ROSPlan.git
+    uri: https://github.com/b-it-bots/ROSPlan.git
     version: master
 
 - git:

From 7de8d10615290158a3de7e4c85a90839762009a8 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 4 Sep 2020 16:43:15 +0200
Subject: [PATCH 106/264] Update README instructions

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 3b99c8467..73a6b446f 100644
--- a/README.md
+++ b/README.md
@@ -66,6 +66,8 @@ For external users, the following instructions should get you a working system:
   wstool init src
   wstool merge -t src https://raw.githubusercontent.com/b-it-bots/mas_domestic_robotics/devel/mas-domestic.rosinstall
   ```
+  **Note:** If you want to set up a development environment, replace `mas-domestic.rosinstall` with `mas-domestic-devel.rosinstall` in the above command.
+
 2. Get the code and dependencies
 
   ```

From 49455d51082552acbd6da1ac5327eb1201ada78e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Sep 2020 17:49:11 +0200
Subject: [PATCH 107/264] [gripper_controller] Added fns for impact detection
 along z (useful for placing)

I want to do impact detection as change detection over force measurements,
so I've added two functions:
* one for initialising the measurements
* one for checking if the difference between the initial measurements
  and the current measurements is above a threshold
---
 .../gripper_controller_base.py                   | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
index 69050a9b6..ebe3e3375 100644
--- a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
+++ b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
@@ -2,17 +2,25 @@
 
 class GripperControllerBase(object):
     def open(self):
-        rospy.loginfo('[OPEN_GRIPPER] Ignoring request')
+        rospy.loginfo('[open] Ignoring request')
         raise NotImplementedError()
 
     def close(self):
-        rospy.loginfo('[CLOSE_GRIPPER] Ignoring request')
+        rospy.loginfo('[close] Ignoring request')
+        raise NotImplementedError()
+
+    def init_impact_detection_z(self):
+        rospy.loginfo('[init_impact_detection_z] Ignoring request')
+        raise NotImplementedError()
+
+    def detect_impact_z(self):
+        rospy.loginfo('[detect_impact_z] Ignoring request')
         raise NotImplementedError()
 
     def init_grasp_verification(self):
-        rospy.loginfo('[INIT_GRASP_VERIFICATION] Ignoring request')
+        rospy.loginfo('[init_grasp_verification] Ignoring request')
         raise NotImplementedError()
 
     def verify_grasp(self):
-        rospy.loginfo('[VERIFY_GRASP] Ignoring request')
+        rospy.loginfo('[verify_grasp] Ignoring request')
         raise NotImplementedError()

From 81fcdb28bfe0df62af172a396e91f5b8b51138d1 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Sep 2020 19:04:16 +0200
Subject: [PATCH 108/264] [place_action] Added downward arm movement for
 placing by detecting surface impact

---
 .../mdr_place_action/README.md                |  1 +
 .../ros/launch/place_action.launch            |  1 +
 .../mdr_place_action/ros/scripts/place_action |  6 +++++-
 .../ros/src/mdr_place_action/action_states.py | 19 +++++++++++++++----
 4 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md
index 1495a078d..a356c5a53 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md
@@ -61,6 +61,7 @@ The following parameters may be passed when launching the action server:
 * ``base_elbow_offset``: An optional offset between `base_link` and the manipulator's elbow; used for aligning the base with the placing pose so that the manipulator can easily reach it (default: -1)
 * ``placing_dmp``:  Path to a YAML file containing the weights of a dynamic motion primitive used for placing (default: '')
 * ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 1)
+* ``downward_placing_vel``: Velocity with which the arm moves downwards to detect impact with the placing surface (default: -0.02)
 * ``placing_orientation``: For more constrained manipulators, it might make sense to use a fixed placing orientation (expressed as an (x, y, z, w) quaternion) to ensure easier reachability; for instance, we might want to keep the orientation with which an object was grasped instead of allowing arbitrary orientations (default: [], in which case the argument is ignored)
 
 ### Action client
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
index c9e0a1651..806ecb4fd 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
@@ -9,6 +9,7 @@
         <param name="base_elbow_offset" value="-1." />
         <param name="placing_dmp" value="$(find mdr_place_action)/config/trajectory_weights/weights_table_place.yaml" />
         <param name="dmp_tau" value="30" />
+        <param name="downward_placing_vel" value="-0.02" />
         <rosparam param="placing_orientation">[0, 0, 0, 1]</rosparam>
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action
index 38d267601..661a4c515 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action
@@ -22,6 +22,8 @@ class PlaceServer(object):
     * placing_dmp: Path to a YAML file containing the weights of a dynamic
                    motion primitive used for placing (default: '')
     * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1)
+    * downward_placing_vel: Velocity with which the arm moves downwards to detect impact
+                            with the placing surface (default -0.02rad/s)
     * placing_orientation: For more constrained manipulators, it might make sense to use
                            a fixed placing orientation (expressed as an (x, y, z, w) quaternion)
                            to ensure easier reachability; for instance, we might want to keep
@@ -43,6 +45,7 @@ class PlaceServer(object):
         placing_orientation = rospy.get_param('~placing_orientation', list())
         placing_dmp = rospy.get_param('~placing_dmp', '')
         dmp_tau = float(rospy.get_param('~dmp_tau', 1.))
+        downward_placing_vel = float(rospy.get_param('~downward_placing_vel', -0.02))
 
         rospy.loginfo('[place] Initialising state machine')
         self.action_sm = PlaceSM(move_arm_server=move_arm_server,
@@ -53,7 +56,8 @@ class PlaceServer(object):
                                  base_elbow_offset=base_elbow_offset,
                                  placing_orientation=placing_orientation,
                                  placing_dmp=placing_dmp,
-                                 dmp_tau=dmp_tau)
+                                 dmp_tau=dmp_tau,
+                                 downward_placing_vel=downward_placing_vel)
         rospy.loginfo('[place] State machine initialised')
 
         self.action_server = actionlib.SimpleActionServer('place_server',
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
index 993d3ec84..0227ad30a 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
@@ -22,9 +22,10 @@ def __init__(self, timeout=120.0,
                  move_arm_server='move_arm_server',
                  move_base_server='move_base_server',
                  base_elbow_offset=-1.,
-                 placing_orientation=list(),
+                 placing_orientation=None,
                  placing_dmp='',
                  dmp_tau=1.,
+                 downward_placing_vel=-0.02,
                  max_recovery_attempts=1):
         super(PlaceSM, self).__init__('Place', [], max_recovery_attempts)
         self.timeout = timeout
@@ -32,7 +33,7 @@ def __init__(self, timeout=120.0,
         gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name)
         GripperControllerClass = getattr(import_module(gripper_controller_module_name),
                                          'GripperController')
-        self.gripper = GripperControllerClass()
+        self.gripper = GripperControllerClass(joint_vel=downward_placing_vel)
 
         self.preplace_config_name = preplace_config_name
         self.preplace_low_config_name = preplace_low_config_name
@@ -71,7 +72,7 @@ def running(self):
         pose = self.goal.pose
         pose.header.stamp = rospy.Time(0)
         pose_base_link = self.tf_listener.transformPose('base_link', pose)
-        if self.placing_orientation:
+        if self.placing_orientation is not None:
             pose_base_link.pose.orientation.x = self.placing_orientation[0]
             pose_base_link.pose.orientation.y = self.placing_orientation[1]
             pose_base_link.pose.orientation.z = self.placing_orientation[2]
@@ -97,8 +98,18 @@ def running(self):
             rospy.logerr('[place] Arm motion unsuccessful')
             self.result = self.set_result(False)
             return FTSMTransitions.DONE
-
         rospy.loginfo('[place] Arm motion successful')
+
+        # the arm is moved down until it makes an impact with the placing surface
+        rospy.loginfo('[place] Moving arm down until surface impact is detected...')
+        self.gripper.init_impact_detection_z()
+        while not self.gripper.detect_impact_z():
+            self.gripper.move_down()
+            rospy.sleep(0.1)
+        self.gripper.stop_arm()
+        rospy.loginfo('[place] Impact detected')
+
+        # the object can be released now that impact with the surface has been made
         rospy.loginfo('[place] Opening the gripper...')
         self.gripper.open()
 

From 12ebc19357ed69ca38b102dd1d73b35479168c5b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 12 Sep 2020 13:49:08 +0200
Subject: [PATCH 109/264] [move_arm_action/dmp] Removed unused fn
 (generate_trajectory)

---
 .../ros/src/mdr_move_arm_action/dmp.py        | 33 ++-----------------
 1 file changed, 3 insertions(+), 30 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index f6c894291..560421578 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -74,33 +74,6 @@ def move_to(self, goal):
 
         self.follow_path(initial_pos, goal)
 
-    def generate_trajectory(self, initial_pos, goal):
-        '''Returns a 2D numpy array representing a path of [x, y, z] points
-        from "initial_pos" to "goal" that follows the trajectory encoded
-        by self.dmp_name. Each row in the resulting array is a single point
-        on the path.
-
-        Keyword arguments:
-        initial_pos: numpy.ndarray -- initial position for the motion
-        goal: numpy.ndarray -- goal position
-
-        '''
-        initial_pose = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0., 0., 0.])
-        goal_pose = np.array([goal[0], goal[1], goal[2], 0., 0., 0.])
-
-        print('[move_arm/dmp] Querying trajectory')
-        print('[move_arm/dmp] Initial pose: ', initial_pose)
-        print('[move_arm/dmp] Goal pose: ', goal_pose)
-        cartesian_trajectory, _ = self.roll_dmp.get_trajectory_and_path(goal_pose,
-                                                                        initial_pose,
-                                                                        self.tau)
-
-        path = np.array([[state.pose.position.x,
-                          state.pose.position.y,
-                          state.pose.position.z]
-                         for state in cartesian_trajectory.cartesian_state])
-        return path
-
     def follow_path(self, initial_pos,  goal):
         '''Moves a manipulator so that it follows the given path. If whole body
         motion is enabled and some points on the path lie outside the reachable
@@ -301,7 +274,7 @@ def sigma_values_cb(self, msg):
         self.min_sigma_value = min(msg.data)
 
     def instantiate_dmp(self, initial_pose, goal_pose):
-        '''Instantiates a DMP object from learned weights, given 
+        '''Instantiates a DMP object from learned weights, given
         the initial and final poses.
 
         Keyword arguments:
@@ -321,10 +294,10 @@ def instantiate_dmp(self, initial_pose, goal_pose):
         dmp_weights[4, :] = dmp_weights_dict['pitch']
         dmp_weights[5, :] = dmp_weights_dict['yaw']
 
-        return pydmps.dmp_discrete.DMPs_discrete(n_dmps=n_dmps, 
+        return pydmps.dmp_discrete.DMPs_discrete(n_dmps=n_dmps,
                                                  n_bfs=n_bfs,
                                                  dt=self.time_step,
                                                  y0=initial_pose,
                                                  goal=goal_pose,
-                                                 ay=None, 
+                                                 ay=None,
                                                  w=dmp_weights)

From b1952de127bba1b33f973feea1e2b35ed62f5331 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 12 Sep 2020 14:19:15 +0200
Subject: [PATCH 110/264] [move_arm_action/dmp] Fixed a transform bug in the
 DMP executor

The problem here was that the goal was given in the base link frame and
the initial position of the end effector was also taken wrt base link,
but during the execution, the poses were transformed to the odometry
frame. Both the initial pose and the goal pose are now transformed to
the odometry frame before execution starts.
---
 .../src/mdr_move_arm_action/action_states.py  |  8 +--
 .../ros/src/mdr_move_arm_action/dmp.py        | 60 ++++++++-----------
 2 files changed, 29 insertions(+), 39 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
index 2db534bc3..5f8151687 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
@@ -33,7 +33,7 @@ def running(self):
             rospy.loginfo('[move_arm] Planning motion and trying to move arm...')
             success = self.arm.go(wait=True)
         elif self.goal.goal_type == MoveArmGoal.END_EFFECTOR_POSE:
-            pose = self.goal.end_effector_pose
+            goal = self.goal.end_effector_pose
             dmp_name = self.goal.dmp_name
             tau = self.goal.dmp_tau
             rospy.loginfo('[move_arm] Planning motion and trying to move arm...')
@@ -42,8 +42,6 @@ def running(self):
             # otherwise, we just use moveit for planning a trajectory and moving the arm
             if dmp_name:
                 dmp_traj_executor = DMPExecutor(dmp_name, tau)
-                goal = np.array([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
-
                 dmp_execution_thread = Thread(target=dmp_traj_executor.move_to, args=(goal,))
                 dmp_execution_thread.start()
                 while not dmp_traj_executor.motion_completed and \
@@ -58,8 +56,8 @@ def running(self):
                     self.result = self.set_result(False)
                     return FTSMTransitions.DONE
             else:
-                self.arm.set_pose_reference_frame(pose.header.frame_id)
-                self.arm.set_pose_target(pose.pose)
+                self.arm.set_pose_reference_frame(goal.header.frame_id)
+                self.arm.set_pose_target(goal.pose)
                 success = self.arm.go(wait=True)
         elif self.goal.goal_type == MoveArmGoal.JOINT_VALUES:
             joint_values = self.goal.joint_values
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 560421578..14f54bd71 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -50,31 +50,21 @@ def __init__(self, dmp_name, tau):
         self.motion_completed = False
         self.motion_cancelled = False
 
-    def move_to(self, goal):
+    def move_to(self, goal_pose):
         '''Moves the end effector to the given goal position (which is expected
         to be given with respect to the base link frame) by following a trajectory
         as encoded by self.dmp_name.
 
         Keyword arguments:
-        goal: numpy.ndarray -- end effector goal position in the base link frame
+        goal_pose: geometry_msgs.msg.PoseStamped -- end effector goal pose
 
         '''
-        initial_pos = None
-        try:
-            self.tf_listener.waitForTransform(self.base_link_frame_name,
-                                              self.palm_link_name,
-                                              rospy.Time.now(),
-                                              rospy.Duration(30))
-            (trans, _) = self.tf_listener.lookupTransform(self.base_link_frame_name,
-                                                          self.palm_link_name,
-                                                          rospy.Time(0))
-            initial_pos = np.array(trans)
-        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-            initial_pos = np.zeros(3)
-
-        self.follow_path(initial_pos, goal)
-
-    def follow_path(self, initial_pos,  goal):
+        (trans, _) = self.get_transform(self.odom_frame_name, self.palm_link_name, rospy.Time.now())
+        initial_pos_odom = np.array(trans)
+        goal_odom = self.transform_pose(goal_pose, self.odom_frame_name)
+        self.follow_path(initial_pos_odom, goal_odom)
+
+    def follow_path(self, initial_pos_odom,  goal_pose_odom):
         '''Moves a manipulator so that it follows the given path. If whole body
         motion is enabled and some points on the path lie outside the reachable
         workspace, the base is moved accordingly as well. The path is followed
@@ -105,15 +95,25 @@ def follow_path(self, initial_pos,  goal):
           minimum sigma value (determined experimentally)
 
         Keyword arguments:
-        path: numpy.ndarray -- a 2D array of points (each row represents a point)
+        initial_pos_odom: numpy.ndarray -- the initial position of the end effector
+                                           in the odometry frame
+        goal_pose_odom: geometry_msgs.msg.PoseStamped -- end effector goal pose
+                                                         in the odometry frame
 
         '''
         rospy.loginfo('[move_arm/dmp/follow_path] Executing motion')
         trans, _ = self.get_transform(self.odom_frame_name, self.palm_link_name, rospy.Time(0))
         current_pos = np.array([trans[0], trans[1], trans[2]])
+        goal = np.array([goal_pose_odom.pose.position.x,
+                         goal_pose_odom.pose.position.y,
+                         goal_pose_odom.pose.position.z])
+
         distance_to_goal = np.linalg.norm((goal - current_pos))
 
-        initial_pose = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0., 0., 0.])
+        initial_pose = np.array([initial_pos_odom[0],
+                                 initial_pos_odom[1],
+                                 initial_pos_odom[2],
+                                 0., 0., 0.])
         goal_pose = np.array([goal[0], goal[1], goal[2], 0., 0., 0.])
 
         current_path = initial_pose[:3]
@@ -220,30 +220,22 @@ def get_transform(self, target_frame, source_frame, tf_time):
                 continue
         return (trans, rot)
 
-    def transform_pose(self, position, source_frame, target_frame):
-        '''Transforms a given (x, y, z) position (assuming a (0, 0, 0) rotation)
-        from the source frame to the target frame. Returns a numpy array with the
-        (x, y, z) position represented in the target frame.
+    def transform_pose(self, pose_msg, target_frame):
+        '''Transforms the given pose to the target frame.
 
         Keyword arguments:
-        position: np.array -- (x, y, z) position to be transformed
-        source_frame: str -- name of the transformation source frame
+        pose_msg: geometry_msgs.msg.PoseStamped -- pose to be transformed
         target_frame: str -- name of the transformation target frame
 
         '''
-        pose_msg = PoseStamped()
-        pose_msg.header.frame_id = source_frame
-        pose_msg.pose.position.x = position[0]
-        pose_msg.pose.position.y = position[1]
-        pose_msg.pose.position.z = position[2]
-
+        transformed_pose = pose_msg
         while not rospy.is_shutdown():
             try:
-                pose_ = self.tf_listener.transformPose(target_frame, pose_msg)
+                transformed_pose = self.tf_listener.transformPose(target_frame, pose_msg)
                 break
             except:
                 continue
-        return np.array([pose_.pose.position.x, pose_.pose.position.y, pose_.pose.position.z])
+        return transformed_pose
 
     def publish_path(self, path):
         '''Publishes the given path to the topic specified by self.dmp_executor_path_topic.

From 793b07ee218fde6daf2b5fa83ee0cfe0fc009d02 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 18 Sep 2020 20:14:34 +0200
Subject: [PATCH 111/264] Add point cloud filtering to improve person pose
 estimation

---
 .../ros/src/mdr_find_people/find_people.py    | 31 ++++++++++++++++---
 1 file changed, 26 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index f8f22b751..07a7e1cc3 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -1,17 +1,20 @@
 #!/usr/bin/env python
 
 import os
+
 import numpy as np
+import pcl
+import face_recognition
+from sklearn.impute import SimpleImputer
 
 import rospy
 from rospkg import RosPack
-import face_recognition
-
 from std_msgs.msg import Header
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz
+                                      crop_cloud_to_xyz, crop_organized_cloud_msg
 from mas_perception_libs.visualization import draw_labeled_boxes
 from ssd_keras_ros import SSDKerasObjectDetector
 
@@ -67,8 +70,26 @@ def get_people_poses(cloud_msg, predictions, bounding_boxes):
         for i, _ in enumerate(predictions):
             bb2d = bounding_boxes[i]
 
-            cropped_cloud = crop_cloud_to_xyz(cloud_msg, bb2d)
-            mean_coords = np.nanmean(np.reshape(cropped_cloud, (-1, 3)), axis=0)
+            cropped_cloud = crop_organized_cloud_msg(cloud_msg, bb2d)
+            obj_coords = crop_cloud_to_xyz(cloud_msg, bb2d)
+
+            imputer = SimpleImputer(missing_values=np.nan, strategy='most_frequent')
+            imputer.fit(obj_coords.reshape(obj_coords.shape[0]*obj_coords.shape[1], obj_coords.shape[2]))
+            obj_coords_without_nans = imputer.transform(obj_coords.reshape(obj_coords.shape[0]*obj_coords.shape[1], obj_coords.shape[2]))
+
+            ## PCL Outlier Removal:
+            cloud_without_nans = pcl.PointCloud(obj_coords_without_nans.astype(np.float32))
+            voxel_grid_filter = cloud_without_nans.make_voxel_grid_filter()
+            voxel_grid_filter.set_leaf_size(0.01, 0.01, 0.01)
+            cloud_filtered = voxel_grid_filter.filter()
+
+            passthrough_filter = cloud_filtered.make_passthrough_filter()
+            passthrough_filter.set_filter_field_name("z")
+            passthrough_filter.set_filter_limits(0., 2.5)
+            cloud_filtered = passthrough_filter.filter()
+            cloud_filtered_array = cloud_filtered.to_array()
+
+            mean_coords = np.nanmean(cloud_filtered_array, axis=0)
             mean_ps = PoseStamped()
             mean_ps.header = Header(frame_id=cloud_msg.header.frame_id)
             mean_ps.pose = Pose(Point(mean_coords[0], mean_coords[1], mean_coords[2]), Quaternion())

From 942c1e4b5aa5c5bb2c50b7acd7cc272027d0502d Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 25 Sep 2020 16:08:53 +0200
Subject: [PATCH 112/264] Utilize person pose for pre-approach (in action and
 scenario)

---
 .../ros/action/HandOver.action                |  1 +
 .../ros/launch/hand_over.launch               |  4 +-
 .../ros/scripts/hand_over_action              |  6 ++-
 .../src/mdr_hand_over_action/action_states.py | 37 ++++++++++++++++++-
 .../ros/launch/hand_over.launch               |  2 +-
 .../scenario_states/hand_over.py              |  3 +-
 .../scenario_states/identify_posture.py       | 14 +++++--
 7 files changed, 57 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
index 96fbb520b..0de74b191 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/action/HandOver.action
@@ -3,6 +3,7 @@ string posture_type
 bool obstacle
 bool release_detection
 bool context_aware
+geometry_msgs/PoseStamped person_pose
 ---
 # result
 bool success
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index e5e39ad2c..a61c428ca 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -2,12 +2,14 @@
 <launch>
     <node pkg="mdr_hand_over_action" type="hand_over_action" name="hand_over_server" output="screen">
         <param name="move_arm_server" value="move_arm_server" />
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
+        <param name="move_base_server" value="move_base_server" />
+        <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
         <param name="init_config_name" value="neutral" />
         <param name="hand_over_dmp" value="grasp.yaml" />
         <param name="hand_over_position_policy_parameters_file" value="learned_position_policy_parameters.pkl" />
         <param name="hand_over_policy_config_dir" value="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
         <param name="hand_over_dmp_weights_dir" value="$(find mdr_hand_over_action)/config/trajectory_weights/" />
         <param name="dmp_tau" value="30" />
+        <param name="person_dist_threshold" value="0.5" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
index 46d63e3ec..0faae97e1 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
@@ -13,22 +13,26 @@ class HandOverServer(object):
     def __init__(self):
         gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mdr_gripper_controller')
         move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
+        move_base_server = rospy.get_param('~move_base_server', 'move_base_server')
         init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
         hand_over_dmp = rospy.get_param('~hand_over_dmp', '')
         pos_policy_params_file = rospy.get_param('~hand_over_position_policy_parameters_file', '')
         dmp_weights_dir = rospy.get_param('~hand_over_dmp_weights_dir', '')
         policy_config_dir = rospy.get_param('~hand_over_policy_config_dir', '')
         dmp_tau = rospy.get_param('~dmp_tau', 'dmp_tau')
+        person_dist_threshold = rospy.get_param('~person_dist_threshold', 'person_dist_threshold')
 
         rospy.loginfo('[hand_over] Initialising state machine')
         self.action_sm = HandOverSM(gripper_controller_pkg_name=gripper_controller_pkg_name,
                                       move_arm_server=move_arm_server,
+                                      move_base_server=move_base_server,
                                       init_config_name=init_config_name,
                                       hand_over_policy_config_dir=policy_config_dir,
                                       hand_over_dmp_weights_dir=dmp_weights_dir,
                                       hand_over_position_policy_parameters_file=pos_policy_params_file,
                                       hand_over_dmp=hand_over_dmp,
-                                      dmp_tau=dmp_tau)
+                                      dmp_tau=dmp_tau,
+                                      person_dist_threshold=person_dist_threshold)
         rospy.loginfo('[hand_over] State machine initialised')
 
         self.action_server = actionlib.SimpleActionServer('hand_over_server',
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 462912373..1cbb37a02 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -13,6 +13,7 @@
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
+from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
 from mdr_hand_over_action.msg import HandOverGoal, HandOverResult
 
 from mdr_move_arm_action.dmp import DMPExecutor
@@ -24,12 +25,14 @@ def __init__(self, timeout=120.0,
                  gripper_controller_pkg_name='mdr_gripper_controller',
                  force_sensor_topic='/wrench/raw',
                  move_arm_server='move_arm_server',
+                 move_base_server='move_base_server',
                  init_config_name = 'neutral',
                  hand_over_policy_config_dir='',
                  hand_over_position_policy_parameters_file = 'learned_position_policy_parameters.pkl',
                  hand_over_dmp_weights_dir='',
                  hand_over_dmp = 'grasp.yaml',
                  dmp_tau = 30.,
+                 person_dist_threshold = 0.5,
                  max_recovery_attempts=1):
         super(HandOverSM, self).__init__(
             'HandOver', [], max_recovery_attempts)
@@ -40,6 +43,7 @@ def __init__(self, timeout=120.0,
         self.gripper = GripperControllerClass()
 
         self.move_arm_server = move_arm_server
+        self.move_base_server = move_base_server
 
         self.init_config_name = init_config_name
         self.hand_over_policy_config_dir = hand_over_policy_config_dir
@@ -49,6 +53,7 @@ def __init__(self, timeout=120.0,
         self.dmp_tau = dmp_tau
         self.hand_over_position_policy_parameters_file = join(self.hand_over_policy_config_dir, hand_over_position_policy_parameters_file)
         self.hand_over_position_policy_parameters = self.load_policy_params_from_file(self.hand_over_position_policy_parameters_file)
+        self.person_dist_threshold = person_dist_threshold
 
         self.tf_listener = tf.TransformListener()
         self.force_sensor_topic = force_sensor_topic
@@ -67,6 +72,14 @@ def init(self):
             rospy.logerr('[hand_over] %s', str(exc))
             return FTSMTransitions.INIT_FAILED
 
+        try:
+            self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction)
+            rospy.loginfo('[hand_over] Waiting for %s server', self.move_base_server)
+            self.move_base_client.wait_for_server()
+        except Exception as exc:
+            rospy.logerr('[hand_over] %s', str(exc))
+            return FTSMTransitions.INIT_FAILED
+
         # Start with arm in neutral position:
         rospy.loginfo('[hand_over] Moving arm to neutral position...')
         self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
@@ -130,7 +143,28 @@ def running(self):
         self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename)
 
         ## TODO: determine whether aligning base is necessary for hand_over action here:
-        ## ...
+        self.goal.release_detection = False
+        self.goal.person_pose = self.tf_listener.transformPose("base_link", self.goal.person_pose)
+        print("\n\n Person pose, x: {} \n\n", self.goal.person_pose.pose.position.x)
+        print("\n\n Person pose, y: {} \n\n", self.goal.person_pose.pose.position.y)
+
+        distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 + self.goal.person_pose.pose.position.y**2)
+        if distance_to_person > self.person_dist_threshold:
+            move_to_person_goal = MoveBaseGoal()
+            move_to_person_goal.goal_type = MoveBaseGoal.POSE
+            move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id
+            move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.2
+            move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.2
+            rospy.loginfo('[hand_over] Moving to person...')
+
+            self.move_base_client.send_goal(move_to_person_goal)
+            # TODO: check for action getting stuck (due to very close obst, etc.):
+            self.move_base_client.wait_for_result(rospy.Duration(10.))
+            result = self.move_base_client.get_result()
+
+        # Start with arm in neutral position:
+        rospy.loginfo('[hand_over] Moving arm to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
 
         # Move to chosen hand_over position, along appropriate trajectory:
         rospy.loginfo('[hand_over] Handing object over...')
@@ -222,7 +256,6 @@ def detect_object_reception(self):
 
         self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
 
-        print(self.cumsum_x)
         if self.cumsum_x > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
index ee3e3b5ac..a2f46545d 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
@@ -9,7 +9,7 @@
     </include>
 
     <!-- Actions -->
-    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
+    <include file="$(find mas_hsr_find_people)/ros/launch/find_people.launch" />
     <include file="$(find mdr_hand_over_action)/ros/launch/hand_over.launch" />
 
     <!-- State machine -->
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
index 4952c589f..83ce2f590 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/hand_over.py
@@ -9,7 +9,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'hand_over',
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'failed', 'failed_after_retrying'],
-                                   input_keys=['posture'])
+                                   input_keys=['posture', 'person_pose'])
         self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
         self.action_server = kwargs.get('action_server', 'hand_over_server')
         self.obstacle_present = kwargs.get('obstacle_present', False)
@@ -28,6 +28,7 @@ def execute(self, userdata):
         goal.obstacle = self.obstacle_present
         goal.context_aware = self.context_aware
         goal.release_detection = self.release_detection
+        goal.person_pose = userdata.person_pose
 
         # calling the actionlib server and waiting for the execution to end
         rospy.loginfo('[hand_over] Sending action lib goal to {0}'.format(self.action_server))
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
index d8466725b..3ad380dac 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/src/mdr_demo_context_aware_hand_over/scenario_states/identify_posture.py
@@ -7,9 +7,9 @@ class IdentifyPosture(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'identify_posture',
                                    save_sm_state=save_sm_state,
-                                   outcomes=['succeeded'],
+                                   outcomes=['succeeded', 'failed'],
                                    input_keys=['person_list'],
-                                   output_keys=['posture'])
+                                   output_keys=['posture', 'person_pose'])
         self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
         self.posture_height_width_ratio_ranges = kwargs.get('posture_height_width_ratio_ranges', {})
 
@@ -17,8 +17,12 @@ def execute(self, userdata):
         self.say('I will hand over the object to the closest person')
 
         closest_person_idx = find_closest_person(userdata.person_list.persons)
-        first_person_image_bb_width = float(userdata.person_list.persons[closest_person_idx].body_image.width)
-        first_person_image_bb_height = float(userdata.person_list.persons[closest_person_idx].body_image.height)
+        if not userdata.person_list.persons[closest_person_idx].views:
+            rospy.loginfo('[identify_posture] Failed to get views of person ')
+            return 'failed'
+
+        first_person_image_bb_width = float(userdata.person_list.persons[closest_person_idx].views[0].image.width)
+        first_person_image_bb_height = float(userdata.person_list.persons[closest_person_idx].views[0].image.height)
         bb_height_width_ratio = first_person_image_bb_height / first_person_image_bb_width
         rospy.loginfo('[identify_posture] Height width bb ratio: {0}'.format(bb_height_width_ratio))
 
@@ -27,6 +31,8 @@ def execute(self, userdata):
             if ratio_ranges[0] <= bb_height_width_ratio <= ratio_ranges[1]:
                 person_posture = posture
 
+        userdata.person_pose = userdata.person_list.persons[closest_person_idx].pose
+
         rospy.loginfo('[identify_posture] Found a {0} person'.format(person_posture))
         userdata.posture = person_posture
         return 'succeeded'

From 6a36dae6f6400e97a8bb6c1519902925881951f0 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 25 Sep 2020 16:10:15 +0200
Subject: [PATCH 113/264] [demo_hand_over] Add transition to detect_people if
 identify_posture fails

---
 .../config/context_aware_hand_over_sm.yaml                     | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
index 799419de6..89cb2a23f 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/config/context_aware_hand_over_sm.yaml
@@ -28,6 +28,9 @@ state_descriptions:
             - transition:
                 name: succeeded
                 state: HAND_OVER
+            - transition:
+                name: failed
+                state: DETECT_PERSON
         arguments:
             - argument:
                 name: posture_height_width_ratio_ranges

From 5588aae9f8c6d5444a375820649e783b48b70b81 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 2 Oct 2020 18:11:53 +0200
Subject: [PATCH 114/264] [hand_over_action] Fixed a small bug due to which the
 arm wasn't moving to the hand over pose

In particular, the timestamp of the hand over pose was set to the
current ROS time, so the pose could not be transformed during DMP
execution
---
 .../src/mdr_hand_over_action/action_states.py  | 18 +++++++-----------
 1 file changed, 7 insertions(+), 11 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 1cbb37a02..328b7056d 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -47,7 +47,7 @@ def __init__(self, timeout=120.0,
 
         self.init_config_name = init_config_name
         self.hand_over_policy_config_dir = hand_over_policy_config_dir
-        self.hand_over_dmp_weights_dir = hand_over_dmp_weights_dir 
+        self.hand_over_dmp_weights_dir = hand_over_dmp_weights_dir
 
         self.hand_over_dmp = hand_over_dmp
         self.dmp_tau = dmp_tau
@@ -89,7 +89,6 @@ def init(self):
     def running(self):
         hand_over_pose = PoseStamped()
         hand_over_pose.header.frame_id = 'base_link'
-        hand_over_pose.header.stamp = rospy.Time.now()
         if self.goal.context_aware:
             # > Pick context-dependent hand-over position:
             policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
@@ -101,7 +100,7 @@ def running(self):
                 context_vector = np.array([0.5, 0.0, 0.0])
             elif self.goal.posture_type == 'lying':
                 context_vector = np.array([0.0, 0.7, 0.0])
-        
+
             # Sample upper-level policy (with no exploration) for hand-over position;
             # by calculating the mean of the linear-Gaussian model, given context vector s:
             hand_over_position = np.squeeze(policy_parameter_a + context_vector.dot(policy_parameter_A))
@@ -120,17 +119,15 @@ def running(self):
         hand_over_pose.pose.orientation.z = 0.000
         hand_over_pose.pose.orientation.w = 1.000
 
-        pose_base_link = self.tf_listener.transformPose('base_link', hand_over_pose)
-
         # > Pick context_dependent hand-over trajectory shape:
         if not self.goal.obstacle:
             trajectory_weights_filename = 'grasp.yaml'
         else:
             ## Uncomment one of the following if testing other learned parameters is desired:
-            
+
             ## Initial learned trajectory:
             # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml'
-            
+
             ## Smoothed learned trajectory:
             # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights.yaml'
 
@@ -143,7 +140,6 @@ def running(self):
         self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename)
 
         ## TODO: determine whether aligning base is necessary for hand_over action here:
-        self.goal.release_detection = False
         self.goal.person_pose = self.tf_listener.transformPose("base_link", self.goal.person_pose)
         print("\n\n Person pose, x: {} \n\n", self.goal.person_pose.pose.position.x)
         print("\n\n Person pose, y: {} \n\n", self.goal.person_pose.pose.position.y)
@@ -168,7 +164,7 @@ def running(self):
 
         # Move to chosen hand_over position, along appropriate trajectory:
         rospy.loginfo('[hand_over] Handing object over...')
-        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
+        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, hand_over_pose)
 
         if not self.goal.release_detection:
             # Naive object release strategy:
@@ -195,7 +191,7 @@ def running(self):
                 rospy.sleep(0.1)
 
             # If a pull is detected, release the object:
-            if self.object_reception_detected:        
+            if self.object_reception_detected:
                 rospy.loginfo('[hand_over] Opening the gripper to release object...')
                 self.gripper.open()
             else:
@@ -259,6 +255,6 @@ def detect_object_reception(self):
         if self.cumsum_x > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
-        
+
     def force_sensor_cb(self, force_sensor_msg):
         self.latest_force_measurement_x = force_sensor_msg.wrench.force.x

From a2ea35dfc3202d4964b6df291ef4a6a2d4c466b4 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:20:17 +0200
Subject: [PATCH 115/264] [hand_over_action] Add force sensor topic

---
 .../mdr_hand_over_action/ros/scripts/hand_over_action           | 2 ++
 .../ros/src/mdr_hand_over_action/action_states.py               | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
index 0faae97e1..26ec26203 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action
@@ -14,6 +14,7 @@ class HandOverServer(object):
         gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mdr_gripper_controller')
         move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
         move_base_server = rospy.get_param('~move_base_server', 'move_base_server')
+        force_sensor_topic = rospy.get_param('~force_sensor_topic', '/wrist/compensated')
         init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
         hand_over_dmp = rospy.get_param('~hand_over_dmp', '')
         pos_policy_params_file = rospy.get_param('~hand_over_position_policy_parameters_file', '')
@@ -26,6 +27,7 @@ class HandOverServer(object):
         self.action_sm = HandOverSM(gripper_controller_pkg_name=gripper_controller_pkg_name,
                                       move_arm_server=move_arm_server,
                                       move_base_server=move_base_server,
+                                      force_sensor_topic=force_sensor_topic,
                                       init_config_name=init_config_name,
                                       hand_over_policy_config_dir=policy_config_dir,
                                       hand_over_dmp_weights_dir=dmp_weights_dir,
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 1cbb37a02..f8e5e1696 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -23,7 +23,7 @@
 class HandOverSM(ActionSMBase):
     def __init__(self, timeout=120.0,
                  gripper_controller_pkg_name='mdr_gripper_controller',
-                 force_sensor_topic='/wrench/raw',
+                 force_sensor_topic='/wrench/compensated',
                  move_arm_server='move_arm_server',
                  move_base_server='move_base_server',
                  init_config_name = 'neutral',

From c516ebab23345e0a8d357b79179f89970609c074 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:22:39 +0200
Subject: [PATCH 116/264] [hand_over_action] Update action client script

---
 .../ros/scripts/hand_over_action_client_test  | 62 +++++++------------
 1 file changed, 24 insertions(+), 38 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
index ab29e0609..c7648471d 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
@@ -3,8 +3,6 @@ from __future__ import print_function
 import argparse
 import rospy
 import actionlib
-import numpy as np
-import ast
 
 from mdr_hand_over_action.msg import HandOverAction, HandOverGoal
 
@@ -14,41 +12,29 @@ if __name__ == '__main__':
     release_detection = [0, 1]
 
     parser = argparse.ArgumentParser(description='help text')
-    parser.add_argument("--context", "-c", help="list of context parameters. E.g.: ['sitting', 0]") 
+    parser.add_argument("--posture", "-p", type=str, default='seated',
+                        help="posture context parameters. Choose from 'standing', 'seated', or 'lying'.") 
+    parser.add_argument("--obstacle", "-o", type=bool, default=0,
+                        help="list of context parameters. Choose 0 for no obstacle or 1 for obstacle.") 
+    parser.add_argument("--release_detection", "-r", type=bool, default=1,
+                        help="list of context parameters. E.g.: 0") 
     args = parser.parse_args()
 
-    context_params = ast.literal_eval(args.context)
-
-    if not context_params:
-        print('No context parameters provided!')
-        print('Example inputs: ')
-        print('- python hand_over_action_client_test -c "[\'standing\', 0]"')
-        print('- python hand_over_action_client_test -c "[\'sitting\', 1]"')
-    elif len(context_params) != 2:
-        print('Expected two context parameters: posture and obstacle!')
-    elif context_params[0] not in posture_contexts:
-        print('Invalid posture context!')
-    elif context_params[1] not in obstacle_contexts:
-        print('Invalid obstacle context!')
-    else:
-        posture_context = context_params[0]
-        obstacle_context = bool(context_params[1])
-        release_detection = 1
-
-        rospy.init_node('hand_over_action_client_test')
-
-        client = actionlib.SimpleActionClient('/hand_over_server', HandOverAction)
-        client.wait_for_server()
-
-        goal = HandOverGoal()
-
-        rospy.loginfo('Setting goal for a %s person, with %s obstacle in-between...' % (posture_context, 'an' if obstacle_context else 'no'))
-        goal.posture_type = posture_context
-        goal.obstacle = obstacle_context
-        goal.release_detection = release_detection
-
-        timeout = 15.0
-        rospy.loginfo('Sending action lib goal to hand_over_server')
-        client.send_goal(goal)
-        client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
-        print(client.get_result())
+    rospy.init_node('hand_over_action_client_test')
+
+    client = actionlib.SimpleActionClient('/hand_over_server', HandOverAction)
+    client.wait_for_server()
+
+    goal = HandOverGoal()
+
+    rospy.loginfo('Setting goal for a %s person, with %s obstacle in-between...' % (args.posture, 'an' if args.obstacle else 'no'))
+    goal.posture_type = args.posture
+    goal.obstacle = args.obstacle
+    goal.release_detection = args.release_detection
+    goal.person_pose.header.frame_id = "base_link"
+
+    timeout = 15.0
+    rospy.loginfo('Sending action lib goal to hand_over_server')
+    client.send_goal(goal)
+    client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
+    print(client.get_result())

From 9cac5d57a625e604094bd31d906c1576295bfd66 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:25:11 +0200
Subject: [PATCH 117/264] [hand_over_action] Modify release detection strategy
 to use force readings in z-axis

---
 .../src/mdr_hand_over_action/action_states.py | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index f8e5e1696..7c1d71487 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -58,8 +58,8 @@ def __init__(self, timeout=120.0,
         self.tf_listener = tf.TransformListener()
         self.force_sensor_topic = force_sensor_topic
 
-        self.latest_force_measurement_x = 0.
-        self.cumsum_x = 0
+        self.latest_force_measurement_z = 0.
+        self.cumsum_z = 0
         self.force_detection_threshold = 1.
         self.object_reception_detected = False
 
@@ -187,7 +187,7 @@ def running(self):
             rospy.loginfo('[hand_over] Waiting for object to be received...')
             rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
             self.object_reception_detected = False
-            self.cumsum_x = 0.
+            self.cumsum_z = 0.
 
             start_time = rospy.get_time()
             while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected:
@@ -247,18 +247,18 @@ def load_policy_params_from_file(self, filename):
             return pickle.load(f)
 
     def detect_object_reception(self):
-        mu_0 = -2
-        mu_1 = -20
-        std = 1
+        mu_0 = 0.
+        mu_1 = -6.
+        std = 1.
 
         pdf_0 = norm(mu_0, std)
         pdf_1 = norm(mu_1, std)
 
-        self.cumsum_x += max(0, np.log(pdf_1.pdf(self.latest_force_measurement_x) / pdf_0.pdf(self.latest_force_measurement_x)))
+        self.cumsum_z += max(0., np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
 
-        if self.cumsum_x > self.force_detection_threshold:
+        if self.cumsum_z > self.force_detection_threshold:
             rospy.loginfo('[hand_over] Object reception detected!')
             self.object_reception_detected = True
-        
+
     def force_sensor_cb(self, force_sensor_msg):
-        self.latest_force_measurement_x = force_sensor_msg.wrench.force.x
+        self.latest_force_measurement_z = force_sensor_msg.wrench.force.z

From 30b7b6b38b9d39c3ac8583349d68116d5169f21a Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:26:56 +0200
Subject: [PATCH 118/264] [hand_over_action] Remove hsr-specific topics

---
 .../mdr_hand_over_action/ros/launch/hand_over.launch         | 5 +++--
 .../ros/launch/hand_over.launch                              | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index a61c428ca..100d5101b 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -3,13 +3,14 @@
     <node pkg="mdr_hand_over_action" type="hand_over_action" name="hand_over_server" output="screen">
         <param name="move_arm_server" value="move_arm_server" />
         <param name="move_base_server" value="move_base_server" />
-        <param name="gripper_controller_pkg_name" value="mas_hsr_gripper_controller" />
+        <param name="force_sensor_topic" value="/wrench/compensated" />
+        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
         <param name="init_config_name" value="neutral" />
         <param name="hand_over_dmp" value="grasp.yaml" />
         <param name="hand_over_position_policy_parameters_file" value="learned_position_policy_parameters.pkl" />
         <param name="hand_over_policy_config_dir" value="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
         <param name="hand_over_dmp_weights_dir" value="$(find mdr_hand_over_action)/config/trajectory_weights/" />
         <param name="dmp_tau" value="30" />
-        <param name="person_dist_threshold" value="0.5" />
+        <param name="person_dist_threshold" value="0.4" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
index a2f46545d..ee3e3b5ac 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_hand_over/ros/launch/hand_over.launch
@@ -9,7 +9,7 @@
     </include>
 
     <!-- Actions -->
-    <include file="$(find mas_hsr_find_people)/ros/launch/find_people.launch" />
+    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
     <include file="$(find mdr_hand_over_action)/ros/launch/hand_over.launch" />
 
     <!-- State machine -->

From 6f983fb30aa382f6d53c657bf87a8842cd31fcef Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:28:23 +0200
Subject: [PATCH 119/264] [hand_over_action] Remove specification of post
 message timestamp

---
 .../ros/src/mdr_hand_over_action/action_states.py              | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index 7c1d71487..bcfc30747 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -89,7 +89,6 @@ def init(self):
     def running(self):
         hand_over_pose = PoseStamped()
         hand_over_pose.header.frame_id = 'base_link'
-        hand_over_pose.header.stamp = rospy.Time.now()
         if self.goal.context_aware:
             # > Pick context-dependent hand-over position:
             policy_parameter_a = self.hand_over_position_policy_parameters[0][0]
@@ -168,7 +167,7 @@ def running(self):
 
         # Move to chosen hand_over position, along appropriate trajectory:
         rospy.loginfo('[hand_over] Handing object over...')
-        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link)
+        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, hand_over_pose)
 
         if not self.goal.release_detection:
             # Naive object release strategy:

From 20bffbe8d82f3e3f42bb81b189195b2f5d3fea17 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 23 Oct 2020 18:29:30 +0200
Subject: [PATCH 120/264] [hand_over_action] Remove unnecessary comments and
 add move_base goal cancellation after motion duration

---
 .../src/mdr_hand_over_action/action_states.py    | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index bcfc30747..9c6927b15 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -119,17 +119,15 @@ def running(self):
         hand_over_pose.pose.orientation.z = 0.000
         hand_over_pose.pose.orientation.w = 1.000
 
-        pose_base_link = self.tf_listener.transformPose('base_link', hand_over_pose)
-
         # > Pick context_dependent hand-over trajectory shape:
         if not self.goal.obstacle:
             trajectory_weights_filename = 'grasp.yaml'
         else:
             ## Uncomment one of the following if testing other learned parameters is desired:
-            
+
             ## Initial learned trajectory:
             # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml'
-            
+
             ## Smoothed learned trajectory:
             # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights.yaml'
 
@@ -141,25 +139,21 @@ def running(self):
 
         self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename)
 
-        ## TODO: determine whether aligning base is necessary for hand_over action here:
-        self.goal.release_detection = False
         self.goal.person_pose = self.tf_listener.transformPose("base_link", self.goal.person_pose)
-        print("\n\n Person pose, x: {} \n\n", self.goal.person_pose.pose.position.x)
-        print("\n\n Person pose, y: {} \n\n", self.goal.person_pose.pose.position.y)
 
         distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 + self.goal.person_pose.pose.position.y**2)
         if distance_to_person > self.person_dist_threshold:
             move_to_person_goal = MoveBaseGoal()
             move_to_person_goal.goal_type = MoveBaseGoal.POSE
             move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id
-            move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.2
-            move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.2
+            move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.3
+            move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.3
             rospy.loginfo('[hand_over] Moving to person...')
 
             self.move_base_client.send_goal(move_to_person_goal)
-            # TODO: check for action getting stuck (due to very close obst, etc.):
             self.move_base_client.wait_for_result(rospy.Duration(10.))
             result = self.move_base_client.get_result()
+            self.move_base_client.cancel_goal()
 
         # Start with arm in neutral position:
         rospy.loginfo('[hand_over] Moving arm to neutral position...')

From c06ed075ea2f4fece2d61955a983f91d4fd81e1a Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 23 Oct 2020 20:22:30 +0200
Subject: [PATCH 121/264] [hand_over_action/action_states] Resolved some Codacy
 complaints

---
 .../ros/src/mdr_hand_over_action/action_states.py      | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
index f7f502f55..05800e623 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/src/mdr_hand_over_action/action_states.py
@@ -14,9 +14,7 @@
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
 from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
-from mdr_hand_over_action.msg import HandOverGoal, HandOverResult
-
-from mdr_move_arm_action.dmp import DMPExecutor
+from mdr_hand_over_action.msg import HandOverResult
 
 from importlib import import_module
 
@@ -151,8 +149,12 @@ def running(self):
             rospy.loginfo('[hand_over] Moving to person...')
 
             self.move_base_client.send_goal(move_to_person_goal)
+
+            # we try moving towards the person before handing the object over;
+            # the person should be close by, so we move for a short time
+            # since the robot will either reach the person in that time,
+            # or will stop as close to the person as possible
             self.move_base_client.wait_for_result(rospy.Duration(10.))
-            result = self.move_base_client.get_result()
             self.move_base_client.cancel_goal()
 
         # Start with arm in neutral position:

From 6b83fe92314d713f46956a47b57769460ab99db4 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 23 Oct 2020 20:25:27 +0200
Subject: [PATCH 122/264] [find_people/src/find_people] Removed unnecessary
 line

---
 .../mdr_find_people/ros/src/mdr_find_people/find_people.py      | 2 --
 1 file changed, 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index 07a7e1cc3..146e28f4b 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -70,9 +70,7 @@ def get_people_poses(cloud_msg, predictions, bounding_boxes):
         for i, _ in enumerate(predictions):
             bb2d = bounding_boxes[i]
 
-            cropped_cloud = crop_organized_cloud_msg(cloud_msg, bb2d)
             obj_coords = crop_cloud_to_xyz(cloud_msg, bb2d)
-
             imputer = SimpleImputer(missing_values=np.nan, strategy='most_frequent')
             imputer.fit(obj_coords.reshape(obj_coords.shape[0]*obj_coords.shape[1], obj_coords.shape[2]))
             obj_coords_without_nans = imputer.transform(obj_coords.reshape(obj_coords.shape[0]*obj_coords.shape[1], obj_coords.shape[2]))

From f24117732ea0ba2b81e6835fdeb9bf93d4ffee6f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 25 Oct 2020 13:42:56 +0100
Subject: [PATCH 123/264] [manipulation_actions] Use launch file args in the
 action server launchers

Related to https://github.com/b-it-bots/mas_domestic_robotics/issues/241
---
 .../ros/launch/hand_over.launch               | 34 +++++++++++++------
 .../ros/launch/handle_open_action.launch      | 31 +++++++++++------
 .../ros/launch/move_arm.launch                | 25 +++++++++-----
 .../ros/launch/pickup_action.launch           | 34 +++++++++++++------
 .../ros/launch/place_action.launch            | 28 ++++++++++-----
 .../ros/launch/push_pull_object.launch        | 19 +++++++----
 6 files changed, 116 insertions(+), 55 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
index 100d5101b..6dd91c874 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/launch/hand_over.launch
@@ -1,16 +1,28 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="move_base_server" />
+    <arg name="force_sensor_topic" default="/wrench/compensated" />
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="init_config_name" default="pregrasp" />
+    <arg name="hand_over_dmp" default="grasp.yaml" />
+    <arg name="hand_over_position_policy_parameters_file" default="learned_position_policy_parameters.pkl" />
+    <arg name="hand_over_policy_config_dir" default="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
+    <arg name="hand_over_dmp_weights_dir" default="$(find mdr_hand_over_action)/config/trajectory_weights/" />
+    <arg name="dmp_tau" default="30" />
+    <arg name="person_dist_threshold" default="0.4" />
+
     <node pkg="mdr_hand_over_action" type="hand_over_action" name="hand_over_server" output="screen">
-        <param name="move_arm_server" value="move_arm_server" />
-        <param name="move_base_server" value="move_base_server" />
-        <param name="force_sensor_topic" value="/wrench/compensated" />
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
-        <param name="init_config_name" value="neutral" />
-        <param name="hand_over_dmp" value="grasp.yaml" />
-        <param name="hand_over_position_policy_parameters_file" value="learned_position_policy_parameters.pkl" />
-        <param name="hand_over_policy_config_dir" value="$(find mdr_hand_over_action)/config/learned_policy_parameters/" />
-        <param name="hand_over_dmp_weights_dir" value="$(find mdr_hand_over_action)/config/trajectory_weights/" />
-        <param name="dmp_tau" value="30" />
-        <param name="person_dist_threshold" value="0.4" />
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="force_sensor_topic" value="$(arg force_sensor_topic)" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="init_config_name" value="$(arg init_config_name)" />
+        <param name="hand_over_dmp" value="$(arg hand_over_dmp)" />
+        <param name="hand_over_position_policy_parameters_file" value="$(arg hand_over_position_policy_parameters_file)" />
+        <param name="hand_over_policy_config_dir" value="$(arg hand_over_policy_config_dir)" />
+        <param name="hand_over_dmp_weights_dir" value="$(arg hand_over_dmp_weights_dir)" />
+        <param name="dmp_tau" value="$(arg dmp_tau)" />
+        <param name="person_dist_threshold" value="$(arg person_dist_threshold)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
index 4946e6a9b..4ca1f1139 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/ros/launch/handle_open_action.launch
@@ -1,15 +1,26 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="move_base_server" />
+    <arg name="move_forward_server" default="move_forward_server" />
+    <arg name="detect_handle_server" default="detect_handle_server" />
+    <arg name="force_sensor_topic" default="/wrist_force/raw"/>
+    <arg name="pregrasp_config_name" default="pregrasp_low" />
+    <arg name="final_config_name" default="pregrasp" />
+    <arg name="handle_open_dmp" default="$(find mdr_handle_open_action)/config/trajectory_weights/weights_grasp_handle.yaml" />
+    <arg name="dmp_tau" default="30" />
+
     <node pkg="mdr_handle_open_action" type="handle_open_action" name="handle_open_server" output="screen">
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
-        <param name="move_arm_server" value="move_arm_server" />
-        <param name="move_base_server" value="move_base_server" />
-        <param name="move_forward_server" value="move_forward_server" />
-        <param name="detect_handle_server" value="detect_handle_server" />
-        <param name="force_sensor_topic" value="/wrist_force/raw"/>
-        <param name="pregrasp_config_name" value="pregrasp_low" />
-        <param name="final_config_name" value="pregrasp" />
-        <param name="handle_open_dmp" value="$(find mdr_handle_open_action)/config/trajectory_weights/weights_grasp_handle.yaml" />
-        <param name="dmp_tau" value="30" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="move_forward_server" value="$(arg move_forward_server)" />
+        <param name="detect_handle_server" value="$(arg detect_handle_server)" />
+        <param name="force_sensor_topic" value="$(arg force_sensor_topic)"/>
+        <param name="pregrasp_config_name" value="$(arg pregrasp_config_name)" />
+        <param name="final_config_name" value="$(arg final_config_name)" />
+        <param name="handle_open_dmp" value="$(arg handle_open_dmp)" />
+        <param name="dmp_tau" value="$(arg dmp_tau)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
index d3873a2f7..ba5cc4d98 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
@@ -1,13 +1,22 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="arm_name" default="arm" />
+    <arg name="palm_link_name" default="/palm_link" />
+    <arg name="cartesian_velocity_topic" default="/arm_1/arm_controller/cartesian_velocity_command" />
+    <arg name="base_vel_topic" default="/cmd_vel" />
+    <arg name="arm_controller_sigma_values_topic" default="/arm_1/arm_controller/sigma_values" />
+    <arg name="path_topic" default="/dmp_executor/path" />
+    <arg name="move_base_server" default="move_base" />
+    <arg name="dmp_param_file" default="$(find mdr_move_arm_action)/config/dmp.yaml" />
+
     <node pkg="mdr_move_arm_action" type="move_arm_action" name="move_arm_server" output="screen">
-        <param name="arm_name" value="arm" />
-        <param name="palm_link_name" value="/palm_link" />
-        <param name="cartesian_velocity_topic" value="/arm_1/arm_controller/cartesian_velocity_command" />
-        <param name="base_vel_topic" value="/cmd_vel" />
-        <param name="arm_controller_sigma_values_topic" value="/arm_1/arm_controller/sigma_values" />
-        <param name="path_topic" value="/dmp_executor/path" />
-        <param name="move_base_server" value="move_base" />
-        <rosparam command="load" file="$(find mdr_move_arm_action)/config/dmp.yaml" />
+        <param name="arm_name" value="$(arg arm_name)" />
+        <param name="palm_link_name" value="$(arg palm_link_name)" />
+        <param name="cartesian_velocity_topic" value="$(arg cartesian_velocity_topic)" />
+        <param name="base_vel_topic" value="$(arg base_vel_topic)" />
+        <param name="arm_controller_sigma_values_topic" value="$(arg arm_controller_sigma_values_topic)" />
+        <param name="path_topic" value="$(arg path_topic)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <rosparam command="load" file="$(arg dmp_param_file)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_action.launch
index 16cec7af7..3107b2680 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_action.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_action.launch
@@ -1,16 +1,28 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="move_base_server" />
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="pregrasp_config_name" default="pregrasp" />
+    <arg name="intermediate_grasp_offset" default="0.1" />
+    <arg name="safe_arm_joint_config" default="folded" />
+    <arg name="base_elbow_offset" default="-1." />
+    <arg name="grasping_dmp" default="$(find mdr_pickup_action)/config/trajectory_weights/weights_table_grasp.yaml" />
+    <arg name="dmp_tau" default="30" />
+    <arg name="number_of_retries" default="0" />
+    <arg name="grasping_orientation" default="[0, 0, 0, 1]" />
+
     <node pkg="mdr_pickup_action" type="pickup_action" name="pickup_server" output="screen">
-        <param name="move_arm_server" value="move_arm_server" />
-        <param name="move_base_server" value="move_base_server" />
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
-        <param name="pregrasp_config_name" value="pregrasp" />
-        <param name="intermediate_grasp_offset" value="0.1" />
-        <param name="safe_arm_joint_config" value="folded" />
-        <param name="base_elbow_offset" value="-1." />
-        <param name="grasping_dmp" value="$(find mdr_pickup_action)/config/trajectory_weights/weights_table_grasp.yaml" />
-        <param name="dmp_tau" value="30" />
-        <rosparam param="grasping_orientation">[0, 0, 0, 1]</rosparam>
-        <param name="number_of_retries" value="0" />
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="pregrasp_config_name" value="$(arg pregrasp_config_name)" />
+        <param name="intermediate_grasp_offset" value="$(arg intermediate_grasp_offset)" />
+        <param name="safe_arm_joint_config" value="$(arg safe_arm_joint_config)" />
+        <param name="base_elbow_offset" value="$(arg base_elbow_offset)" />
+        <param name="grasping_dmp" value="$(arg grasping_dmp)" />
+        <param name="dmp_tau" value="$(arg dmp_tau)" />
+        <param name="number_of_retries" value="$(arg number_of_retries)" />
+        <rosparam param="grasping_orientation" subst_value="True">$(arg grasping_orientation)</rosparam>
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
index c9e0a1651..b27b08015 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch
@@ -1,14 +1,24 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="move_base_server" />
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="preplace_config_name" default="pregrasp" />
+    <arg name="safe_arm_joint_config" default="folded" />
+    <arg name="base_elbow_offset" default="-1." />
+    <arg name="placing_dmp" default="$(find mdr_place_action)/config/trajectory_weights/weights_table_place.yaml" />
+    <arg name="dmp_tau" default="30" />
+    <arg name="placing_orientation" default="[0, 0, 0, 1]" />
+
     <node pkg="mdr_place_action" type="place_action" name="place_server" output="screen">
-        <param name="move_arm_server" value="move_arm_server" />
-        <param name="move_base_server" value="move_base_server" />
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
-        <param name="preplace_config_name" value="pregrasp" />
-        <param name="safe_arm_joint_config" value="folded" />
-        <param name="base_elbow_offset" value="-1." />
-        <param name="placing_dmp" value="$(find mdr_place_action)/config/trajectory_weights/weights_table_place.yaml" />
-        <param name="dmp_tau" value="30" />
-        <rosparam param="placing_orientation">[0, 0, 0, 1]</rosparam>
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="preplace_config_name" value="$(arg preplace_config_name)" />
+        <param name="safe_arm_joint_config" value="$(arg safe_arm_joint_config)" />
+        <param name="base_elbow_offset" value="$(arg base_elbow_offset)" />
+        <param name="placing_dmp" value="$(arg placing_dmp)" />
+        <param name="dmp_tau" value="$(arg dmp_tau)" />
+        <rosparam param="placing_orientation" subst_value="True">$(arg placing_orientation)</rosparam>
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/launch/push_pull_object.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/launch/push_pull_object.launch
index d5200b21d..3c572d093 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/launch/push_pull_object.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/launch/push_pull_object.launch
@@ -1,11 +1,18 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="cmd_vel_topic" default="/cmd_vel" />
+    <arg name="movement_speed_ms" default="0.1" />
+    <arg name="safe_arm_joint_config" default="folded" />
+    <arg name="number_of_retries" default="0" />
+
     <node pkg="mdr_push_pull_object_action" type="push_pull_object_action" name="push_pull_object_server" output="screen">
-        <param name="gripper_controller_pkg_name" value="mdr_gripper_controller" />
-        <param name="move_arm_server" value="move_arm_server" />
-        <param name="cmd_vel_topic" value="/cmd_vel" />
-        <param name="movement_speed_ms" value="0.1" />
-        <param name="safe_arm_joint_config" value="folded" />
-        <param name="number_of_retries" value="0" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
+        <param name="movement_speed_ms" value="$(arg movement_speed_ms)" />
+        <param name="safe_arm_joint_config" value="$(arg safe_arm_joint_config)" />
+        <param name="number_of_retries" value="$(arg number_of_retries)" />
     </node>
 </launch>

From 6e2881059134ed50ecb48adfde6410cec2f0268f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 25 Oct 2020 14:01:05 +0100
Subject: [PATCH 124/264] [navigation_actions] Use launch file args in the
 action server launchers

Related to https://github.com/b-it-bots/mas_domestic_robotics/issues/241
---
 .../ros/launch/follow_person.launch           | 43 +++++++++++--------
 .../ros/launch/move_base_action.launch        | 13 ++++--
 .../ros/launch/move_forward.launch            |  4 +-
 .../ros/launch/turn_base_to_server.launch     | 11 +++--
 4 files changed, 43 insertions(+), 28 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/launch/follow_person.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/launch/follow_person.launch
index ff4a5d7d3..4606467d1 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/launch/follow_person.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/launch/follow_person.launch
@@ -1,23 +1,28 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="max_translational_velocity" default="0.5" />
+    <arg name="max_rotational_velocity" default="1.0" />
+    <arg name="translational_deacceleration_factor" default="1.0" />
+    <arg name="rotational_deacceleration_factor" default="1.0" />
+    <arg name="minimum_distance_to_person" default="1.15" />	<!-- in meters -->
+    <arg name="allowed_angular_variance" default="5.0" />   <!-- in degrees !-->
+    <arg name="driving_backward_enabled" default="true" />   <!-- bool !-->
+    <arg name="distance_when_driving_backward" default="1.0" />	<!-- in meters -->
+    <arg name="people_positions_topic" default="/mcr_perception/waist_tracking/people_positions" />
+    <arg name="odom_topic" default="/base_controller/odometry" />
+    <arg name="cmd_vel_topic" default="/base_controller/command_direct" />
 
-  <node name="follow_person" pkg="mcr_follow_person" type="follow_person_node" ns="mcr_behaviors" output="screen">		
-    <param name="max_translational_velocity" value="0.5" />	
-    <param name="max_rotational_velocity" value="1.0" />
-		
-    <param name="translational_deacceleration_factor" value="1.0" />
-    <param name="rotational_deacceleration_factor" value="1.0" />
-		
-    <param name="minimum_distance_to_person" value="1.15" />	<!-- in meter -->
-    <param name="allowed_angular_variance" value="5.0" />   <!-- in degree !-->
-		
-    <param name="driving_backward_enabled" value="true" />   <!-- bool !-->
-    <param name="distance_when_driving_backward" value="1.0" />	<!-- in meter -->
-	
-    <remap from="~people_positions" to="/mcr_perception/waist_tracking/people_positions" />
-    <remap from="~odometry" to="/base_controller/odometry" />
-    <remap from="~cmd_vel" to="/base_controller/command_direct" />
-
-  </node>
-
+    <node name="follow_person" pkg="mcr_follow_person" type="follow_person_node" ns="mcr_behaviors" output="screen">
+        <param name="max_translational_velocity" value="$(arg max_translational_velocity)" />
+        <param name="max_rotational_velocity" value="$(arg max_rotational_velocity)" />
+        <param name="translational_deacceleration_factor" value="$(arg translational_deacceleration_factor)" />
+        <param name="rotational_deacceleration_factor" value="$(arg rotational_deacceleration_factor)" />
+        <param name="minimum_distance_to_person" value="$(arg minimum_distance_to_person)" />
+        <param name="allowed_angular_variance" value="$(arg allowed_angular_variance)" />
+        <param name="driving_backward_enabled" value="$(arg driving_backward_enabled)" />
+        <param name="distance_when_driving_backward" value="$(arg distance_when_driving_backward)" />
+        <remap from="~people_positions" to="$(arg people_positions_topic)" />
+        <remap from="~odometry" to="$(arg odom_topic)" />
+        <remap from="~cmd_vel" to="$(arg cmd_vel_topic)" />
+    </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
index d0630f1da..889b902ea 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
@@ -1,11 +1,16 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="/move_base" />
+    <arg name="pose_frame" default="map" />
+    <arg name="safe_arm_joint_config" default="folded" />
+
     <node pkg="mdr_move_base_action" type="move_base_action" name="move_base_server" output="screen">
-        <param name="move_base_server" value="/move_base" />
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="pose_frame" value="$(arg pose_frame)" />
+        <param name="safe_arm_joint_config" value="$(arg safe_arm_joint_config)" />
         <param name="pose_description_file" value="$(find mdr_environments)/$(arg robot_env)/navigation_goals.yaml" />
-        <param name="pose_frame" value="map" />
-        <param name="safe_arm_joint_config" value="folded" />
-        <param name="move_arm_server" value="move_arm_server" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/launch/move_forward.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/launch/move_forward.launch
index 15baf34cb..688e8597c 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/launch/move_forward.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/launch/move_forward.launch
@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="cmd_vel_topic" default="/cmd_vel" />
+
     <node pkg="mdr_move_forward_action" type="move_forward_action" name="move_forward_server" output="screen">
-        <param name="velocity_topic" value="/cmd_vel" />
+        <param name="velocity_topic" value="$(arg cmd_vel_topic)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/launch/turn_base_to_server.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/launch/turn_base_to_server.launch
index 6e7ccb2f3..f56ad1809 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/launch/turn_base_to_server.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/launch/turn_base_to_server.launch
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <node pkg="mdr_turn_base_to_action" type="turn_base_to_action" name="turn_base_to_action" output="screen" ns="mdr_actions" >
-    <param name="move_base_server" value="/move_base_server" />
-    <param name="rotation_frame" value="base_link" />
-  </node>
+    <arg name="move_base_server" default="/move_base_server" />
+    <arg name="rotation_frame" default="base_link" />
+
+    <node pkg="mdr_turn_base_to_action" type="turn_base_to_action" name="turn_base_to_action" output="screen" ns="mdr_actions" >
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="rotation_frame" value="$(arg rotation_frame)" />
+    </node>
 </launch>

From fa3b59f74250f679f0e3ed2bd1e326cbe23fec38 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 25 Oct 2020 16:09:25 +0100
Subject: [PATCH 125/264] [perception_actions] Use launch file args in the
 action server launchers

Related to https://github.com/b-it-bots/mas_domestic_robotics/issues/241
---
 .../ros/launch/detect_person.launch                 |  7 +++++--
 .../ros/launch/find_object.launch                   | 13 +++++++++----
 .../ros/launch/gender_recognition.launch            |  8 +++++---
 .../ros/launch/recognize_emotion.launch             |  7 +++++--
 4 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/launch/detect_person.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/launch/detect_person.launch
index c75d38a8d..fb5b5304f 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/launch/detect_person.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/launch/detect_person.launch
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="config_file" default="$(find mdr_detect_person)/common/config/haarcascade_frontalface_default.xml"/>
+    <arg name="image_topic" default="/cam3d/rgb/image_raw"/>
+
     <node pkg="mdr_detect_person" type="detect_person_action" name="detect_person_server" output="screen" ns="mdr_actions" >
-        <param name="config_file" value="$(find mdr_detect_person)/common/config/haarcascade_frontalface_default.xml"/>
-        <param name="image_topic" value="/cam3d/rgb/image_raw"/>
+        <param name="config_file" value="$(arg config_file)"/>
+        <param name="image_topic" value="$(arg image_topic)"/>
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch
index fd8ed0c7e..9a02224b8 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch
@@ -1,9 +1,14 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="ontology_url" default="file://$(find mas_knowledge_base)/common/ontology/apartment.owl" />
+    <arg name="ontology_class_prefix" default="apartment" />
+    <arg name="retry_count_on_failure" default="0" />
+    <arg name="timeout_s" default="120" />
+
     <node pkg="mdr_find_object_action" type="find_object_action" name="find_object_server" output="screen">
-        <param name="ontology_url" value="file://$(find mas_knowledge_base)/common/ontology/apartment.owl" />
-        <param name="ontology_class_prefix" value="apartment" />
-        <param name="retry_count_on_failure" value="0" />
-        <param name="timeout_s" value="120" />
+        <param name="ontology_url" value="$(arg ontology_url)" />
+        <param name="ontology_class_prefix" value="$(arg ontology_class_prefix)" />
+        <param name="retry_count_on_failure" value="$(arg retry_count_on_failure)" />
+        <param name="timeout_s" value="$(arg timeout_s)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/launch/gender_recognition.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/launch/gender_recognition.launch
index b0ca893d9..0625889eb 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/launch/gender_recognition.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/launch/gender_recognition.launch
@@ -1,8 +1,10 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="gender_model_path" default="$(find mdr_gender_recognition)/common/config/simple_CNN.81-0.96.hdf5" />
+    <arg name="image_topic" default="/cam3d/rgb/image_raw"/>
+
     <node pkg="mdr_gender_recognition" type="gender_recognition_action" name="gender_recognition_server" output="screen" ns="mdr_actions">
-        <param name="gender_model_path" value="$(find mdr_gender_recognition)/common/config/simple_CNN.81-0.96.hdf5" />
-        <param name="image_topic" value="/cam3d/rgb/image_raw"/>
+        <param name="gender_model_path" value="$(arg gender_model_path)" />
+        <param name="image_topic" value="$(arg image_topic)"/>
     </node>
 </launch>
-
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/launch/recognize_emotion.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/launch/recognize_emotion.launch
index 49354aeb4..6b57d86ba 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/launch/recognize_emotion.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/launch/recognize_emotion.launch
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="emotion_model_path" default="$(find mdr_recognize_emotion_action)/common/config/emotion_classifier.hdf5" />
+    <arg name="image_topic" default="/cam3d/rgb/image_raw"/>
+
     <node pkg="mdr_recognize_emotion_action" type="recognize_emotion_action" name="recognize_emotion_server" output="screen" ns="mdr_actions">
-        <param name="emotion_model_path" value="$(find mdr_recognize_emotion_action)/common/config/emotion_classifier.hdf5" />
-        <param name="image_topic" value="/cam3d/rgb/image_raw"/>
+        <param name="emotion_model_path" value="$(arg emotion_model_path)" />
+        <param name="image_topic" value="$(arg image_topic)"/>
     </node>
 </launch>

From dd5840559efc6efb409d72e47fdcbb84e72e027f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 25 Oct 2020 16:10:09 +0100
Subject: [PATCH 126/264] [listen_action] Use launch file args in the action
 server launcher

Related to https://github.com/b-it-bots/mas_domestic_robotics/issues/241
---
 .../mdr_listen_action/ros/launch/listen.launch             | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/launch/listen.launch b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/launch/listen.launch
index 2db7be591..0821d0237 100644
--- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/launch/listen.launch
+++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/launch/listen.launch
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="model_directory" type="str" default="$(find kaldi_speech_models)/common/config/en-US" />
+    <arg name="use_kaldi" default="true"/>
+
     <node pkg="mdr_listen_action" type="listen" name="listen_server" output="screen" >
-        <param name="model_directory" type="str" value="$(find kaldi_speech_models)/common/config/en-US" />
-        <param name="use_kaldi"       value="true"/>
+        <param name="model_directory" type="str" value="$(arg model_directory)" />
+        <param name="use_kaldi" value="$(arg use_kaldi)"/>
     </node>
 </launch>

From 75cb803b8bb3b005f450e29f2b3d3c8527d07554 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 25 Oct 2020 16:11:03 +0100
Subject: [PATCH 127/264] [actions] Use launch file args in the action client
 launchers

Related to https://github.com/b-it-bots/mas_domestic_robotics/issues/241
---
 .../ros/launch/pickup_client.launch                | 12 ++++++++----
 .../ros/launch/place_client.launch                 | 12 ++++++++----
 .../ros/launch/throw_client.launch                 | 12 ++++++++----
 .../ros/launch/move_base_client.launch             |  9 ++++++---
 .../ros/launch/find_object_client.launch           |  9 ++++++---
 .../ros/launch/find_people_client.launch           |  9 ++++++---
 .../ros/launch/perceive_plane_client.launch        | 14 ++++++++------
 7 files changed, 50 insertions(+), 27 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch
index b00d629a6..50cf63c34 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch
@@ -1,14 +1,18 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="pickup" />
+    <arg name="server_name" default="/pickup_server" />
+    <arg name="action_timeout" default="120" />
+    <arg name="grasping_pose_frame" default="base_link" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
 
     <node pkg="mdr_pickup_action" type="pickup_client" name="pickup_client" output="screen">
-        <param name="action_name" type="str" value="pickup" />
-        <param name="server_name" type="str" value="/pickup_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
+        <param name="grasping_pose_frame" type="str" value="$(arg grasping_pose_frame)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
-        <param name="grasping_pose_frame" type="str" value="base_link" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch
index 610a4555c..e21db55e1 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch
@@ -1,14 +1,18 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="place" />
+    <arg name="server_name" default="/place_server" />
+    <arg name="action_timeout" default="120" />
+    <arg name="placing_pose_frame" default="base_link" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
 
     <node pkg="mdr_place_action" type="place_client" name="place_client" output="screen">
-        <param name="action_name" type="str" value="place" />
-        <param name="server_name" type="str" value="/place_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
+        <param name="placing_pose_frame" type="str" value="$(arg placing_pose_frame)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
-        <param name="placing_pose_frame" type="str" value="base_link" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/throw_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/throw_client.launch
index d05cb16f3..c3d9723f1 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/throw_client.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/throw_client.launch
@@ -4,15 +4,19 @@
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
     <arg name="knowledge_update_service" default="/rosplan_knowledge_base/update"/>
     <arg name="attribute_fetching_service" default="/rosplan_knowledge_base/state/propositions"/>
+    <arg name="action_name" default="throw" />
+    <arg name="server_name" default="/place_server" />
+    <arg name="action_timeout" default="120" />
+    <arg name="placing_pose_frame" default="base_link" />
 
     <node pkg="mdr_place_action" type="throw_client" name="throw_client" output="screen">
-        <param name="action_name" type="str" value="throw" />
-        <param name="server_name" type="str" value="/place_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
+        <param name="placing_pose_frame" type="str" value="$(arg placing_pose_frame)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
         <remap from="knowledge_update_service" to="$(arg knowledge_update_service)" />
         <remap from="attribute_fetching_service" to="$(arg attribute_fetching_service)" />
-        <param name="placing_pose_frame" type="str" value="base_link" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch
index 10a356294..9fbb6d2a0 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch
@@ -1,12 +1,15 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="move_base" />
+    <arg name="server_name" default="move_base_server" />
+    <arg name="action_timeout" default="120" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
 
     <node pkg="mdr_move_base_action" type="move_base_client" name="mdr_move_base_client" output="screen">
-        <param name="action_name" type="str" value="move_base" />
-        <param name="server_name" type="str" value="move_base_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
     </node>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch
index b70002cb5..c3470d3ec 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch
@@ -1,12 +1,15 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="find_object" />
+    <arg name="server_name" default="find_object_server" />
+    <arg name="action_timeout" default="120" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
 
     <node pkg="mdr_find_object_action" type="find_object_client" name="find_object_client" output="screen">
-        <param name="action_name" type="str" value="find_object" />
-        <param name="server_name" type="str" value="find_object_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
     </node>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
index 18a732d63..95a3d0c33 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
@@ -1,12 +1,15 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="find_people" />
+    <arg name="server_name" default="find_people_server" />
+    <arg name="action_timeout" default="120" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
 
     <node pkg="mdr_find_people" type="find_people_client" name="find_people_client" output="screen">
-        <param name="action_name" type="str" value="find_people" />
-        <param name="server_name" type="str" value="find_people_server" />
-        <param name="action_timeout" type="double" value="120" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
     </node>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch
index f8ef50e31..960eb6d2d 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch
@@ -1,15 +1,17 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="action_name" default="perceive_plane" />
+    <arg name="server_name" default="/mdr_actions/perceive_plane_server" />
+    <arg name="action_timeout" default="120" />
+    <arg name="clear_plane_memory" default="true" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
-    <arg name="knowledge_update_service" default="/rosplan_knowledge_base/update"/>
-    <arg name="attribute_fetching_service" default="/rosplan_knowledge_base/state/propositions"/>
 
     <node pkg="mdr_perceive_plane_action" type="perceive_plane_client" name="perceive_plane_client" output="screen" ns="mdr_actions">
-        <param name="action_name" type="str" value="perceive_plane" />
-        <param name="server_name" type="str" value="/mdr_actions/perceive_plane_server" />
-        <param name="action_timeout" type="double" value="120" />
-        <param name="clear_plane_memory" type="bool" value="true" />
+        <param name="action_name" type="str" value="$(arg action_name)" />
+        <param name="server_name" type="str" value="$(arg server_name)" />
+        <param name="action_timeout" type="double" value="$(arg action_timeout)" />
+        <param name="clear_plane_memory" type="bool" value="$(arg clear_plane_memory)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
         <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
     </node>

From 6624b166b2924f2c73e9fc79a169ee9b7efe1945 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 1 Nov 2020 15:20:02 +0100
Subject: [PATCH 128/264] [place_action] Added a new goal field to the action
 definition

The field is a Boolean "release_on_impact", which specifies whether the
object should be released by detecting impact with the surface or by
simply releasing the object (the latter is used for throwing an object)
---
 .../mdr_place_action/ros/action/Place.action                  | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action
index d6001c17a..f112cf64d 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action
@@ -1,5 +1,7 @@
 # goal
-geometry_msgs/PoseStamped pose
+geometry_msgs/PoseStamped pose        # pose at which the object should be placed
+bool release_on_impact                # whether the object should be released only if
+                                      # impact with a placing surface is detected
 ---
 # result
 bool success

From d09fb49fce4fbb3726664623e9d975c7f83e69b9 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 1 Nov 2020 15:23:13 +0100
Subject: [PATCH 129/264] [place_action/action_states] The release_on_impact
 goal field is now used by the action

---
 .../ros/src/mdr_place_action/action_states.py     | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
index 0227ad30a..33d058960 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
@@ -101,13 +101,14 @@ def running(self):
         rospy.loginfo('[place] Arm motion successful')
 
         # the arm is moved down until it makes an impact with the placing surface
-        rospy.loginfo('[place] Moving arm down until surface impact is detected...')
-        self.gripper.init_impact_detection_z()
-        while not self.gripper.detect_impact_z():
-            self.gripper.move_down()
-            rospy.sleep(0.1)
-        self.gripper.stop_arm()
-        rospy.loginfo('[place] Impact detected')
+        if self.goal.release_on_impact:
+            rospy.loginfo('[place] Moving arm down until surface impact is detected...')
+            self.gripper.init_impact_detection_z()
+            while not self.gripper.detect_impact_z():
+                self.gripper.move_down()
+                rospy.sleep(0.1)
+            self.gripper.stop_arm()
+            rospy.loginfo('[place] Impact detected')
 
         # the object can be released now that impact with the surface has been made
         rospy.loginfo('[place] Opening the gripper...')

From 8e348726f67e3c80cf65b966c4caa73ec9e34858 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 1 Nov 2020 15:59:24 +0100
Subject: [PATCH 130/264] [place_action/clients] Setting 'release_on_impact' in
 the place and throw clients

The field is set to true and false respectively (we want to detect
surface impact while placing, but we don't want to wait for impact while
throwing an object)
---
 .../mdr_place_action/ros/scripts/place_client                 | 4 ++++
 .../mdr_place_action/ros/scripts/throw_client                 | 4 ++++
 2 files changed, 8 insertions(+)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client
index d62bfd2ed..bc317ad72 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client
@@ -75,6 +75,10 @@ class PlaceClient(ActionClientBase):
                 self.robot_name = param.value
 
         goal.pose = self.get_placing_pose()
+
+        # we want to place the object by detecting impact with the placing surface
+        goal.release_on_impact = True
+
         return goal
 
     def get_placing_pose(self):
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client
index 21ea56e9c..58d5d6ccf 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client
@@ -72,6 +72,10 @@ class ThrowClient(ActionClientBase):
                 self.robot_name = param.value
 
         goal.pose = self.get_throwing_pose()
+
+        # we don't want to wait for impact when throwing the object
+        goal.release_on_impact = False
+
         return goal
 
     def get_throwing_pose(self):

From e182ea5ef389e87dd539d6d97c38616f686086ad Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 4 Sep 2020 17:15:47 +0200
Subject: [PATCH 131/264] Remove redundant state in
 mdr_demo_simple_pick_and_place and cleanup

---
 .../config/pick_and_place_sm.yaml             | 27 +++----------------
 1 file changed, 3 insertions(+), 24 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml
index ae84c2a3a..7f9426f8c 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml
@@ -2,8 +2,8 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_simple_pick_and_place
-states: [GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, FIND_OBJECTS_BEFORE_PICKING, PLACE_OBJECT]
-outcomes: [DONE, FAILED, TIMEOUT]
+states: [GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, PLACE_OBJECT]
+outcomes: [FAILED]
 state_descriptions:
     - state:
         name: GO_TO_TABLE
@@ -63,7 +63,7 @@ state_descriptions:
                 state: FAILED
             - transition:
                 name: find_objects_before_picking
-                state: FIND_OBJECTS_BEFORE_PICKING
+                state: PERCEIVE_TABLE
         arguments:
             - argument:
                 name: number_of_retries
@@ -71,27 +71,6 @@ state_descriptions:
             - argument:
                 name: picking_surface_prefix
                 value: table
-    - state:
-        name: FIND_OBJECTS_BEFORE_PICKING
-        state_module_name: mdr_perception_behaviours.perceive_planes
-        state_class_name: PerceivePlanes
-        transitions:
-            - transition:
-                name: succeeded
-                state: PICK_OBJECT
-            - transition:
-                name: failed
-                state: FIND_OBJECTS_BEFORE_PICKING
-            - transition:
-                name: failed_after_retrying
-                state: FAILED
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-            - argument:
-                name: plane_prefix
-                value: table
     - state:
         name: PLACE_OBJECT
         state_module_name: mdr_manipulation_behaviours.place

From 071113b32c4131252530973447f782036cadf276 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 4 Sep 2020 17:20:18 +0200
Subject: [PATCH 132/264] Remove unused outcomes in demo scenarios

---
 .../mdr_demo_describe_people/config/describe_people_sm.yaml     | 2 +-
 .../mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml   | 2 +-
 .../mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml             | 2 +-
 .../config/throw_table_objects_sm.yaml                          | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_describe_people/config/describe_people_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_describe_people/config/describe_people_sm.yaml
index b2a4c7863..97bda609c 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_describe_people/config/describe_people_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_describe_people/config/describe_people_sm.yaml
@@ -3,7 +3,7 @@
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_describe_people
 states: [WAIT_FOR_NAME, GIVE_POSITIONING_INSTRUCTIONS, DESCRIBE_PEOPLE, THANK_PEOPLE]
-outcomes: [DONE, TIMEOUT, FAILED]
+outcomes: []
 state_descriptions:
     - state:
         name: WAIT_FOR_NAME
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
index 4b6fad0bf..3d07856d8 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/config/lab_manager_sm.yaml
@@ -3,7 +3,7 @@
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_lab_manager
 states: [GO_TO_MONITORING_POSE, MONITOR_DOOR, VERIFY_PERSON, ASK_NAME, GIVE_DISINFECTION_FOLLOWING_INSTRUCTIONS, TURN_PERSON_AWAY, GO_TO_DISINFECTION_SPOT, PERFORM_DISINFECTION, GIVE_ASSIGNED_SPOT_FOLLOWING_INSTRUCTIONS, GUIDE_TO_EMPTY_SPOT, GIVE_LAB_LEAVE_INSTRUCTIONS]
-outcomes: [DONE, FAILED]
+outcomes: [FAILED]
 state_descriptions:
     - state:
         name: GO_TO_MONITORING_POSE
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml
index 82d572058..9996a8d49 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml
@@ -3,7 +3,7 @@
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_patrol
 states: [GO_TO]
-outcomes: [DONE, TIMEOUT, FAILED]
+outcomes: [FAILED]
 state_descriptions:
     - state:
         name: GO_TO
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml
index 685a1ee67..78a55d0ed 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml
@@ -3,7 +3,7 @@
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_demo_throw_table_objects
 states: [GO_TO_OBSERVATION_POSE, PERCEIVE_TABLE, GO_TO_TABLE, PERCEIVE_TABLE_BEFORE_PICKING, PICK_OBJECT, GO_TO_TRASH_CAN, THROW_OBJECT]
-outcomes: [DONE, TIMEOUT, FAILED]
+outcomes: [FAILED]
 state_descriptions:
     - state:
         name: GO_TO_OBSERVATION_POSE

From 65df9cd8783f81bdd9b9e5120639400364ce259d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 14 Nov 2020 15:11:17 +0100
Subject: [PATCH 133/264] [manipulation_actions] Added an action for receiving
 an object from a person/another agent

---
 .../mdr_receive_object_action/CMakeLists.txt  |   47 +
 .../mdr_receive_object_action/README.md       |   99 +
 .../learned_position_policy_parameters.pkl    |  Bin 0 -> 6050 bytes
 .../trajectory_weights/receive_object.yaml    | 1667 +++++++++++++++++
 .../mdr_receive_object_action/package.xml     |   31 +
 .../ros/action/ReceiveObject.action           |   12 +
 .../ros/launch/receive_object.launch          |   26 +
 .../ros/scripts/receive_object_action         |   62 +
 .../scripts/receive_object_action_client_test |   35 +
 .../src/mdr_receive_object_action/__init__.py |    0
 .../action_states.py                          |  234 +++
 .../mdr_receive_object_action/setup.py        |   11 +
 12 files changed, 2224 insertions(+)
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/README.md
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/learned_policy_parameters/learned_position_policy_parameters.pkl
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/trajectory_weights/receive_object.yaml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/package.xml
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/action/ReceiveObject.action
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/launch/receive_object.launch
 create mode 100755 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action
 create mode 100755 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/__init__.py
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
 create mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/setup.py

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/CMakeLists.txt
new file mode 100644
index 000000000..9c6d16c55
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_receive_object_action)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roslint
+  actionlib
+  actionlib_msgs
+  genmsg
+  message_generation
+  geometry_msgs
+  mas_execution_manager
+)
+
+roslint_python()
+catkin_python_setup()
+
+add_action_files(DIRECTORY ros/action
+  FILES
+  ReceiveObject.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+   rospy
+   actionlib
+   actionlib_msgs
+   message_runtime
+   geometry_msgs
+   mas_execution_manager
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+install(PROGRAMS
+  ros/scripts/receive_object_action
+  ros/scripts/receive_object_action_client_test
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
+)
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/README.md
new file mode 100644
index 000000000..d49f42cb1
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/README.md
@@ -0,0 +1,99 @@
+# mdr_receive_object_action
+
+An action for receiving an object from a person. The action is intended to be adaptive to different contexts of the task, such as the receiver's posture, receiver attributes, etc. The action is essentially equivalent to `mdr_hand_over_action` (the implementation of that action is reused almost completely), except that here the robot is waiting for an object to be placed in its hand.
+
+Just as the hand-over action, the object reception action is currently only adaptive to postural context, through an object reception position selection policy acquired using reinforcement learning.
+
+The context-aware object reception demo scenario demonstrates the intended workflow of the action, including:
+* Detecting a person
+* Determining their posture
+* Receiving an object
+
+## Action definition
+
+### Goal:
+
+* ``string posture_type``: The receiver's posture: standing up, seated, or lying down
+* ``bool obstacle``: Indicates whether an obstacle is present between the robot and the receiver
+* ``bool reception_detection``: Determines whether a force sensor-based object reception detection strategy is used
+* ``bool context_aware``: Determines whether knowledge about context is utilized to adapt execution
+
+### Result:
+
+* ``bool success``
+
+### Feedback:
+
+* ``string current_state``
+* ``string message``
+
+## Directory structure
+
+```
+mdr_receive_object_action
+|    CMakeLists.txt
+|    package.xml
+|    setup.py
+|    README.md
+|____config
+|    |____learned_policy_parameters
+|    |    |_____learned_position_policy_parameters.pkl
+|    |    |
+|    |____trajectory_weights
+|         |_____receive_object.yaml
+|____ros
+     |____action
+     |    |_____ReceiveObject.action
+     |    |
+     |____launch
+     |    |_____receive_object.launch
+     |    |
+     |____scripts
+     |    |     receive_object_action
+     |    |_____receive_object_client_test
+     |    |
+     |____src
+          |____mdr_receive_object_action
+               |    __init__.py
+               |____action_states.py
+```
+
+## Launch file parameters
+
+### Action server
+
+The following parameters may be passed when launching the action server:
+* ``move_arm_server``: Name of the `move_arm` action server (default: 'move_arm_server')
+* ``gripper_controller_pkg_name``: Name of a package that implements functionalities for controlling a robot's gripper (default: 'mdr_gripper_controller')
+* ``init_config_name``: Name of the initial arm configuration for object reception (default: 'pregrasp')
+* ``receive_object_dmp``:  Path to a YAML file containing the weights of a dynamic motion primitive used for object reception (default: '')
+* ``receive_object_position_policy_parameters_file``:  Name of a YAML file containing the parameters of a learned context-dependent reception position policy (default: 'learned_position_policy_parameters.pkl')
+* ``receive_object_policy_config_dir``: Path to a directory containing learned reception position policy parameters (default: '')
+* ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 30)
+
+## Action execution summary
+
+It is assumed that the robot starts without an object in its gripper and that the gripper is open. The action proceeds through the following steps:
+1. The end-effector position at which the object will be received is first determined:
+  - if the ``context_aware`` goal parameter is set to ``True``, a position is sampled from learned policy, given the value of ``posture_type``,
+  - otherwise, a pre-set, context-independent position is used.
+2. The arm is moved to the ``pregrasp`` configuration.
+3. The arm is then moved such that the end-effector ends in the position chosen in step 1 (currently the trajectory to be followed is set to a learned DMP trajectory).
+4. If the ``reception_detection`` action parameter is set to ``True``, the robot utilizes the wrist force sensor to determine when the person has put the object in the gripper with the intention of releasing it, using a signal change detection algorithm, and then grasps the object. Otherwise, the robot waits for a short time and simply closes the gripper.
+5. The arm is moved back to the ``pregrasp`` configuration.
+
+## Dependencies
+
+* ``pyftsm``
+* ``mas_execution_manager``
+* ``actionlib``
+* ``tf``
+* ``geometry_msgs``
+* ``mdr_move_arm_action``
+* ``mdr_move_base_action``
+
+## Example usage
+
+1. Run the ``move_arm`` action server: ``roslaunch mdr_move_arm_action move_arm.launch``
+2. Run the action server: ``roslaunch mdr_receive_object_action receive_object.launch``
+3. Run the client example: ``rosrun mdr_receive_object_action receive_object_action_client_test -p standing -r 1``, where `p` is a posture argument and `r` is 1 or 0 if the robot should detect when the object has been received
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/learned_policy_parameters/learned_position_policy_parameters.pkl b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/learned_policy_parameters/learned_position_policy_parameters.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..87ba1ee44febdace1eaa678f12b91a4ff6559954
GIT binary patch
literal 6050
zcmX|F2U`?buq6o!5+o{uiXftdRRmWtObZ5*gi%BZl7t0Op^;#Akerd6K_rcmBuHky
zug(v9@AUfKA|IaHeM8*}r>fduk@u?NW$D|Hyppo~ke3xN%8PQ!%5vV?W|igVmArag
zURIG;ZmXL8nPvOx*`K$nd7N3DYLS<fSMn@B@3n3ID-GDHmXFQN%*=9CtH;*-|3yxD
z(W{H<fqK<iui7-I2leWqfH-shv#eKb|8L7@<!?*5((bVZR}@@P5679snK!8RdetEy
z?$3VLRmX&cgxLS#&%b}-%*s`#+@?ay$EUa^7T?7be9#9YI?D(QV&KsyS=K$1MO&|c
z5d6@oM+zx{vtD&6S6y=p%?kgO-K~%Ux$9LAO(086preHj+Lo2vQU=k9Nh>U3LUwfM
z!)DxxO4Lh-E(z0EmL~jqBzt0duKiA77}XfinM<3DqlPc*WmNb4kbK92%qO8yOz1#k
z*2~NI1p^1On3hhPtFp-D7&wo`JE8c737vXO6Vy|$9xqqD{u9(&6ZC{$J^7!Yr~U}~
zoPwfF>aL!bg`5{Ubjr#nEMw=SjOn6acrG*8_mys4lD^<lDrMTxxuW@+p7J&lOP{B#
zyz!A)Vhnhr{bR7~6MwW!zzj@@GOm*aX~D4dUFnfQo$8|rcUrIdmaBfb4eA*hC$8`R
zwdt=3c~-9m{1LLRUOo3mNQ0}HnJrq*%R1_?m8FUGRraWWuZ7t#U`{5agZArk&w(KZ
zjj0~;1B1k<xhwl<!cIjl3?FUvSmL0K*h#jrScZM<aZt#|eJ9g&c`$fM{l&inWSUSm
z6aUGU4a;XRT)lJJ6W_duWLb|MOFh1E0=mvhhqOw&e9@-2V_xQ@Q=0B+?<S<aH2=6>
zgDXXj!S|(wFd;iZCd*N+O`pl8T|3Z@UXDtoHbv?~&`InZ9xqD?XwxPwCt*YeKVwWA
zY{|N`rARHdIKq+)9?aBf((%cfY+<Mhv)Z)L8<-11hZs2sK)V<-#85zdFc)iB7t?KR
zbwhF06*T2&^MA=S_GJ65m^iYr_mMX9DDH}%zUY^lb7<foC_60fdRRVZf5YBoPH&V(
zj?jNJ<qTEbLL4j&kFbg!>}oGe*COp;a4dkKf)6{Wm?ms#FGghk1-h+dpM$Wc|Hv%9
zln}bffGl6asxNV%bffhU-%Ru5XxS)|`P;HWTqb)K>0x3Z441LXx*IV>GMgNLVQF+C
z#wI1gpe$p8cX57z8th9CYPB2pVXVAZHek^1{L@yxNgE~86fQsAWG?|D_`!iijHio}
zrd0-lA<Pd(XAL*Jx#s9|Tsy7(Dg&<_ji)%s$PH%X{5nH;3>y(4vgbp1`F;f&ZfaYU
zp!JrMhiqzR%qL$zSR6rA=@9w!i44%~uv!%4DVtcPqtcz%rR#$j^R>ZZ{Bv}n&W_ky
zdIF@o6l3)B@((gVn+%9ab9$qf|0heSCA6?W%WH2N?cu(5)FX_)eM&G8jsdK`F3TY9
zhDpY6KS)mFq|R_!<g4^b^<x+c%*|!ub7m?G{?gCuOv<(k2Qqx)`BPbyUSg_u6us2o
znsjo77%>@dF54fTz;NxNjD7M%4Ix@Sp_zU^jcskxL}nVAtev&x)z~I8Rbxv3kS_-B
zIiV)uMlxUY$391+tt`jG@Bstz8?7?w7tJ|yB+f1|hE(05YH27F`Yo_`8;0Xnn9n*a
z-|jM)JfdyMmAf(YF@-TOhCT25Fj#%6%CpK&gy9Gpd2XXaC9!mRlwQK9FW+<U^Mtfw
z+mj2ijwNsU$WK||=Ge%<(r1k8Zov>zT=4PyYaM5Iy!OVJY+Y7p0DP4iY+ZJVq?tYN
z6BB8%Tz51z?eRlwQC1sSql^9c2L=i#4Y{<%Lv#{Hkga-UoLo4?QHA~IU@^!zq3;Xo
zy!sfLh!gQ6r(A~6Py8>H%vD&GX!s|pU&Km(Ay4pqa$I`yuo6Xam;aF;PdzIsj8!nh
zWd~hPD7kc9%a&;!zEI%5rP)0~dfpM94!&P_){)>Zcap`7bi3Z<7ImNBWlBR<1TP^Y
z*bVs<7|tDxVaKG?UKc`GFGCL;j$z4%_(!&rzwEgkB3wTsW4SWnZmDFGmpakFo2{Ny
z&~t}Y9Ey<XK&ALjl{cn?!W0Y?qPi>+jS<Re<=iW&lW9dk=NlPLw2+M`<(;3an`}z!
zUFB_1CBx3>AERK@2SywDrp76i&e9Q#_Q3m!tY89D)W%OtakR%+SY$>p_;9c*3;2ra
zQ0a+PFn-yG@jV%PSGg^HQvE=-WtT@9WyY@DH8WiC%(B3ae8o3k#jVOrMrA%4-*s3w
z=bm*Q86G4F>-Qly-=BuzJci3K^}x;#h8JY}$$#u%qApIy;-`F<CE53)>aH?LchkOY
z?hN{(vuMA2!3~Cc7`i9FFe|mksp$rc6<(lJV`r$oS{Xh18grK}^2KkNm(h2a!|YX#
z8hJruiav%$l{M5-n_nnYJ;TUVz9yu&$uvXF?hel$<lGf4w1=fje<)B!vnh6P5`%S4
zwsJKa<#}B^q>+<bg`ZBwpAe;rQFc-=pb?RJP~i^U=W<}U<(SE1W@RfA221REoC`dr
z@x<(%B1*p*sfF`8zK6;f`cHCDCO;Fs?69RhU$9R~#Fjm$OK$*XtEAc{9R_<JOocte
z79l2Id6alLb7y+|<|D_rM2(u}>95tVu>Xp=96zMJ@-y1(c!49BIUr-ltqGmH<u{Y}
zSTv`&Yb68=h;cJ6rm9<*?3)?<9lSqt+E|E~l3(z!koZ81%i>|}+-}*ZL|?M3a3EcL
zz)EcJ8s*4FBQ0q{a}alCLhFsju9)8QNNKqKE<=6?P$eJHVey#(J41|y_$-;bf0Rmn
z<%W&(^t)kA!OgD6b8lX!D*dIaj0MF$);W;DVpeF=S@=SQOYKQozLf+rme0|ye0`2S
zX_3|fZo7FEU;Gb91L-0{I&uOYRTaYUkaggWYkF`IDMrVz_<rI!AxuxmRn5<_%_&wP
zGH8W8baCO)P^<=*dU3lG#~dBcpXHhPcBNX{PtZhckin-{95BMmIin}<2t_e!D*r->
z_76l}BCb6B{%&X{!Aq&lW#QX-kdx7Wg`qzV<2w#s<T+rX5R=;To)7UAm<yv;0%!+T
zS+UZ{!AW#o(<&`{9(q?ytewbXyBxeeBqNp5T0yxSEU;E;T}Dyqg<a32CLhhb4JOT*
zBW2)zF^dQX=}g4}`d-2ibqQabW%FDq=kFiIa3a2^t6Hfv_{q`ZZ5bC1xsc%7BGuQN
zQFC2ts#y4x^LexMIeoB}4q`C|;72?*-VnP|u6A2&;j6Y_G(w-6OS{34V`m;f3Txv*
z@6c|2_f<YmPdd)f-77bz?ipZT-5CIb_|9A0Jm(c1n!+oe!Q?;mgl)8HPB{BMgUNXt
zyNpz6J<C+OMGY)I;H!aT*^tSTY7((l+wZ#vdB?#}oN(M5lTP<w@OxuU1=JUl-V<2U
zE;tqan;K;Dj_l{8e~#t4LF|X(*GDUDk3|g0k2sbA`+90DzRb$6fO*vo!v$!M@+4$y
zQGbB+wou3<d)K2-3~w;Pv}z**UN+388<#2LF5PvXo9s!qw<5+k{CFc9N28QCtP;C}
zoMkIXIT&3Wbq2;q%E1ps*r`Ngy7EbynaX#QeY2FeCGMW6f0M1a$hQMeSc5NSE12`Y
ze^^%2vXxI5NB;@*=(3b+Xt6n}cgs{hTO}2^97$6!^$z{TGKrrl3U<oEW#MB<lENfk
z!-9Aa)eoZ-Y)da;^JSY;%5|y6V5m;I{w6y`J4p>8bs`-F2a?W6CslliHQ&l>dN~>C
z#b<nz#VA=|*e7f8BSW4nMp)VhMVXN_3>y6`SxPq(#ss4Sh6h+U?uGgw!cR7rq|vT`
zvn?=I>V%FI+BSrt$AV!6vt+rqZ?NU6$F#<M9T>k&$1zzxVqsz~gD3Hgt${&WB4kV>
zVd;;7A)b|Q?MXKn$?pDI)0R)CnYL79J6U$!?^!S}_2wxQWj`)KAr;Oh2UUGYQDj2T
z#v$fonj&9y7g(z`$x5bj>u!lGsc3krF!l6HBPN+5hBFmuvk${kMoOBZ1k;1&iVLXO
zapt~Rp5AAksALC|lU4#TAE`W)W%eq&*zvodcwti-BUKpLi<q#JBkhhda^jrks3ZuM
z2eN$qlIBo}nq3Mi=r|>^cqLpq{IBq$+u2N`;~4Ub;GFN6WX764hOQf=D5l(AS82fB
zEzUFg#IgW2ma>+~yBxy3K3r3LQ<*SZSb5EdQ2dh^xP?s@Pwr#Hj-|BSbXSOb4x{cP
zy5G{=(O6~5Yk7f&1ah744btio8){DKVD21dOw3r^)_i<kCENdyHWo5h9Npcd%kMNr
z*rL|gWuHp>b&6~DvSltT@hR_?9xj_HFONpTz&4nj&3MXlTJ6}~E7*8_mGz>SY*Alv
z1+`l3xNri-(<iXRf)%yY*>@HrEAO>YCsus)XtC2~ZE+h`&W9r@Dck2s>2Z$DVioEv
zlQ(jiYZi!~yJUJ=%QtOCr!U$+<Y9<+xp*7xe(^En1U8dD)2`*GQ+?$A%nWp%qVj3e
zy-=+^!i+yxcJWfJ*JaJkg>-y7up;QKB|{`cT6D2eXUz$Qqv#2gmQQwcfBZhrv{3DR
z00x(e6R3MG_1w-x4&Ro!2Z=24w{=p-VuK}1A)8!<>9;`!o>nDbOD9v(b^^^0WRrQ-
zbV!CV^7oN5>?usKEMRu9_cy+qv%V;{JIjsQcxMyagu2917x{5e@w$GK*<>KlM`7Y0
zyidRFd;gb`h~C>Q_gbXyigL9?z6DWfbGH<BWg|@EU0A=Tuuz&s^Sc8WN>G9#14AC5
zNw3JTJ~5LO>>z!-DdP#Ru)(aMNR6yTko%iu?Xlv24o$Sprv8bdQrK#LDy{j6LYMA9
zL#3=TQm{immkAZ7GzQJG)C9|ltfwnvAydNP{FDqv8y{-|PG=}in4raf-A~O>?CIMz
zi62g-D`99Uc^`z?$4WZCb}W=!!9<D@mvaPzNzy_=C-~8TWp%$-qH>yj_OjGowu)n5
zqFM&6U8E^S@hpfdR?r-)NRK!3=Zth@{>uU_0hkF#U9y7VRCL~Fo%m3ZYN_{;$#=4u
zs(h9udVFJPe46r6Y9g?ZcRoYGP`C^qFY?V)EDuP3f!Wneg&oxd(=}G4H$!1KE=l!i
z)Rv_y5wRaIo{G&UN+DX0CCjhy$I3G{cs){G6!J(pe~hVtWQElSN{p;UA7@0ZCM(WW
zA7$}#cA~-ta_s`!?WO2=C7PD5=G2*#SmiOV+ag=BxiLy4=5D7^?fcP`0>-%W8X*!d
z-pgmykIVQy1#9Ib(HYj0ca+E%c{x14F;aPTmY)<xPRY=9<*%x&H}7OVRJj;i#LP2-
z=~GN6UmY<Qz(&j{rc;k)_vTw?Eu{OU!#lF1m_~i-ggS?hZ-V)7kJ;8IOV)#kXU#5>
z^;fZdmL)9e%2-o&yyVK_+blB0_|!rB+OWj1m+w|gT+ydJFi_-rNV!&sDSNtlgQF6{
z+GH+<DqXWwFq7mOdM1N`YH~S9o37o)46A-im-47o4EVv26>Fa@gZ3djdLjQI`YJIV
z%5O=$q=DIC3EgBUx?Hua-m=C5lbZ*Fq?sbGDOx8mpDVkW9vC9N_ctT4*;W>bOJGdJ
z(t8;t0lzl85+H*gNDOK6rRXor`;yRP{XslaV7CMBF9Qi4nKC22J}~6IWzjr^v2+VQ
zpFETvLn|1MC~4Sqvc}r8GfHuCyjB^b_b=ughIvm&iZorK!Hz|F%4lG?;*O?dhQXvv
zg()W-ZTw%_hiZ*i?-?0!kfA{3W5HS3k7AT(gcV6`>IGYcAvlL>*Ft9SHOA=|%-*`g
zL^^te_v<W@2^%a#9A!;~QJZ(Z=Es;`139S&qmk~UOFx+oJ0c^_Y`4f0J!I91VkdSh
z)ZS03YV<z0$)X<>q8FP~y;M6fMf9ReJpqHQ7kO<bEtKt+8FtTCIm<Sk;H(1^Y2956
z&=Qyryvmrdpd#5&Ooayop`WRXbxFrFY@YLHO2BW_L_W+CBTF;&y!PobP_G7+tHJ+$
zR}IlVS^lM0FZ}z-vQEAD=Yyp$zpB2MsT<Nt(J_6Kjo)5ayfj3k<)$V1Z&Nz12AtBV
zq1v98^lDhS8vfs&m$g0r)~i?kZ_lfL_VnYPr({Rw*&_Clrw4T8y3w@L_#MD6)tT4|
dOH8i9vQCZAHoc}-ua~Pga?915^{Ou9e*t1%^(O!T

literal 0
HcmV?d00001

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/trajectory_weights/receive_object.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/trajectory_weights/receive_object.yaml
new file mode 100644
index 000000000..2bcb6488b
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/config/trajectory_weights/receive_object.yaml
@@ -0,0 +1,1667 @@
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223,
+  -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096,
+  -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595,
+  -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776,
+  -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316,
+  -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115,
+  -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066,
+  -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736,
+  -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944,
+  -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777,
+  -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774,
+  -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614,
+  -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464,
+  -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514,
+  -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704,
+  -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118,
+  -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555,
+  -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456,
+  -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158,
+  -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113,
+  -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834,
+  -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257,
+  -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332,
+  -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204,
+  -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437,
+  -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826,
+  -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198,
+  -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525,
+  -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501,
+  -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623,
+  -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465,
+  -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925,
+  -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477,
+  -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792,
+  -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102,
+  -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663,
+  -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824,
+  -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146,
+  -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656,
+  -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234,
+  -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734,
+  -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934,
+  -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986,
+  -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363,
+  -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668,
+  -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839,
+  -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874,
+  -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192,
+  -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154,
+  -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287,
+  -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938,
+  -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163,
+  -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446,
+  -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945,
+  -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823,
+  -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033,
+  -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043,
+  -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923,
+  -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432,
+  -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889,
+  -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596,
+  -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364,
+  -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604,
+  -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642,
+  -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686,
+  -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615,
+  -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385,
+  -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576,
+  -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794,
+  -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084,
+  -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425,
+  40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273,
+  -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242,
+  -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348,
+  -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675,
+  -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794,
+  -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823,
+  -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525,
+  63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779,
+  -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285,
+  -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233,
+  -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494,
+  4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534,
+  5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616,
+  -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531,
+  -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582,
+  -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527,
+  -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938,
+  -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925,
+  13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073,
+  6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941,
+  -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595,
+  -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226,
+  -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772,
+  -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734,
+  29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656,
+  25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073,
+  -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377,
+  -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623,
+  -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374,
+  31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135,
+  71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668,
+  70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188,
+  -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115,
+  2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502,
+  101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024,
+  163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724,
+  80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944,
+  -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563,
+  -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664,
+  -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177,
+  -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046,
+  -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798,
+  -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077,
+  -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752,
+  -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202,
+  58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274,
+  48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283,
+  -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057,
+  -123.43864361815416, -140.9579908724867, -157.005848574077]
+y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111,
+  -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728,
+  -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983,
+  -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277,
+  -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074,
+  -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947,
+  -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737,
+  -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779,
+  -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293,
+  -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643,
+  -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577,
+  -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588,
+  -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851,
+  -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558,
+  -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179,
+  -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748,
+  -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684,
+  -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735,
+  -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668,
+  -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535,
+  -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122,
+  -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498,
+  -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707,
+  -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863,
+  -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154,
+  -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564,
+  -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214,
+  -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676,
+  -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744,
+  -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956,
+  -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839,
+  -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955,
+  -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735,
+  -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345,
+  -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231,
+  -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039,
+  -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063,
+  -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203,
+  -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401,
+  -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627,
+  -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747,
+  -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766,
+  -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141,
+  -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347,
+  -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216,
+  -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654,
+  -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015,
+  -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534,
+  -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731,
+  -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451,
+  -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993,
+  -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378,
+  -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472,
+  -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751,
+  -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873,
+  -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556,
+  -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702,
+  -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539,
+  -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257,
+  -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107,
+  -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192,
+  -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091,
+  -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265,
+  -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897,
+  -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183,
+  -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083,
+  -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788,
+  -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237,
+  -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944,
+  -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927,
+  -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068,
+  -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397,
+  -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006,
+  -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678,
+  -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265,
+  -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213,
+  -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764,
+  -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582,
+  67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551,
+  74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474,
+  35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262,
+  204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635,
+  246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412,
+  87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006,
+  66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695,
+  158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347,
+  150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332,
+  116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529,
+  42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666,
+  -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254,
+  -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793,
+  -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045,
+  -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597,
+  -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263,
+  -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724,
+  -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274,
+  -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294,
+  -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296,
+  -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972,
+  -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207,
+  -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173,
+  -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385,
+  -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214,
+  28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786,
+  46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807,
+  110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782,
+  173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245,
+  197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484,
+  210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167,
+  189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755,
+  62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095,
+  -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295,
+  -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374,
+  -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395,
+  -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038,
+  36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376,
+  65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127,
+  -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059,
+  -127.19340520577649, -154.63036780787843, -180.74870760607394]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567,
+  205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663,
+  -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808,
+  -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975,
+  -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551,
+  -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826,
+  -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272,
+  104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167,
+  82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434,
+  -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583,
+  -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846,
+  -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196,
+  -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552,
+  -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175,
+  -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248,
+  -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993,
+  -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733,
+  -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155,
+  -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093,
+  -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656,
+  -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873,
+  25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458,
+  40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028,
+  -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413,
+  -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641,
+  -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581,
+  22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803,
+  15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514,
+  -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898,
+  -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569,
+  -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616,
+  -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532,
+  -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893,
+  18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601,
+  22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818,
+  32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779,
+  48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617,
+  143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107,
+  130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158,
+  44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568,
+  115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683,
+  149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516,
+  80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459,
+  99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014,
+  172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139,
+  113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729,
+  96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513,
+  62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735,
+  -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649,
+  21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656,
+  65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635,
+  70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916,
+  74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807,
+  136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928,
+  168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346,
+  121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441,
+  96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392,
+  104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364,
+  114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289,
+  98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998,
+  49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645,
+  16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556,
+  50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409,
+  60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868,
+  51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244,
+  53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015,
+  64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777,
+  38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126,
+  -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391,
+  -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894,
+  -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211,
+  -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903,
+  -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216,
+  -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931,
+  -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333,
+  21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149,
+  42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823,
+  16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427,
+  -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764,
+  -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054,
+  -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404,
+  -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496,
+  -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292,
+  -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934,
+  -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776,
+  -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126,
+  -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246,
+  -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633,
+  -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934,
+  -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297,
+  15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244,
+  26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861,
+  15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945,
+  36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882,
+  48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032,
+  34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864,
+  10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116,
+  -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808,
+  -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259,
+  -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883,
+  -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713,
+  6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992,
+  15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006,
+  6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738,
+  -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728,
+  -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381,
+  -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837,
+  -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521,
+  -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423,
+  -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537,
+  -111.02550604149512]
+pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223,
+  -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096,
+  -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595,
+  -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776,
+  -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316,
+  -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115,
+  -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066,
+  -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736,
+  -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944,
+  -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777,
+  -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774,
+  -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614,
+  -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464,
+  -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514,
+  -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704,
+  -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118,
+  -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555,
+  -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456,
+  -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158,
+  -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113,
+  -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834,
+  -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257,
+  -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332,
+  -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204,
+  -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437,
+  -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826,
+  -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198,
+  -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525,
+  -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501,
+  -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623,
+  -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465,
+  -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925,
+  -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477,
+  -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792,
+  -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102,
+  -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663,
+  -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824,
+  -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146,
+  -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656,
+  -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234,
+  -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734,
+  -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934,
+  -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986,
+  -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363,
+  -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668,
+  -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839,
+  -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874,
+  -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192,
+  -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154,
+  -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287,
+  -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938,
+  -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163,
+  -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446,
+  -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945,
+  -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823,
+  -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033,
+  -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043,
+  -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923,
+  -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432,
+  -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889,
+  -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596,
+  -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364,
+  -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604,
+  -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642,
+  -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686,
+  -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615,
+  -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385,
+  -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576,
+  -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794,
+  -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084,
+  -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425,
+  40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273,
+  -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242,
+  -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348,
+  -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675,
+  -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794,
+  -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823,
+  -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525,
+  63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779,
+  -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285,
+  -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233,
+  -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494,
+  4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534,
+  5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616,
+  -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531,
+  -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582,
+  -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527,
+  -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938,
+  -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925,
+  13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073,
+  6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941,
+  -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595,
+  -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226,
+  -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772,
+  -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734,
+  29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656,
+  25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073,
+  -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377,
+  -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623,
+  -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374,
+  31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135,
+  71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668,
+  70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188,
+  -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115,
+  2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502,
+  101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024,
+  163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724,
+  80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944,
+  -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563,
+  -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664,
+  -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177,
+  -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046,
+  -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798,
+  -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077,
+  -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752,
+  -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202,
+  58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274,
+  48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283,
+  -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057,
+  -123.43864361815416, -140.9579908724867, -157.005848574077]
+y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111,
+  -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728,
+  -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983,
+  -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277,
+  -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074,
+  -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947,
+  -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737,
+  -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779,
+  -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293,
+  -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643,
+  -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577,
+  -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588,
+  -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851,
+  -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558,
+  -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179,
+  -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748,
+  -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684,
+  -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735,
+  -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668,
+  -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535,
+  -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122,
+  -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498,
+  -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707,
+  -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863,
+  -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154,
+  -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564,
+  -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214,
+  -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676,
+  -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744,
+  -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956,
+  -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839,
+  -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955,
+  -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735,
+  -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345,
+  -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231,
+  -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039,
+  -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063,
+  -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203,
+  -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401,
+  -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627,
+  -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747,
+  -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766,
+  -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141,
+  -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347,
+  -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216,
+  -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654,
+  -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015,
+  -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534,
+  -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731,
+  -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451,
+  -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993,
+  -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378,
+  -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472,
+  -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751,
+  -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873,
+  -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556,
+  -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702,
+  -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539,
+  -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257,
+  -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107,
+  -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192,
+  -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091,
+  -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265,
+  -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897,
+  -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183,
+  -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083,
+  -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788,
+  -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237,
+  -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944,
+  -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927,
+  -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068,
+  -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397,
+  -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006,
+  -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678,
+  -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265,
+  -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213,
+  -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764,
+  -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582,
+  67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551,
+  74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474,
+  35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262,
+  204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635,
+  246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412,
+  87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006,
+  66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695,
+  158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347,
+  150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332,
+  116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529,
+  42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666,
+  -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254,
+  -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793,
+  -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045,
+  -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597,
+  -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263,
+  -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724,
+  -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274,
+  -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294,
+  -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296,
+  -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972,
+  -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207,
+  -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173,
+  -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385,
+  -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214,
+  28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786,
+  46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807,
+  110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782,
+  173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245,
+  197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484,
+  210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167,
+  189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755,
+  62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095,
+  -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295,
+  -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374,
+  -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395,
+  -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038,
+  36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376,
+  65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127,
+  -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059,
+  -127.19340520577649, -154.63036780787843, -180.74870760607394]
+yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766,
+  -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698,
+  -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024,
+  -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475,
+  -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925,
+  -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358,
+  -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696,
+  -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558,
+  -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959,
+  -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712,
+  -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775,
+  -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227,
+  -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924,
+  -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524,
+  -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154,
+  -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213,
+  -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968,
+  -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885,
+  -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287,
+  -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954,
+  -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699,
+  -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334,
+  -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104,
+  -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568,
+  -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354,
+  -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645,
+  -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636,
+  -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587,
+  -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638,
+  -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972,
+  -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447,
+  -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077,
+  -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827,
+  -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278,
+  -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243,
+  -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458,
+  -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384,
+  -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971,
+  -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393,
+  -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256,
+  -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992,
+  -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318,
+  -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932,
+  -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875,
+  -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126,
+  -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044,
+  -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763,
+  -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265,
+  -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126,
+  -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113,
+  -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113,
+  -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535,
+  -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696,
+  -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247,
+  -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216,
+  -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353,
+  -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494,
+  -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333,
+  -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105,
+  -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883,
+  -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927,
+  -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884,
+  -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401,
+  -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479,
+  -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717,
+  -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403,
+  -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617,
+  -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946,
+  -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314,
+  -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325,
+  -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804,
+  -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694,
+  -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923,
+  -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567,
+  -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941,
+  -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014,
+  -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199,
+  -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829,
+  -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106,
+  -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166,
+  -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521,
+  -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426,
+  -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046,
+  -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633,
+  -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434,
+  -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539,
+  -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053,
+  -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804,
+  -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645,
+  -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914,
+  -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364,
+  -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178,
+  -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894,
+  -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255,
+  -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792,
+  -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684,
+  -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029,
+  -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007,
+  -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605,
+  -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889,
+  -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275,
+  -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017,
+  -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505,
+  -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556,
+  -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376,
+  -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741,
+  -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233,
+  -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081,
+  -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436,
+  -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614,
+  -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242,
+  -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417,
+  -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184,
+  -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262,
+  -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124,
+  -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636,
+  -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339,
+  -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535,
+  -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313,
+  -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217,
+  -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734,
+  -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575,
+  -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034,
+  -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034,
+  -420.89393277454684]
+z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567,
+  205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663,
+  -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808,
+  -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975,
+  -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551,
+  -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826,
+  -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272,
+  104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167,
+  82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434,
+  -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583,
+  -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846,
+  -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196,
+  -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552,
+  -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175,
+  -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248,
+  -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993,
+  -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733,
+  -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155,
+  -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093,
+  -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656,
+  -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873,
+  25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458,
+  40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028,
+  -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413,
+  -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641,
+  -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581,
+  22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803,
+  15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514,
+  -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898,
+  -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569,
+  -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616,
+  -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532,
+  -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893,
+  18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601,
+  22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818,
+  32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779,
+  48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617,
+  143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107,
+  130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158,
+  44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568,
+  115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683,
+  149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516,
+  80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459,
+  99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014,
+  172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139,
+  113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729,
+  96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513,
+  62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735,
+  -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649,
+  21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656,
+  65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635,
+  70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916,
+  74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807,
+  136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928,
+  168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346,
+  121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441,
+  96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392,
+  104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364,
+  114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289,
+  98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998,
+  49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645,
+  16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556,
+  50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409,
+  60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868,
+  51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244,
+  53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015,
+  64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777,
+  38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126,
+  -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391,
+  -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894,
+  -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211,
+  -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903,
+  -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216,
+  -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931,
+  -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333,
+  21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149,
+  42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823,
+  16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427,
+  -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764,
+  -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054,
+  -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404,
+  -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496,
+  -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292,
+  -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934,
+  -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776,
+  -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126,
+  -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246,
+  -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633,
+  -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934,
+  -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297,
+  15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244,
+  26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861,
+  15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945,
+  36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882,
+  48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032,
+  34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864,
+  10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116,
+  -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808,
+  -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259,
+  -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883,
+  -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713,
+  6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992,
+  15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006,
+  6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738,
+  -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728,
+  -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381,
+  -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837,
+  -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521,
+  -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423,
+  -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537,
+  -111.02550604149512]
+pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703,
+  -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488,
+  -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888,
+  -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515,
+  -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485,
+  -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643,
+  -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232,
+  -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416,
+  -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985,
+  -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466,
+  -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374,
+  -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516,
+  -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498,
+  -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307,
+  -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517,
+  -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573,
+  -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114,
+  -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282,
+  -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618,
+  -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058,
+  -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321,
+  -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244,
+  -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404,
+  -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978,
+  -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411,
+  -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874,
+  -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595,
+  -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251,
+  -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766,
+  -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436,
+  20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966,
+  50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989,
+  53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028,
+  0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609,
+  -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265,
+  -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877]
+y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552,
+  -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052,
+  -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495,
+  -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031,
+  -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644,
+  -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909,
+  -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699,
+  -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666,
+  -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736,
+  -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901,
+  -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841,
+  -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048,
+  -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707,
+  -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386,
+  -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789,
+  -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165,
+  -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913,
+  -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266,
+  -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426,
+  -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594,
+  -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051,
+  -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277,
+  -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796,
+  -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399,
+  66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038,
+  133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598,
+  87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964,
+  -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265,
+  -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668,
+  -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168,
+  -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977,
+  2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377,
+  118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348,
+  85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387,
+  -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273,
+  -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885]
+yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798,
+  -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956,
+  -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042,
+  -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638,
+  -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552,
+  -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574,
+  -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392,
+  -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652,
+  -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402,
+  -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593,
+  -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262,
+  -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446,
+  -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112,
+  -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592,
+  -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205,
+  -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366,
+  -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897,
+  -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097,
+  -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819,
+  -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161,
+  -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005,
+  -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615,
+  -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436,
+  -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045,
+  -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623,
+  -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157,
+  -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133,
+  -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636,
+  -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865,
+  -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675,
+  -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706,
+  -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407,
+  -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109,
+  -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643,
+  -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047,
+  -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373,
+  -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863]
+z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878,
+  -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717,
+  -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224,
+  -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236,
+  -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699,
+  -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219,
+  -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572,
+  -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576,
+  -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458,
+  -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585,
+  3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895,
+  47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762,
+  98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556,
+  110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317,
+  108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497,
+  54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635,
+  83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588,
+  116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785,
+  88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314,
+  52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302,
+  43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712,
+  -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181,
+  -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577,
+  0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294,
+  -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832,
+  -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253,
+  -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826,
+  -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276,
+  10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813,
+  20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483,
+  -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721,
+  -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323,
+  -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184,
+  -29.84051753345265, -36.298155831550744, -43.32322171459229]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/package.xml
new file mode 100644
index 000000000..af9657347
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/package.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_receive_object_action</name>
+  <version>1.0.0</version>
+  <description>An action for receiving objects from a person</description>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>genmsg</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>mas_execution_manager</run_depend>
+
+  <export></export>
+</package>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/action/ReceiveObject.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/action/ReceiveObject.action
new file mode 100644
index 000000000..142b7557f
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/action/ReceiveObject.action
@@ -0,0 +1,12 @@
+# goal
+string posture_type
+bool reception_detection
+bool context_aware
+geometry_msgs/PoseStamped person_pose
+---
+# result
+bool success
+---
+# feedback
+string current_state
+string message
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/launch/receive_object.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/launch/receive_object.launch
new file mode 100644
index 000000000..835160221
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/launch/receive_object.launch
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="move_arm_server" default="move_arm_server" />
+    <arg name="move_base_server" default="move_base_server" />
+    <arg name="force_sensor_topic" default="/wrench/compensated" />
+    <arg name="gripper_controller_pkg_name" default="mdr_gripper_controller" />
+    <arg name="init_config_name" default="pregrasp" />
+    <arg name="receive_object_dmp" default="$(find mdr_receive_object_action)/config/trajectory_weights/receive_object.yaml" />
+    <arg name="receive_object_position_policy_parameters_file" default="learned_position_policy_parameters.pkl" />
+    <arg name="receive_object_policy_config_dir" default="$(find mdr_receive_object_action)/config/learned_policy_parameters/" />
+    <arg name="dmp_tau" default="30" />
+    <arg name="person_dist_threshold" default="0.4" />
+
+    <node pkg="mdr_receive_object_action" type="receive_object_action" name="receive_object_server" output="screen">
+        <param name="move_arm_server" value="$(arg move_arm_server)" />
+        <param name="move_base_server" value="$(arg move_base_server)" />
+        <param name="force_sensor_topic" value="$(arg force_sensor_topic)" />
+        <param name="gripper_controller_pkg_name" value="$(arg gripper_controller_pkg_name)" />
+        <param name="init_config_name" value="$(arg init_config_name)" />
+        <param name="receive_object_dmp" value="$(arg receive_object_dmp)" />
+        <param name="receive_object_position_policy_parameters_file" value="$(arg receive_object_position_policy_parameters_file)" />
+        <param name="receive_object_policy_config_dir" value="$(arg receive_object_policy_config_dir)" />
+        <param name="dmp_tau" value="$(arg dmp_tau)" />
+        <param name="person_dist_threshold" value="$(arg person_dist_threshold)" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action
new file mode 100755
index 000000000..6131bea30
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action
@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+
+from mdr_receive_object_action.msg import ReceiveObjectAction
+from mdr_receive_object_action.action_states import ReceiveObjectSM
+
+class ReceiveObjectServer(object):
+    '''A server exposing an action for receiving objects from people.
+
+    The server expects the following parameters to be made available on the ROS parameter server:
+    '''
+    def __init__(self):
+        gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', 'mdr_gripper_controller')
+        move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server')
+        move_base_server = rospy.get_param('~move_base_server', 'move_base_server')
+        force_sensor_topic = rospy.get_param('~force_sensor_topic', '/wrist/compensated')
+        init_config_name = rospy.get_param('~init_config_name', 'init_config_name')
+        receive_object_dmp = rospy.get_param('~receive_object_dmp', '')
+        pos_policy_params_file = rospy.get_param('~receive_object_position_policy_parameters_file', '')
+        policy_config_dir = rospy.get_param('~receive_object_policy_config_dir', '')
+        dmp_tau = rospy.get_param('~dmp_tau', 'dmp_tau')
+        person_dist_threshold = rospy.get_param('~person_dist_threshold', 'person_dist_threshold')
+
+        rospy.loginfo('[receive_object] Initialising state machine')
+        self.action_sm = ReceiveObjectSM(gripper_controller_pkg_name=gripper_controller_pkg_name,
+                                         move_arm_server=move_arm_server,
+                                         move_base_server=move_base_server,
+                                         force_sensor_topic=force_sensor_topic,
+                                         init_config_name=init_config_name,
+                                         receive_object_policy_config_dir=policy_config_dir,
+                                         receive_object_position_policy_parameters_file=pos_policy_params_file,
+                                         receive_object_dmp=receive_object_dmp,
+                                         dmp_tau=dmp_tau,
+                                         person_dist_threshold=person_dist_threshold)
+        rospy.loginfo('[receive_object] State machine initialised')
+
+        self.action_server = actionlib.SimpleActionServer('receive_object_server',
+                                                          ReceiveObjectAction,
+                                                          self.execute, False)
+        self.action_server.start()
+        rospy.loginfo('receive_object action server ready')
+
+    def execute(self, goal):
+        rospy.loginfo('[receive_object] Received an action request')
+        self.action_sm.goal = goal
+        self.action_sm.result = None
+        self.action_sm.execution_requested = True
+        while not self.action_sm.result:
+            rospy.sleep(0.05)
+        self.action_server.set_succeeded(self.action_sm.result)
+
+if __name__ == '__main__':
+    rospy.init_node('receive_object_server')
+    receive_object_server = ReceiveObjectServer()
+    try:
+        receive_object_server.action_sm.run()
+        while receive_object_server.action_sm.is_running and not rospy.is_shutdown():
+            rospy.spin()
+    except (KeyboardInterrupt, SystemExit):
+        print('{0} interrupted; exiting...'.format(receive_object_server.action_sm.name))
+        receive_object_server.action_sm.stop()
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
new file mode 100755
index 000000000..429b3c953
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
@@ -0,0 +1,35 @@
+#! /usr/bin/env python
+import argparse
+import rospy
+import actionlib
+
+from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
+
+if __name__ == '__main__':
+    posture_contexts = ['standing', 'seated', 'lying']
+    release_detection = [0, 1]
+
+    parser = argparse.ArgumentParser(description='help text')
+    parser.add_argument("--posture", "-p", type=str, default='seated',
+                        help="posture context parameters. Choose from 'standing', 'seated', or 'lying'.")
+    parser.add_argument("--release_detection", "-r", type=bool, default=1,
+                        help="list of context parameters. E.g.: 0")
+    args = parser.parse_args()
+
+    rospy.init_node('receive_object_action_client_test')
+
+    client = actionlib.SimpleActionClient('/receive_object_server', ReceiveObjectAction)
+    client.wait_for_server()
+
+    goal = ReceiveObjectGoal()
+
+    rospy.loginfo('Setting goal for a %s person...', args.posture)
+    goal.posture_type = args.posture
+    goal.release_detection = args.release_detection
+    goal.person_pose.header.frame_id = "base_link"
+
+    timeout = 15.0
+    rospy.loginfo('Sending action lib goal to receive_object_server')
+    client.send_goal(goal)
+    client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
+    print(client.get_result())
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/__init__.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
new file mode 100644
index 000000000..a4900704c
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
@@ -0,0 +1,234 @@
+#!/usr/bin/python
+from importlib import import_module
+from os.path import join
+import pickle
+import numpy as np
+from scipy.stats import norm
+
+import rospy
+import tf
+import actionlib
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import WrenchStamped
+
+from pyftsm.ftsm import FTSMTransitions
+from mas_execution.action_sm_base import ActionSMBase
+from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
+from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
+from mdr_receive_object_action.msg import ReceiveObjectResult
+
+class ReceiveObjectSM(ActionSMBase):
+    def __init__(self, timeout=120.0,
+                 gripper_controller_pkg_name='mdr_gripper_controller',
+                 force_sensor_topic='/wrench/compensated',
+                 move_arm_server='move_arm_server',
+                 move_base_server='move_base_server',
+                 init_config_name = 'neutral',
+                 receive_object_policy_config_dir='',
+                 receive_object_position_policy_parameters_file = 'learned_position_policy_parameters.pkl',
+                 receive_object_dmp = '',
+                 dmp_tau = 30.,
+                 person_dist_threshold = 0.5,
+                 max_recovery_attempts=1):
+        super(ReceiveObjectSM, self).__init__('ReceiveObject', [], max_recovery_attempts)
+        self.timeout = timeout
+
+        gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name)
+        GripperControllerClass = getattr(import_module(gripper_controller_module_name), 'GripperController')
+        self.gripper = GripperControllerClass()
+
+        self.move_arm_server = move_arm_server
+        self.move_base_server = move_base_server
+
+        self.init_config_name = init_config_name
+        self.receive_object_policy_config_dir = receive_object_policy_config_dir
+
+        self.receive_object_dmp = receive_object_dmp
+        self.dmp_tau = dmp_tau
+        self.receive_object_position_policy_parameters_file = join(self.receive_object_policy_config_dir, receive_object_position_policy_parameters_file)
+        self.receive_object_position_policy_parameters = self.load_policy_params_from_file(self.receive_object_position_policy_parameters_file)
+        self.person_dist_threshold = person_dist_threshold
+
+        self.tf_listener = tf.TransformListener()
+        self.force_sensor_topic = force_sensor_topic
+
+        self.latest_force_measurement_z = 0.
+        self.cumsum_z = 0
+        self.force_detection_threshold = 1.
+        self.object_reception_detected = False
+
+    def init(self):
+        try:
+            self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction)
+            rospy.loginfo('[receive_object] Waiting for %s server', self.move_arm_server)
+            self.move_arm_client.wait_for_server()
+        except Exception as exc:
+            rospy.logerr('[receive_object] %s', str(exc))
+            return FTSMTransitions.INIT_FAILED
+
+        try:
+            self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction)
+            rospy.loginfo('[receive_object] Waiting for %s server', self.move_base_server)
+            self.move_base_client.wait_for_server()
+        except Exception as exc:
+            rospy.logerr('[receive_object] %s', str(exc))
+            return FTSMTransitions.INIT_FAILED
+
+        # Start with arm in neutral position:
+        rospy.loginfo('[receive_object] Moving arm to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
+
+        return FTSMTransitions.INITIALISED
+
+    def running(self):
+        receive_object_pose = PoseStamped()
+        receive_object_pose.header.frame_id = 'base_link'
+        if self.goal.context_aware:
+            # > Pick context-dependent receive object position:
+            policy_parameter_a = self.receive_object_position_policy_parameters[0][0]
+            policy_parameter_A = self.receive_object_position_policy_parameters[0][1]
+
+            if self.goal.posture_type == 'standing':
+                context_vector = np.array([0.0, 0.0, 0.4])
+            elif self.goal.posture_type == 'seated':
+                context_vector = np.array([0.5, 0.0, 0.0])
+            elif self.goal.posture_type == 'lying':
+                context_vector = np.array([0.0, 0.7, 0.0])
+
+            # Sample upper-level policy (with no exploration) for object reception position;
+            # by calculating the mean of the linear-Gaussian model, given context vector s
+            receive_object_position = np.squeeze(policy_parameter_a + context_vector.dot(policy_parameter_A))
+
+            receive_object_pose.pose.position.x = receive_object_position[0]
+            receive_object_pose.pose.position.y = receive_object_position[1]
+            receive_object_pose.pose.position.z = receive_object_position[2]
+        else:
+            # Pick context-independent object reception position
+            receive_object_pose.pose.position.x = 0.5
+            receive_object_pose.pose.position.y = 0.078
+            receive_object_pose.pose.position.z = 0.8
+
+        receive_object_pose.pose.orientation.x = 0.000
+        receive_object_pose.pose.orientation.y = 0.000
+        receive_object_pose.pose.orientation.z = 0.000
+        receive_object_pose.pose.orientation.w = 1.000
+
+        self.goal.person_pose = self.tf_listener.transformPose("base_link", self.goal.person_pose)
+        distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 + self.goal.person_pose.pose.position.y**2)
+        if distance_to_person > self.person_dist_threshold:
+            move_to_person_goal = MoveBaseGoal()
+            move_to_person_goal.goal_type = MoveBaseGoal.POSE
+            move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id
+            move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.3
+            move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.3
+            rospy.loginfo('[receive_object] Moving to person...')
+
+            self.move_base_client.send_goal(move_to_person_goal)
+
+            # we try moving towards the person before receiving the object;
+            # the person should be close by, so we move for a short time
+            # since the robot will either reach the person in that time,
+            # or will stop as close to the person as possible
+            self.move_base_client.wait_for_result(rospy.Duration(10.))
+            self.move_base_client.cancel_goal()
+
+        # Start with arm in neutral position:
+        rospy.loginfo('[receive_object] Moving arm to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
+
+        # Move to chosen receive_object position, along appropriate trajectory:
+        rospy.loginfo('[receive_object] Receiving object...')
+        self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, receive_object_pose)
+
+        if not self.goal.reception_detection:
+            # Naive object release strategy:
+            rospy.loginfo('[receive_object] Waiting before releasing object...')
+            rospy.sleep(5.)
+
+            # Release the object:
+            rospy.loginfo('[receive_object] Opening the gripper...')
+            self.gripper.open()
+
+            # Since no detection is used here, the object is released and we always set the reception to True.
+            self.object_reception_detected = True
+        else:
+            # Force sensing object release strategy:
+            rospy.sleep(1.)
+            rospy.loginfo('[receive_object] Waiting for object to be received...')
+            rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb)
+            self.object_reception_detected = False
+            self.cumsum_z = 0.
+
+            start_time = rospy.get_time()
+            while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected:
+                self.detect_object_reception()
+                rospy.sleep(0.1)
+
+            # If a pull is detected, release the object:
+            if self.object_reception_detected:
+                rospy.loginfo('[receive_object] Closing the gripper to receive the object...')
+                self.gripper.close()
+            else:
+                rospy.loginfo('[receive_object] Not waiting anymore since no reception was detected')
+
+        # Return to a neutral arm position:
+        rospy.loginfo('[receive_object] Moving back to neutral position...')
+        self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
+
+        if self.object_reception_detected:
+            self.result = self.set_result(True)
+        else:
+            self.result = self.set_result(False)
+        return FTSMTransitions.DONE
+
+    def __move_arm(self, goal_type, goal):
+        '''Sends a request to the 'move_arm' action server and waits for the
+        results of the action execution.
+        Keyword arguments:
+        goal_type -- 'MoveArmGoal.NAMED_TARGET' or 'MoveArmGoal.END_EFFECTOR_POSE'
+        goal -- A string if 'goal_type' is 'MoveArmGoal.NAMED_TARGET';
+                a 'geometry_msgs/PoseStamped' if 'goal_type' is 'MoveArmGoal.END_EFFECTOR_POSE'
+        '''
+        move_arm_goal = MoveArmGoal()
+        move_arm_goal.goal_type = goal_type
+        if goal_type == MoveArmGoal.NAMED_TARGET:
+            move_arm_goal.named_target = goal
+        elif goal_type == MoveArmGoal.END_EFFECTOR_POSE:
+            move_arm_goal.end_effector_pose = goal
+            move_arm_goal.dmp_name = self.receive_object_dmp
+            move_arm_goal.dmp_tau = self.dmp_tau
+        self.move_arm_client.send_goal(move_arm_goal)
+        self.move_arm_client.wait_for_result()
+        result = self.move_arm_client.get_result()
+        return result
+
+    def recovering(self):
+        ## TODO: include recovery behaviours?
+        rospy.sleep(5.)
+        return FTSMTransitions.DONE_RECOVERING
+
+    def set_result(self, success):
+        result = ReceiveObjectResult()
+        result.success = success
+        return result
+
+    def load_policy_params_from_file(self, filename):
+        with open(filename, 'rb') as f:
+            return pickle.load(f)
+
+    def detect_object_reception(self):
+        mu_0 = 0.
+        mu_1 = -6.
+        std = 1.
+
+        pdf_0 = norm(mu_0, std)
+        pdf_1 = norm(mu_1, std)
+
+        self.cumsum_z += max(0., np.log(pdf_1.pdf(self.latest_force_measurement_z) / pdf_0.pdf(self.latest_force_measurement_z)))
+
+        if self.cumsum_z > self.force_detection_threshold:
+            rospy.loginfo('[receive_object] Object reception detected!')
+            self.object_reception_detected = True
+
+    def force_sensor_cb(self, force_sensor_msg):
+        self.latest_force_measurement_z = force_sensor_msg.wrench.force.z
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/setup.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/setup.py
new file mode 100644
index 000000000..03ab16ec3
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_receive_object_action'],
+    package_dir={'mdr_receive_object_action': 'ros/src/mdr_receive_object_action'}
+)
+
+setup(**d)

From cf17bc5bddeb46747d0b18d8cbf1b8004ff57cda Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sat, 14 Nov 2020 17:30:58 +0100
Subject: [PATCH 134/264] [demos] Added context-aware object reception demo

---
 .../CMakeLists.txt                            | 27 ++++++++
 .../context_aware_receive_object_sm.yaml      | 64 +++++++++++++++++++
 .../package.xml                               | 27 ++++++++
 .../ros/launch/receive_object.launch          | 19 ++++++
 .../__init__.py                               |  0
 .../scenario_states/__init__.py               |  0
 .../scenario_states/receive_object.py         | 48 ++++++++++++++
 .../setup.py                                  | 11 ++++
 8 files changed, 196 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/config/context_aware_receive_object_sm.yaml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/launch/receive_object.launch
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/receive_object.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/CMakeLists.txt
new file mode 100644
index 000000000..cb1c92911
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_demo_context_aware_receive_object)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  mdr_move_arm_action
+  mdr_move_base_action
+  mdr_find_people
+  mdr_receive_object_action
+  mas_execution_manager
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mdr_move_arm_action
+    mdr_move_base_action
+    mdr_find_people
+    mdr_receive_object_action
+    mas_execution_manager
+)
+
+catkin_python_setup()
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/config/context_aware_receive_object_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/config/context_aware_receive_object_sm.yaml
new file mode 100644
index 000000000..73a5b9bfa
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/config/context_aware_receive_object_sm.yaml
@@ -0,0 +1,64 @@
+sm_id: mdr_demo_context_aware_receive_object
+states: [DETECT_PERSON, IDENTIFY_POSTURE, RECEIVE_OBJECT]
+outcomes: [DONE, FAILED]
+state_descriptions:
+    - state:
+        name: DETECT_PERSON
+        state_module_name: mdr_demo_context_aware_hand_over.scenario_states.detect_person
+        state_class_name: DetectPerson
+        transitions:
+            - transition:
+                name: succeeded
+                state: IDENTIFY_POSTURE
+            - transition:
+                name: failed
+                state: DETECT_PERSON
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: IDENTIFY_POSTURE
+        state_module_name: mdr_demo_context_aware_hand_over.scenario_states.identify_posture
+        state_class_name: IdentifyPosture
+        transitions:
+            - transition:
+                name: succeeded
+                state: RECEIVE_OBJECT
+            - transition:
+                name: failed
+                state: DETECT_PERSON
+        arguments:
+            - argument:
+                name: posture_height_width_ratio_ranges
+                value:
+                    standing: [1.65, 5.0]
+                    seated: [0.6, 1.65]
+                    lying: [0.0, 0.6]
+    - state:
+        name: RECEIVE_OBJECT
+        state_module_name: mdr_demo_context_aware_receive_object.scenario_states.receive_object
+        state_class_name: ReceiveObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: DONE
+            - transition:
+                name: failed
+                state: RECEIVE_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: context_aware
+                value: True
+            - argument:
+                name: release_detection
+                value: True
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/package.xml
new file mode 100644
index 000000000..a834ec39e
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_demo_context_aware_receive_object</name>
+  <version>1.0.0</version>
+  <description>State machine and launch files for a scenario in which a robot detects a person and receives and object from them</description>
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <author email="ahmed.abdelrahman@smail.inf.h-brs.de">Ahmed Faisal Abdelrahman</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>mdr_move_arm_action</build_depend>
+  <build_depend>mdr_move_base_action</build_depend>
+  <build_depend>mdr_find_people</build_depend>
+  <build_depend>mdr_receive_object_action</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>mdr_move_arm_action</run_depend>
+  <run_depend>mdr_move_base_action</run_depend>
+  <run_depend>mdr_find_people</run_depend>
+  <run_depend>mdr_receive_object_action</run_depend>
+  <run_depend>mas_execution_manager</run_depend>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/launch/receive_object.launch b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/launch/receive_object.launch
new file mode 100644
index 000000000..4b8d53f9b
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/launch/receive_object.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+
+    <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch">
+        <arg name="data_path" value="$(find mdr_demo_context_aware_receive_object)/config/" />
+        <arg name="problem_path" value="$(find mdr_demo_context_aware_receive_object)/config/problem.pddl" />
+        <arg name="strl_file_path" value="$(find mdr_demo_context_aware_receive_object)/config/plan.strl" />
+    </include>
+
+    <!-- Actions -->
+    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
+    <include file="$(find mdr_receive_object_action)/ros/launch/receive_object.launch" />
+
+    <!-- State machine -->
+    <node pkg="mas_execution_manager" type="state_machine_creator" name="context_aware_receive_object" output="screen">
+        <param name="sm_config_file" type="str" value="$(find mdr_demo_context_aware_receive_object)/config/context_aware_receive_object_sm.yaml" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/receive_object.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/receive_object.py
new file mode 100644
index 000000000..39f7b4ffe
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/ros/src/mdr_demo_context_aware_receive_object/scenario_states/receive_object.py
@@ -0,0 +1,48 @@
+import rospy
+import actionlib
+
+from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class ReceiveObject(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'receive_object',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'],
+                                   input_keys=['posture', 'person_pose'])
+        self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_receive_object')
+        self.action_server = kwargs.get('action_server', 'receive_object_server')
+        self.timeout = kwargs.get('timeout', 120.)
+        self.context_aware = kwargs.get('context_aware', True)
+        self.reception_detection = kwargs.get('reception_detection', True)
+
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+        self.client = actionlib.SimpleActionClient(self.action_server, ReceiveObjectAction)
+        self.client.wait_for_server(rospy.Duration(10.))
+
+    def execute(self, userdata):
+        goal = ReceiveObjectGoal()
+        goal.posture_type = userdata.posture
+        goal.context_aware = self.context_aware
+        goal.reception_detection = self.reception_detection
+        goal.person_pose = userdata.person_pose
+
+        # calling the actionlib server and waiting for the execution to end
+        rospy.loginfo('[receive_object] Sending action lib goal to {0}'.format(self.action_server))
+        self.say('Receiving object from a {0} person'.format(userdata.posture))
+        self.client.send_goal(goal)
+        self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        result = self.client.get_result()
+
+        if result and result.success:
+            rospy.loginfo('Received object')
+            return 'succeeded'
+        else:
+            rospy.logerr('Could not receive object')
+            self.say('Object not received')
+            if self.retry_count == self.number_of_retries:
+                self.say('Aborting operation')
+                return 'failed_after_retrying'
+            self.retry_count += 1
+            return 'failed'
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/setup.py
new file mode 100644
index 000000000..e6ede3675
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_context_aware_receive_object/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_demo_context_aware_receive_object'],
+    package_dir={'mdr_demo_context_aware_receive_object': 'ros/src/mdr_demo_context_aware_receive_object'}
+)
+
+setup(**d)

From 58a409dc53860affb0eff78ddd5becabf339a9f7 Mon Sep 17 00:00:00 2001
From: Santosh Thoduka <santosh.thoduka@gmail.com>
Date: Tue, 10 Nov 2020 12:00:40 +0100
Subject: [PATCH 135/264] [hand_over_action] fix use of arguments in action
 client test and README

---
 .../mdr_hand_over_action/README.md                  |  3 ++-
 .../ros/scripts/hand_over_action_client_test        | 13 ++++++++-----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
index a595762dc..e7c40251e 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/README.md
@@ -101,4 +101,5 @@ It is assumed that the robot starts with the object to be handed over already in
 
 1. Run the ``move_arm`` action server: ``roslaunch mdr_move_arm_action move_arm.launch``
 2. Run the action server: ``roslaunch mdr_hand_over_action hand_over.launch``
-3. Run the client example: ``rosrun mdr_hand_over_action hand_over_action_client_test ['standing', 0, 1]``, providing a list of the action's parameters (context, obstacle, release_detection) as arguments
+3. Run the client example: ``rosrun mdr_hand_over_action hand_over_action_client_test -p 'standing'``. Additional arguments are: `-c` for context-aware execution, `-r` for release detection using the force-torque sensor and `-o` for execution with obstacle avoidance.
+
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
index c7648471d..6ebfda458 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_hand_over_action/ros/scripts/hand_over_action_client_test
@@ -13,11 +13,13 @@ if __name__ == '__main__':
 
     parser = argparse.ArgumentParser(description='help text')
     parser.add_argument("--posture", "-p", type=str, default='seated',
-                        help="posture context parameters. Choose from 'standing', 'seated', or 'lying'.") 
-    parser.add_argument("--obstacle", "-o", type=bool, default=0,
-                        help="list of context parameters. Choose 0 for no obstacle or 1 for obstacle.") 
-    parser.add_argument("--release_detection", "-r", type=bool, default=1,
-                        help="list of context parameters. E.g.: 0") 
+                        help="posture context parameters. Choose from 'standing', 'seated', or 'lying'.")
+    parser.add_argument("--context_aware", "-c", action="store_true", dest="context_aware",
+                        help="Use context-based handover")
+    parser.add_argument("--obstacle_avoidance", "-o", action="store_true", dest="obstacle",
+                        help="Use obstacle avoidance")
+    parser.add_argument("--release_detection", "-r", action="store_true", dest="release_detection",
+                        help="Use force-torque sensor for release detection")
     args = parser.parse_args()
 
     rospy.init_node('hand_over_action_client_test')
@@ -31,6 +33,7 @@ if __name__ == '__main__':
     goal.posture_type = args.posture
     goal.obstacle = args.obstacle
     goal.release_detection = args.release_detection
+    goal.context_aware = args.context_aware
     goal.person_pose.header.frame_id = "base_link"
 
     timeout = 15.0

From c66b3bb2ecab6d498cbac733ae48562072ac0e4d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 16:24:19 +0100
Subject: [PATCH 136/264] [find_people] Replaced the SSD keras detector with a
 torch Faster R-CNN detector

---
 .../ros/launch/find_people.launch             |  8 +++
 .../ros/src/mdr_find_people/action_states.py  | 52 +++++++++++++++----
 .../ros/src/mdr_find_people/find_people.py    | 50 ++++++------------
 3 files changed, 68 insertions(+), 42 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
index 0f3acf8a2..c69707b7f 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
@@ -1,10 +1,18 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="pointcloud_topic" default="/mdr_perception/rectified_points"/>
+    <arg name="class_annotation_file" default="$(find mas_perception_libs)/models/coco_classes.yaml"/>
+    <arg name="detector_module" default="torchvision.models.detection"/>
+    <arg name="detector_name" default="fasterrcnn_resnet50_fpn"/>
+    <arg name="detection_threshold" default="0.8"/>
     <arg name="face_embedding_model_path" default="/home/robot/models/face_embedding_model.pt"/>
 
     <node pkg="mdr_find_people" type="find_people_server" name="find_people_server" output="screen" >
         <param name="pointcloud_topic" type="string" value="$(arg pointcloud_topic)" />
+        <param name="class_annotation_file" type="string" value="$(arg class_annotation_file)" />
+        <param name="detector_module" type="string" value="$(arg detector_module)" />
+        <param name="detector_name" type="string" value="$(arg detector_name)" />
+        <param name="detection_threshold" type="string" value="$(arg detection_threshold)" />
         <param name="face_embedding_model_path" type="string" value="$(arg face_embedding_model_path)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 0cc9aec02..8730b7031 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -1,8 +1,11 @@
+from importlib import import_module
+from PIL import Image as PILImage
+import yaml
+
 import rospy
 import smach
 import torch
 
-from PIL import Image as PILImage
 import tf
 from sensor_msgs.msg import PointCloud2, Image
 from mdr_find_people.msg import FindPeopleResult
@@ -25,16 +28,44 @@ def __init__(self):
 
         self._listener = tf.TransformListener()
         self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
+        self.class_annotation_file = rospy.get_param('class_annotation_file', '')
+        self.detector_module = rospy.get_param('detector_module', 'torchvision.models.detection')
+        self.detector_name = rospy.get_param('detector_name', 'fasterrcnn_resnet50_fpn')
+        self.detection_threshold = float(rospy.get_param('detection_threshold', 0.))
         self.face_embedding_model_path = rospy.get_param("~face_embedding_model_path", '')
 
+        if not self.class_annotation_file:
+            raise ValueError('Parameter "class_annotation_file" cannot be empty!')
+
+        self.class_annotations = None
+        with open(self.class_annotation_file, 'r') as annotations_file:
+            self.class_annotations = yaml.safe_load(annotations_file)
+        self.class_annotations[0] = 'background'
+
+        if not self.detector_module:
+            rospy.logwarn('[find_people] Found empty "detector_module"; using default torchvision.models.detection')
+            self.detector_module = 'torchvision.models.detection'
+
+        if not self.detector_name:
+            rospy.logwarn('[find_people] Found empty "detector_name"; using default fasterrcnn_resnet50_fpn')
+            self.detector_module = 'fasterrcnn_resnet50_fpn'
+
+        self.model_device = torch.device('cuda') if torch.cuda.is_available() \
+                                                 else torch.device('cpu')
+        detector_class = getattr(import_module(self.detector_module),
+                                 self.detector_name)
+        self.detector = detector_class(pretrained=True)
+        self.detector.eval()
+        self.detector.to(self.model_device)
+
         self.face_embedding_model = None
         if self.face_embedding_model_path:
             self.face_embedding_model = SiameseNetwork()
             self.face_embedding_model.load_state_dict(torch.load(self.face_embedding_model_path))
             self.face_embedding_model.eval()
+            self.face_embedding_model.to(self.model_device)
 
-            device = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu')
-            self.face_embedding_model.to(device)
+        self.cv_bridge = CvBridge()
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FIND_PEOPLE')
@@ -43,22 +74,25 @@ def execute(self, userdata):
         cloud_msg = rospy.wait_for_message(self.pointcloud_topic, PointCloud2)
 
         # Get positions of people
-        predictions, bb2ds, poses = FindPeople.detect(cloud_msg)
+        predictions, bb2ds, poses = FindPeople.detect(cloud_msg,
+                                                      self.detector,
+                                                      self.model_device,
+                                                      self.class_annotations,
+                                                      self.detection_threshold)
 
         # Get people images
         cv_image = cloud_msg_to_cv_image(cloud_msg)
-        bridge = CvBridge()
         images = []
         face_images = []
         for i, bb2d in enumerate(bb2ds):
             cropped_cv = crop_image(cv_image, bb2d)
-            cropped_img_msg = bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
+            cropped_img_msg = self.cv_bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
 
             rospy.loginfo('[find_people] Attempting to extract face of person {0}'.format(i+1))
             cropped_face_img = FindPeople.extract_face_image(cropped_cv)
             if cropped_face_img is not None:
-                cropped_face_img_msg = bridge.cv2_to_imgmsg(cropped_face_img,
-                                                            encoding='passthrough')
+                cropped_face_img_msg = self.cv_bridge.cv2_to_imgmsg(cropped_face_img,
+                                                                    encoding='passthrough')
             else:
                 cropped_face_img_msg = Image()
 
@@ -83,7 +117,7 @@ def execute(self, userdata):
             face_view.image = face_images[i]
 
             if self.face_embedding_model is not None and face_view.image.data:
-                face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
+                face_cv2 = self.cv_bridge.imgmsg_to_cv2(face_view.image)
                 img_tensor = get_transforms()(PILImage.fromarray(face_cv2))
                 img_tensor.unsqueeze_(0)
 
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index 146e28f4b..d00d2cd52 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -1,68 +1,53 @@
 #!/usr/bin/env python
 
-import os
-
 import numpy as np
 import pcl
 import face_recognition
 from sklearn.impute import SimpleImputer
+import torch
 
 import rospy
-from rospkg import RosPack
 from std_msgs.msg import Header
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 
-from mas_perception_libs import ImageDetectionKey, ImageDetectorBase
+from mas_perception_libs import ImageDetectionKey, ImageDetectorBase, TorchImageDetector
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
                                       crop_cloud_to_xyz, crop_organized_cloud_msg
 from mas_perception_libs.visualization import draw_labeled_boxes
-from ssd_keras_ros import SSDKerasObjectDetector
+
+import dataset_interface.object_detection.transforms as T
 
 
 class FindPeople(object):
     @staticmethod
-    def detect(cloud_msg):
-        # Transform point cloud to base link
-        #cloud_msg = transform_cloud_with_listener(cloud_msg, '/base_link', tf.TransformListener())
-
-        # use RosPack to point to appropriate configuration files
-        rp = RosPack()
-        package_path = rp.get_path('ssd_keras_ros')
-        class_ann_file = os.path.join(package_path, 'models', 'coco_classes.yml')
-        kwargs_file = os.path.join(package_path, 'models', 'ssd_keras_object_detector_kwargs.yml')
-
-        # create SSDKerasObjectDetector object and call detection on
-        detector = SSDKerasObjectDetector(class_file=class_ann_file, model_kwargs_file=kwargs_file)
-        predictions = detector.detect([cloud_msg_to_image_msg(cloud_msg)])[0]
+    def detect(cloud_msg, detector, detector_device, class_annotations, detection_threshold):
+        img = cloud_msg_to_cv_image(cloud_msg)
+        transform = T.ToTensor()
+        img, _ = transform(img, None)
+        with torch.no_grad():
+            predictions = detector([img.to(detector_device)])
 
         # Get bounding boxes
-        bb2ds = ImageDetectorBase.prediction_to_bounding_boxes(predictions)[0]
-
-        # Filter results for people
-        predictions, bb2ds = FindPeople.filter_people(predictions, bb2ds)
-
-        # Get poses
-        poses = FindPeople.get_people_poses(cloud_msg, predictions, bb2ds)
-
-        return predictions, bb2ds, poses
-
+        detections = TorchImageDetector.process_predictions(predictions=predictions,
+                                                            classes=class_annotations,
+                                                            detection_threshold=detection_threshold)
+        bounding_boxes = ImageDetectorBase.prediction_to_bounding_boxes(detections)[0]
+        people_detections, bb2ds = FindPeople.filter_people(detections, bounding_boxes)
+        poses = FindPeople.get_people_poses(cloud_msg, people_detections, bounding_boxes)
+        return people_detections, bb2ds, poses
 
     @staticmethod
     def filter_people(predictions, bounding_boxes):
         people_preds = []
         people_bbs = []
-
         for i, _ in enumerate(predictions):
             pred = predictions[i]
             bb2d = bounding_boxes[i]
-
             if pred[ImageDetectionKey.CLASS] == 'person':
                 people_preds.append(pred)
                 people_bbs.append(bb2d)
-
         return people_preds, people_bbs
 
-
     @staticmethod
     def get_people_poses(cloud_msg, predictions, bounding_boxes):
         poses = []
@@ -95,7 +80,6 @@ def get_people_poses(cloud_msg, predictions, bounding_boxes):
 
         return poses
 
-
     @staticmethod
     def render_image_with_detections(cloud_msg, bounding_boxes):
         cv_image = cloud_msg_to_cv_image(cloud_msg)

From 2e4018e18b09a6c7d19ce4b1c6fd606c641c4630 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:04:50 +0100
Subject: [PATCH 137/264] [find_people/action_states] Fixed scope of newly
 added parameters

---
 .../ros/src/mdr_find_people/action_states.py              | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
index 8730b7031..a784abd58 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py
@@ -28,10 +28,10 @@ def __init__(self):
 
         self._listener = tf.TransformListener()
         self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
-        self.class_annotation_file = rospy.get_param('class_annotation_file', '')
-        self.detector_module = rospy.get_param('detector_module', 'torchvision.models.detection')
-        self.detector_name = rospy.get_param('detector_name', 'fasterrcnn_resnet50_fpn')
-        self.detection_threshold = float(rospy.get_param('detection_threshold', 0.))
+        self.class_annotation_file = rospy.get_param('~class_annotation_file', '')
+        self.detector_module = rospy.get_param('~detector_module', 'torchvision.models.detection')
+        self.detector_name = rospy.get_param('~detector_name', 'fasterrcnn_resnet50_fpn')
+        self.detection_threshold = float(rospy.get_param('~detection_threshold', 0.))
         self.face_embedding_model_path = rospy.get_param("~face_embedding_model_path", '')
 
         if not self.class_annotation_file:

From 83f14238cdd0129e5457f132e52e490d3a605ead Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:07:12 +0100
Subject: [PATCH 138/264] [find_people.launch] coco_classes.yaml ->
 coco_classes.yml

This is the name of the file in mas_perception_libs
---
 .../mdr_find_people/ros/launch/find_people.launch               | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
index c69707b7f..9a57dd13c 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="pointcloud_topic" default="/mdr_perception/rectified_points"/>
-    <arg name="class_annotation_file" default="$(find mas_perception_libs)/models/coco_classes.yaml"/>
+    <arg name="class_annotation_file" default="$(find mas_perception_libs)/models/coco_classes.yml"/>
     <arg name="detector_module" default="torchvision.models.detection"/>
     <arg name="detector_name" default="fasterrcnn_resnet50_fpn"/>
     <arg name="detection_threshold" default="0.8"/>

From 1ab3324cac1fb9d6dc8135007d8dfbdc90fff362 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:14:19 +0100
Subject: [PATCH 139/264] [find_people] Fixed incorrectly passed argument to
 mas_perception_libs util fn

---
 .../mdr_find_people/ros/src/mdr_find_people/find_people.py      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index d00d2cd52..20c4f1cae 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -28,7 +28,7 @@ def detect(cloud_msg, detector, detector_device, class_annotations, detection_th
             predictions = detector([img.to(detector_device)])
 
         # Get bounding boxes
-        detections = TorchImageDetector.process_predictions(predictions=predictions,
+        detections = TorchImageDetector.process_predictions(predictions=predictions[0],
                                                             classes=class_annotations,
                                                             detection_threshold=detection_threshold)
         bounding_boxes = ImageDetectorBase.prediction_to_bounding_boxes(detections)[0]

From b044f1e13638157ec9f8b8951baab5956b7d0e24 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:32:55 +0100
Subject: [PATCH 140/264] [find_people] Added some debugging msgs during
 execution

---
 .../ros/src/mdr_find_people/find_people.py            | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index 20c4f1cae..56c1fa61b 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -24,16 +24,27 @@ def detect(cloud_msg, detector, detector_device, class_annotations, detection_th
         img = cloud_msg_to_cv_image(cloud_msg)
         transform = T.ToTensor()
         img, _ = transform(img, None)
+
+        rospy.loginfo('Running detector...')
         with torch.no_grad():
             predictions = detector([img.to(detector_device)])
 
         # Get bounding boxes
+        rospy.loginfo('Processing detections...')
         detections = TorchImageDetector.process_predictions(predictions=predictions[0],
                                                             classes=class_annotations,
                                                             detection_threshold=detection_threshold)
+
+        rospy.loginfo('Extracting bounding boxes...')
         bounding_boxes = ImageDetectorBase.prediction_to_bounding_boxes(detections)[0]
+
+        rospy.loginfo('Extracting people detections...')
         people_detections, bb2ds = FindPeople.filter_people(detections, bounding_boxes)
+
+        rospy.loginfo('Extracting person poses...')
         poses = FindPeople.get_people_poses(cloud_msg, people_detections, bounding_boxes)
+        rospy.loginfo('Person detection complete')
+
         return people_detections, bb2ds, poses
 
     @staticmethod

From ecb40c48bd92fe5e6baaac3bbea50c3efe6e0d31 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:37:44 +0100
Subject: [PATCH 141/264] [find_people/client_test] Fixed a runtime error;
 added img publisher for debugging purposes

---
 .../ros/scripts/find_people_client_test          | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
index a6b7dcf47..e11e7ab34 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client_test
@@ -3,11 +3,14 @@
 import rospy
 import actionlib
 
+from sensor_msgs.msg import Image
+
 from mdr_find_people.msg import FindPeopleAction, FindPeopleGoal
 
 
 if __name__ == '__main__':
     rospy.init_node('find_people_client_test')
+    person_img_pub = rospy.Publisher('/find_people_client_test/person', Image, queue_size=1)
 
     rospy.loginfo('Waiting for server...')
     client = actionlib.SimpleActionClient('find_people_server', FindPeopleAction)
@@ -16,9 +19,9 @@ if __name__ == '__main__':
     goal = FindPeopleGoal()
 
     try:
-        print('Sending goal...')
+        rospy.loginfo('Sending goal...')
         client.send_goal(goal)
-        print('Waiting for result...')
+        rospy.loginfo('Waiting for result...')
         client.wait_for_result(rospy.Duration.from_sec(120))
 
         result = client.get_result()
@@ -26,7 +29,12 @@ if __name__ == '__main__':
             rospy.logerr('No people found')
         rospy.loginfo('Found {0} people'.format(len(result.person_list.persons)))
         for person in result.person_list.persons:
-            rospy.loginfo('Image_size: width -- {0}, height -- {1}'.format(person.rgb_image.width, person.rgb_image.height))
+            rospy.loginfo('Image_size: width -- {0}, height -- {1}'.format(person.views[0].image.width,
+                                                                           person.views[0].image.height))
+
+            rospy.loginfo('Publishing image and sleeping for 3s')
+            person_img_pub.publish(person.views[0].image)
+            rospy.sleep(3.)
+        rospy.loginfo('Client test done')
     except Exception as e:
         rospy.logerr('Failed to execute find people action: {}'.format(repr(e)))
-

From 0a7f1b5b9989be048bd693e101bd0f851a9040d8 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 17:54:42 +0100
Subject: [PATCH 142/264] [find_people] Fixed a bug in the pose extraction call

---
 .../ros/src/mdr_find_people/find_people.py    | 21 +++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index 56c1fa61b..068ab74a9 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -39,13 +39,22 @@ def detect(cloud_msg, detector, detector_device, class_annotations, detection_th
         bounding_boxes = ImageDetectorBase.prediction_to_bounding_boxes(detections)[0]
 
         rospy.loginfo('Extracting people detections...')
-        people_detections, bb2ds = FindPeople.filter_people(detections, bounding_boxes)
-
-        rospy.loginfo('Extracting person poses...')
-        poses = FindPeople.get_people_poses(cloud_msg, people_detections, bounding_boxes)
+        person_detections, person_bb2ds = FindPeople.filter_people(detections,
+                                                                   bounding_boxes)
+
+        if len(person_detections) == 1:
+            rospy.loginfo('Found one person')
+        else:
+            rospy.loginfo('Found {0} people'.format(len(person_detections)))
+
+        person_poses = []
+        if person_detections:
+            rospy.loginfo('Extracting person poses...')
+            person_poses = FindPeople.get_people_poses(cloud_msg,
+                                                       person_detections,
+                                                       person_bb2ds)
         rospy.loginfo('Person detection complete')
-
-        return people_detections, bb2ds, poses
+        return (person_detections, person_bb2ds, person_poses)
 
     @staticmethod
     def filter_people(predictions, bounding_boxes):

From b967f3797858c0378cb7118d7626c287fe7b8564 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 16 Nov 2020 18:24:11 +0100
Subject: [PATCH 143/264] [find_people/package.xml] Added Python package
 dependencies

---
 .../mdr_perception_actions/mdr_find_people/package.xml      | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
index 1ce968710..d2c8b54ef 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
@@ -27,4 +27,10 @@
   <run_depend>mas_perception_msgs</run_depend>
   <run_depend>mas_perception_libs</run_depend>
 
+  <run_depend>numpy</run_depend>
+  <run_depend>torch</run_depend>
+  <run_depend>scikit-learn</run_depend>
+  <run_depend>face_recognition</run_depend>
+  <run_depend>python-pcl</run_depend>
+
 </package>

From 4052976c121504a001cf96d440913a579ffa402e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 18 Nov 2020 18:27:26 +0100
Subject: [PATCH 144/264] [receive_object/action_states] Changed the sign of
 the mean for CUSUM

---
 .../ros/src/mdr_receive_object_action/action_states.py          | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
index a4900704c..d51b2a273 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/src/mdr_receive_object_action/action_states.py
@@ -218,7 +218,7 @@ def load_policy_params_from_file(self, filename):
 
     def detect_object_reception(self):
         mu_0 = 0.
-        mu_1 = -6.
+        mu_1 = 6.
         std = 1.
 
         pdf_0 = norm(mu_0, std)

From b50052ed0d3343b40fa0f7821be99d592972454b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 23 Nov 2020 21:06:57 +0100
Subject: [PATCH 145/264] [perceive_plane/action_states] Only create a
 recognition proxy if this feature is enabled

---
 .../ros/src/mdr_perceive_plane_action/action_states.py     | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py
index 429db5b11..291059f78 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py
@@ -21,9 +21,12 @@ def __init__(self, detection_service_proxy,
                  max_recovery_attempts=1):
         super(PerceivePlaneSM, self).__init__('PerceivePlane', [], max_recovery_attempts)
         self._detector = PlaneDetector(detection_service_proxy)
-        self._recog_service_proxy = RecognizeImageServiceProxy(recog_service_name, recog_model_name,
-                                                               preprocess_input_module)
         self._classify_object = classify_object
+        self._recog_service_proxy = None
+        if self._classify_object:
+            self._recog_service_proxy = RecognizeImageServiceProxy(recog_service_name,
+                                                                   recog_model_name,
+                                                                   preprocess_input_module)
         self._timeout_duration = timeout_duration
         self._target_frame = target_frame
         self._detecting_done = False

From 404c9467355999ace6b4777bc2d61767b92f0668 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 23 Nov 2020 21:08:21 +0100
Subject: [PATCH 146/264] [perceive_plane] Added a config dir and an object
 detector config file

---
 .../config/object_detection_kwargs.yml                         | 3 +++
 1 file changed, 3 insertions(+)
 create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/config/object_detection_kwargs.yml

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/config/object_detection_kwargs.yml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/config/object_detection_kwargs.yml
new file mode 100644
index 000000000..65d014200
--- /dev/null
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/config/object_detection_kwargs.yml
@@ -0,0 +1,3 @@
+detector_module: torchvision.models.detection
+detector_instantiator: fasterrcnn_resnet50_fpn
+detection_threshold: 0.8

From 6cffdbdc70d2ad148c022a5385f4ef2b409848a6 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 23 Nov 2020 21:09:58 +0100
Subject: [PATCH 147/264] [perceive_plane] Launch a plane detector within the
 action launcher

The plane detector uses a PyTorch-based object detector (config
parameters specified in mdr_perceive_plane_action/config)
---
 .../ros/launch/perceive_plane.launch             | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane.launch
index b95cd74b0..d415bc62e 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane.launch
@@ -9,6 +9,12 @@
     <arg name="classify_object" default="false"/>
     <arg name="head_controller_pkg_name" default="mdr_head_controller"/>
 
+    <arg name="cloud_topic" default="" />
+    <arg name="object_detection_module" default="mas_perception_libs" />
+    <arg name="object_detection_class" default="TorchImageDetector" />
+    <arg name="object_detection_class_annotations" default="$(find mas_perception_libs)/models/coco_classes.yml" />
+    <arg name="object_detection_kwargs_file" default="$(find mdr_perceive_plane_action)/config/object_detection_kwargs.yml" />
+
     <node pkg="mdr_perceive_plane_action" type="perceive_plane_action" name="perceive_plane_server" output="screen"
           ns="mdr_actions">
         <param name="target_frame" type="string" value="$(arg target_frame)" />
@@ -20,6 +26,16 @@
         <param name="head_controller_pkg_name" type="string" value="$(arg head_controller_pkg_name)" />
     </node>
 
+    <include file="$(find mas_perception_libs)/ros/launch/plane_detection.launch" >
+        <arg name="cloud_topic" value="$(arg cloud_topic)" />
+        <arg name="target_frame" value="$(arg target_frame)" />
+        <arg name="action_name" value="$(arg detection_action_name)" />
+        <arg name="detection_module" value="$(arg object_detection_module)" />
+        <arg name="detection_class" value="$(arg object_detection_class)" />
+        <arg name="class_annotations" value="$(arg object_detection_class_annotations)" />
+        <arg name="kwargs_file" value="$(arg object_detection_kwargs_file)" />
+    </include>
+
     <include file="$(find mdr_object_recognition)/ros/launch/object_recognition.launch">
         <arg name="service_name" value="$(arg recognition_service_name)"/>
     </include>

From 87d9c580ee609da257a6dfc07b9e85902a0b5fee Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 27 Nov 2020 16:48:35 +0100
Subject: [PATCH 148/264] [mas-domestic-devel.rosinstall] Added
 demonstrated_trajectory_recorder to the list of dependencies

---
 mas-domestic-devel.rosinstall | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/mas-domestic-devel.rosinstall b/mas-domestic-devel.rosinstall
index e906adf02..aa9c2e363 100644
--- a/mas-domestic-devel.rosinstall
+++ b/mas-domestic-devel.rosinstall
@@ -82,6 +82,11 @@
     uri: https://github.com/ros/kdl_parser.git
     version: kinetic-devel
 
+- git:
+    local-name: demonstrated_trajectory_recorder
+    uri: https://github.com/b-it-bots/demonstrated_trajectory_recorder.git
+    version: master
+
 - git:
     local-name: ros_dmp
     uri: https://github.com/b-it-bots/ros_dmp.git

From 7594ea6c6bdf4efc5bf8be7bc99b8d93c07343ce Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 18 Nov 2020 22:33:45 +0100
Subject: [PATCH 149/264] [robocup_tasks] Added a skeleton package for a
 METRICS drink transportation task

---
 .../CMakeLists.txt                            | 37 +++++++++++++++++++
 .../package.xml                               | 37 +++++++++++++++++++
 .../launch/receive_and_transport_drink.launch | 22 +++++++++++
 .../__init__.py                               |  0
 .../mdr_receive_and_transport_drink/setup.py  | 11 ++++++
 5 files changed, 107 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/launch/receive_and_transport_drink.launch
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/src/mdr_receive_and_transport_drink/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/CMakeLists.txt
new file mode 100644
index 000000000..4d3fec887
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_receive_and_transport_drink)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roslint
+  mas_execution_manager
+  mdr_find_people
+  mdr_move_base_action
+  mdr_receive_object_action
+  mdr_perceive_plane_action
+  mdr_place_action
+  mdr_navigation_behaviours
+  mdr_perception_behaviours
+  mdr_manipulation_behaviours
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mas_execution_manager
+    mdr_find_people
+    mdr_move_base_action
+    mdr_receive_object_action
+    mdr_perceive_plane_action
+    mdr_place_action
+    mdr_navigation_behaviours
+    mdr_perception_behaviours
+    mdr_manipulation_behaviours
+)
+
+catkin_python_setup()
+roslint_python()
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/package.xml
new file mode 100644
index 000000000..e7683e279
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/package.xml
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mdr_receive_and_transport_drink</name>
+  <version>1.0.0</version>
+
+  <description>State machine and launch files for a METRICS "Receive and Transport Drink" scenario</description>
+
+  <license>GPLv3</license>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>mdr_find_people</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+  <build_depend>mdr_move_base_action</build_depend>
+  <build_depend>mdr_receive_object_action</build_depend>
+  <build_depend>mdr_perceive_plane_action</build_depend>
+  <build_depend>mdr_place_action</build_depend>
+  <build_depend>mdr_navigation_behaviours</build_depend>
+  <build_depend>mdr_perception_behaviours</build_depend>
+  <build_depend>mdr_manipulation_behaviours</build_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>mas_execution_manager</exec_depend>
+  <exec_depend>mdr_find_people</exec_depend>
+  <exec_depend>mdr_move_base_action</exec_depend>
+  <exec_depend>mdr_receive_object_action</exec_depend>
+  <exec_depend>mdr_perceive_plane_action</exec_depend>
+  <exec_depend>mdr_place_action</exec_depend>
+  <exec_depend>mdr_navigation_behaviours</exec_depend>
+  <exec_depend>mdr_perception_behaviours</exec_depend>
+  <exec_depend>mdr_manipulation_behaviours</exec_depend>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/launch/receive_and_transport_drink.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/launch/receive_and_transport_drink.launch
new file mode 100644
index 000000000..0b4360f41
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/launch/receive_and_transport_drink.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)" />
+    <include file="$(find mdr_rosplan_interface)/ros/launch/rosplan.launch">
+        <arg name="data_path" value="$(find mdr_receive_and_transport_drink)/config/" />
+        <arg name="domain_path" value="$(find mdr_rosplan_interface)/config/default_domain.pddl" />
+        <arg name="problem_path" value="$(find mdr_receive_and_transport_drink)/config/problem.pddl" />
+        <arg name="strl_file_path" value="$(find mdr_receive_and_transport_drink)/config/plan.strl" />
+    </include>
+
+    <include file="$(find mdr_find_people)/ros/launch/find_people.launch" />
+    <include file="$(find mdr_receive_object_action)/ros/launch/receive_object.launch" />
+    <include file="$(find mdr_find_people)/ros/launch/find_people_client.launch" />
+    <include file="$(find mdr_move_base_action)/ros/launch/move_base_client.launch" />
+    <include file="$(find mdr_perceive_plane_action)/ros/launch/perceive_plane_client.launch" />
+    <include file="$(find mdr_place_action)/ros/launch/place_client.launch" />
+
+    <!-- State machine -->
+    <node pkg="mas_execution_manager" type="state_machine_creator" name="receive_and_transport_drink" output="screen">
+        <param name="sm_config_file" type="str" value="$(find mdr_receive_and_transport_drink)/config/receive_and_transport_drink_sm.yaml" />
+    </node>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/src/mdr_receive_and_transport_drink/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/ros/src/mdr_receive_and_transport_drink/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/setup.py
new file mode 100644
index 000000000..8d9d56c75
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_receive_and_transport_drink'],
+    package_dir={'mdr_receive_and_transport_drink': 'ros/src/mdr_receive_and_transport_drink'}
+)
+
+setup(**d)

From ad45ff54672b65c6f5ac3e2665ff6e90106819cf Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 12:58:10 +0100
Subject: [PATCH 150/264] [find_people/client] Added optional person
 recognition and msg removal from the KB

---
 .../ros/launch/find_people_client.launch      |  3 +-
 .../ros/scripts/find_people_client            | 54 ++++++++++++++++---
 2 files changed, 48 insertions(+), 9 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
index 95a3d0c33..e95771c8c 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
@@ -5,12 +5,13 @@
     <arg name="action_timeout" default="120" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch"/>
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback"/>
+    <arg name="clear_person_memory" default="true"/>
 
     <node pkg="mdr_find_people" type="find_people_client" name="find_people_client" output="screen">
         <param name="action_name" type="str" value="$(arg action_name)" />
         <param name="server_name" type="str" value="$(arg server_name)" />
         <param name="action_timeout" type="double" value="$(arg action_timeout)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
-        <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
+        <remap from="clear_person_memory" to="$(arg clear_person_memory)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
index b26476797..a76c247f0 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
@@ -2,13 +2,16 @@
 import rospy
 import actionlib
 
+from mas_perception_msgs.msg import Person
+
 from mdr_rosplan_interface.action_client_base import ActionClientBase
 from mdr_find_people.msg import FindPeopleAction, FindPeopleGoal
 
 class FindPeopleClient(ActionClientBase):
     def __init__(self):
         super(FindPeopleClient, self).__init__()
-
+        self.clear_person_memory = rospy.get_param('~clear_person_memory', False)
+        self.perform_recognition = False
         while not rospy.is_shutdown():
             rospy.sleep(0.1)
 
@@ -43,21 +46,56 @@ class FindPeopleClient(ActionClientBase):
         for param in rosplan_action_msg.parameters:
             if param.key == 'bot':
                 self.robot_name = param.value
+            elif param.key == 'perform_recognition':
+                self.perform_recognition = param.value
         return goal
 
     def update_knowledge_base(self, person_list):
-        '''Updates the knowledge base with the following facts:
-        * a list of people that the robot sees
+        '''Updates the knowledge base with a list of detected people.
+        Removes all existing people in the knowledge base before adding
+        the new people if "self.clear_person_memory" is set to True.
+        Performs person recognition if set.perform_recognition is set to True.
+
+        Keyword arguments:
+        person_list: List[Person] -- a list of mas_perception_msgs/Person msgs
+
         '''
+        if self.clear_person_memory:
+            self.__delete_people_from_kb()
+
         facts_to_add = []
-        objects_to_add = []
         for i, person in enumerate(person_list.persons):
             person_identifier = 'person_{0}'.format(i)
-            fact = ('unknown', [('person', person_identifier)])
-            facts_to_add.append(fact)
-            objects_to_add.append((person_identifier, person))
+            self.kb_interface.insert_obj_instance(person_identifier, person)
+
+            recognised_person = None
+            if self.perform_recognition:
+                recognised_person = self.recognise_person(person_identifier)
+
+            if recognised_person is None:
+                rospy.loginfo('{0} not recognised'.format(person_identifier))
+                fact = ('unknown', [('person', person_identifier)])
+                facts_to_add.append(fact)
+            else:
+                rospy.loginfo('Recognised {0}'.format(recognised_person.name))
+                fact = ('known', [('person', recognised_person.name)])
+                person.name = recognised_person.name
+
+                # we reinsert the Person object with the name of the known person
+                # (and delete the originally added unknown person)
+                self.kb_interface.remove_obj_instance(person_identifier, Person._type)
+                self.kb_interface.insert_obj_instance(recognised_person.name, person)
+
         self.kb_interface.insert_facts(facts_to_add)
-        self.kb_interface.insert_objects(objects_to_add)
+
+    def __delete_people_from_kb(self):
+        '''Removes previously seen people from the knowledge base.
+        '''
+        people_to_remove = []
+        people = self.kb_interface.get_all_objects(Person._type)
+        for person in people:
+            people_to_remove.append((person.name, Person._type))
+        self.kb_interface.remove_objects(people_to_remove)
 
 if __name__ == '__main__':
     rospy.init_node('mdr_find_people_client')

From 9e6087cafffca526d7b2e3a22dba699a03587cba Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 13:50:58 +0100
Subject: [PATCH 151/264] [perception_behaviours/find_people] A person
 recognition arg is now passed to the action client

---
 .../ros/src/mdr_perception_behaviours/find_people.py        | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
index 3c5933ac0..c8ca1937d 100644
--- a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
+++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
@@ -13,6 +13,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.state_name = kwargs.get('state_name', 'find_people')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.debug = kwargs.get('debug', False)
+        self.perform_recognition = kwargs.get('perform_recognition', False)
         self.retry_count = 0
         self.timeout = 120.
 
@@ -53,4 +54,9 @@ def get_dispatch_msg(self):
         arg_msg.value = self.robot_name
         dispatch_msg.parameters.append(arg_msg)
 
+        arg_msg = diag_msgs.KeyValue()
+        arg_msg.key = 'perform_recognition'
+        arg_msg.value = self.perform_recognition
+        dispatch_msg.parameters.append(arg_msg)
+
         return dispatch_msg

From f7489be144bc9c2c3b58bddd4ad17afbeee4c821 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 14:09:16 +0100
Subject: [PATCH 152/264] [hri_behaviours] Added an object reception behaviour

This behaviour
(i) estimates the posture of a person and
(ii) calls the 'receive_object' action for receiving an object
---
 .../src/mdr_hri_behaviours/receive_object.py  | 78 +++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
new file mode 100644
index 000000000..a9806a6d1
--- /dev/null
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -0,0 +1,78 @@
+import rospy
+import actionlib
+
+from mas_perception_msgs.msg import Person
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
+
+class ReceiveObject(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'receive_object',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed',
+                                             'failed_after_retrying',
+                                             'person_not_found'],
+                                   input_keys=['person_name'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'get_person_pose')
+        self.action_server = kwargs.get('action_server', 'receive_object_server')
+        self.timeout = kwargs.get('timeout', 120.)
+        self.context_aware = kwargs.get('context_aware', True)
+        self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})
+        self.person_name = kwargs.get('person_name', None)
+
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+        self.client = actionlib.SimpleActionClient(self.action_server, ReceiveObjectAction)
+        self.client.wait_for_server(rospy.Duration(10.))
+
+    def execute(self, userdata):
+        person_to_look_for = self.person_name
+        if not person_to_look_for:
+            person_to_look_for = userdata.person_name
+
+        person_msg = self.kb_interface.get_obj_instance(person_to_look_for, Person._type)
+        if person_msg is None:
+            return 'person_not_found'
+
+        posture = self.get_posture(person_msg)
+        reception_action_result = self.receive_object(person_msg, posture)
+
+        if reception_action_result and reception_action_result.success:
+            rospy.loginfo('Received object')
+            return 'succeeded'
+        else:
+            rospy.logerr('Could not receive object')
+            self.say('Object not received')
+            if self.retry_count == self.number_of_retries:
+                self.say('Aborting operation')
+                return 'failed_after_retrying'
+            self.retry_count += 1
+            return 'failed'
+        return 'succeeded'
+
+    def get_posture(self, person_msg):
+        person_image_bb_width = float(person_msg.views[0].image.width)
+        person_image_bb_height = float(person_msg.views[0].image.height)
+        bb_height_width_ratio = person_image_bb_height / person_image_bb_width
+
+        person_posture = None
+        for posture, ratio_ranges in self.posture_ratio_ranges.items():
+            if ratio_ranges[0] <= bb_height_width_ratio <= ratio_ranges[1]:
+                person_posture = posture
+        return person_posture
+
+    def receive_object(self, person_msg, posture):
+        goal = ReceiveObjectGoal()
+        goal.posture_type = posture
+        goal.context_aware = self.context_aware
+        goal.reception_detection = self.reception_detection
+        goal.person_pose = person_msg.pose
+
+        # calling the actionlib server and waiting for the execution to end
+        rospy.loginfo('[receive_object] Sending action lib goal to {0}'.format(self.action_server))
+        self.say('Receiving object from a {0} person'.format(posture))
+        self.client.send_goal(goal)
+        self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        result = self.client.get_result()
+        return result

From ff2dcc1b89a8d86b749bf6c8b9de9186f5b4cc13 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 14:20:41 +0100
Subject: [PATCH 153/264] [receive_and_transport_drink/config] Added an initial
 SM for the task

---
 .../receive_and_transport_drink_sm.yaml       | 140 ++++++++++++++++++
 1 file changed, 140 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml
new file mode 100644
index 000000000..85b693cec
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml
@@ -0,0 +1,140 @@
+# Description: Defines a state machine for a METRICS task of receiving a drink
+#              from a person and transporting it to a kitchen counter
+# Author: Alex Mitrevski
+# Email: aleksandar.mitrevski@h-brs.de
+sm_id: mdr_receive_and_transport_drink
+states: [ENTER_PERSON_LOCATION, FIND_PEOPLE, RECEIVE_OBJECT, GO_TO_KITCHEN, SCAN_COUNTER, PLACE_OBJECT]
+outcomes: [DONE, FAILED]
+state_descriptions:
+    - state:
+        name: ENTER_PERSON_LOCATION
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: FIND_PEOPLE
+            - transition:
+                name: failed
+                state: ENTER_PERSON_LOCATION
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: destination_locations
+                value: [living_room]
+    - state:
+        name: FIND_PEOPLE
+        state_module_name: mdr_perception_behaviours.find_people
+        state_class_name: FindPeople
+        transitions:
+            - transition:
+                name: succeeded
+                state: RECEIVE_OBJECT
+            - transition:
+                name: failed
+                state: FIND_PEOPLE
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: perform_recognition
+                value: True
+    - state:
+        name: RECEIVE_OBJECT
+        state_module_name: mdr_hri_behaviours.receive_object
+        state_class_name: ReceiveObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_KITCHEN
+            - transition:
+                name: failed
+                state: RECEIVE_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: person_name
+                value: alex
+            - argument:
+                name: posture_ratio_ranges
+                value:
+                    standing: [1.65, 5.0]
+                    seated: [0.6, 1.65]
+                    lying: [0.0, 0.6]
+    - state:
+        name: GO_TO_KITCHEN
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: SCAN_COUNTER
+            - transition:
+                name: failed
+                state: GO_TO_KITCHEN
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: destination_locations
+                value: [kitchen]
+    - state:
+        name: SCAN_COUNTER
+        state_module_name: mdr_perception_behaviours.perceive_planes
+        state_class_name: PerceivePlanes
+        transitions:
+            - transition:
+                name: succeeded
+                state: PLACE_OBJECT
+            - transition:
+                name: failed
+                state: SCAN_COUNTER
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: plane_prefix
+                value: kitchen_counter
+    - state:
+        name: PLACE_OBJECT
+        state_module_name: mdr_manipulation_behaviours.place_based_on_category
+        state_class_name: PlaceBasedOnCategory
+        transitions:
+            - transition:
+                name: succeeded
+                state: DONE
+            - transition:
+                name: failed
+                state: PLACE_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: FAILED
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+            - argument:
+                name: placing_surface_prefix
+                value: kitchen_counter

From 2d7f704c2c497525e03046b615c79c2aa1c5092e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 14:59:20 +0100
Subject: [PATCH 154/264] [gripper_controller/base] Added a 'get_opening_angle'
 function

---
 .../ros/src/mdr_gripper_controller/gripper_controller_base.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
index ebe3e3375..ced41fefb 100644
--- a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
+++ b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
@@ -24,3 +24,7 @@ def init_grasp_verification(self):
     def verify_grasp(self):
         rospy.loginfo('[verify_grasp] Ignoring request')
         raise NotImplementedError()
+
+    def get_opening_angle(self):
+        rospy.loginfo('[get_opening_angle] Ignoring request')
+        raise NotImplementedError()

From 68a8f533c494f7624aee35d82e5018844ce1a7de Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 19 Nov 2020 16:09:54 +0100
Subject: [PATCH 155/264] [gripper_controller/base] Added a 'get_gripper_pose'
 function

---
 .../ros/src/mdr_gripper_controller/gripper_controller_base.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
index ced41fefb..3f0fd1d5b 100644
--- a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
+++ b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py
@@ -28,3 +28,7 @@ def verify_grasp(self):
     def get_opening_angle(self):
         rospy.loginfo('[get_opening_angle] Ignoring request')
         raise NotImplementedError()
+
+    def get_gripper_pose(self, frame_name):
+        rospy.loginfo('[get_gripper_pose] Ignoring request')
+        raise NotImplementedError()

From 7e9a4f6b54dfca89c80d78b80d062c31fbca574b Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 19 Nov 2020 16:14:01 +0100
Subject: [PATCH 156/264] [hri_behaviours/receive_object] Add object size
 estimation

---
 .../src/mdr_hri_behaviours/receive_object.py  | 52 +++++++++++++++----
 1 file changed, 42 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index a9806a6d1..2818e1937 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -1,31 +1,39 @@
+from importlib import import_module
+
 import rospy
 import actionlib
 
-from mas_perception_msgs.msg import Person
+from mas_perception_msgs.msg import Person, Object
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
-from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
+from mdr__receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
 
 class ReceiveObject(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
-        ScenarioStateBase.__init__(self, 'receive_object',
+        ScenarioStateBase.__init__(self, '_receive_object',
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'failed',
                                              'failed_after_retrying',
                                              'person_not_found'],
-                                   input_keys=['person_name'])
+                                   input_keys=['person_name', 'object_to_transport'],
+                                   output_keys=['grasped_object'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'get_person_pose')
-        self.action_server = kwargs.get('action_server', 'receive_object_server')
+        self.action_server = kwargs.get('action_server', '_receive_object_server')
         self.timeout = kwargs.get('timeout', 120.)
         self.context_aware = kwargs.get('context_aware', True)
         self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})
         self.person_name = kwargs.get('person_name', None)
+        self.object_to_transport = kwargs.get('object_to_transport', None)
+        self.gripper_controller_pkg = kwargs.get('gripper_controller_pkg', 'mdr_gripper_controller')
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.retry_count = 0
         self.client = actionlib.SimpleActionClient(self.action_server, ReceiveObjectAction)
         self.client.wait_for_server(rospy.Duration(10.))
 
+        gripper_controller_class = getattr(import_module(self.gripper_controller_pkg), 'GripperController')
+        self.gripper_controller = gripper_controller_class()
+
     def execute(self, userdata):
         person_to_look_for = self.person_name
         if not person_to_look_for:
@@ -35,11 +43,19 @@ def execute(self, userdata):
         if person_msg is None:
             return 'person_not_found'
 
-        posture = self.get_posture(person_msg)
-        reception_action_result = self.receive_object(person_msg, posture)
+        posture = self._get_posture(person_msg)
+        reception_action_result = self._receive_object(person_msg, posture)
 
         if reception_action_result and reception_action_result.success:
             rospy.loginfo('Received object')
+
+            object_to_transport = self.object_to_transport
+            if not object_to_transport:
+                object_to_transport = userdata.object_to_transport
+            userdata.grasped_object = object_to_transport
+
+            estimated_size_m = self._get_estimated_object_size(self.gripper_controller.get_opening_angle())
+            self._insert_object_in_kb(object_to_transport, estimated_size_m)
             return 'succeeded'
         else:
             rospy.logerr('Could not receive object')
@@ -51,7 +67,23 @@ def execute(self, userdata):
             return 'failed'
         return 'succeeded'
 
-    def get_posture(self, person_msg):
+    def _get_estimated_object_size(self, angle):
+        '''Get estimate of object size from gripper opening angle, assuming
+        linear relation. The function parameters have been empirically determined,
+        with different domestic objects including a water bottle, a mug, an eyedropper,
+        a glass bottle.
+        '''
+        return (angle + 0.141) * 0.120
+
+    def _insert_object_in_kb(self, name, size):
+        obj_msg = Object()
+        obj_msg.name = name
+        obj_msg.bounding_box.dimensions.x = size
+        obj_msg.bounding_box.dimensions.y = size
+
+        self.kb_interface.insert_obj_instance(name, obj)
+
+    def _get_posture(self, person_msg):
         person_image_bb_width = float(person_msg.views[0].image.width)
         person_image_bb_height = float(person_msg.views[0].image.height)
         bb_height_width_ratio = person_image_bb_height / person_image_bb_width
@@ -62,7 +94,7 @@ def get_posture(self, person_msg):
                 person_posture = posture
         return person_posture
 
-    def receive_object(self, person_msg, posture):
+    def _receive_object(self, person_msg, posture):
         goal = ReceiveObjectGoal()
         goal.posture_type = posture
         goal.context_aware = self.context_aware
@@ -70,7 +102,7 @@ def receive_object(self, person_msg, posture):
         goal.person_pose = person_msg.pose
 
         # calling the actionlib server and waiting for the execution to end
-        rospy.loginfo('[receive_object] Sending action lib goal to {0}'.format(self.action_server))
+        rospy.loginfo('[_receive_object] Sending action lib goal to {0}'.format(self.action_server))
         self.say('Receiving object from a {0} person'.format(posture))
         self.client.send_goal(goal)
         self.client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))

From d4cc8f2a0a3c3f83867975197b5f6bb0b1bd444a Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Thu, 19 Nov 2020 16:35:38 +0100
Subject: [PATCH 157/264] [hri_behaviours/receive_object] Set position of
 grasped object to position of gripper

---
 .../ros/src/mdr_hri_behaviours/receive_object.py          | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index 2818e1937..1f280a578 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -5,7 +5,7 @@
 
 from mas_perception_msgs.msg import Person, Object
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
-from mdr__receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
+from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
 
 class ReceiveObject(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
@@ -78,6 +78,12 @@ def _get_estimated_object_size(self, angle):
     def _insert_object_in_kb(self, name, size):
         obj_msg = Object()
         obj_msg.name = name
+
+        gripper_pose = self.gripper_controller.get_gripper_pose('base_link')
+        obj_msg.pose = gripper_pose
+        obj_msg.bounding_box.center.x = gripper_pose.pose.position.x
+        obj_msg.bounding_box.center.y = gripper_pose.pose.position.y
+        obj_msg.bounding_box.center.z = gripper_pose.pose.position.z
         obj_msg.bounding_box.dimensions.x = size
         obj_msg.bounding_box.dimensions.y = size
 

From 53f51cde46f0271f0f8a76de787109aa7b0329c3 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 20 Nov 2020 15:38:43 +0100
Subject: [PATCH 158/264] [receive_and_transport_drink] Added a missing
 transition to the SM config

---
 .../ros/src/mdr_hri_behaviours/receive_object.py            | 1 +
 .../config/receive_and_transport_drink_sm.yaml              | 6 ++++++
 2 files changed, 7 insertions(+)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index 1f280a578..ef7210bae 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -41,6 +41,7 @@ def execute(self, userdata):
 
         person_msg = self.kb_interface.get_obj_instance(person_to_look_for, Person._type)
         if person_msg is None:
+            self.say('I can\'t see {0}'.format(person_to_look_for))
             return 'person_not_found'
 
         posture = self._get_posture(person_msg)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml
index 85b693cec..32715d9ad 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_receive_and_transport_drink/config/receive_and_transport_drink_sm.yaml
@@ -62,6 +62,9 @@ state_descriptions:
             - transition:
                 name: failed_after_retrying
                 state: FAILED
+            - transition:
+                name: person_not_found
+                state: FIND_PEOPLE
         arguments:
             - argument:
                 name: number_of_retries
@@ -69,6 +72,9 @@ state_descriptions:
             - argument:
                 name: person_name
                 value: alex
+            - argument:
+                name: object_to_transport
+                value: glass_of_water
             - argument:
                 name: posture_ratio_ranges
                 value:

From 2c56b52979ea18996fe378f0614d310dc08095f3 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 20 Nov 2020 15:45:01 +0100
Subject: [PATCH 159/264] [hri_behaviours/receive_object] Fixed incorrect
 default action server value

---
 .../ros/src/mdr_hri_behaviours/receive_object.py                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index ef7210bae..16b560c95 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -18,7 +18,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    output_keys=['grasped_object'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'get_person_pose')
-        self.action_server = kwargs.get('action_server', '_receive_object_server')
+        self.action_server = kwargs.get('action_server', 'receive_object_server')
         self.timeout = kwargs.get('timeout', 120.)
         self.context_aware = kwargs.get('context_aware', True)
         self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})

From 34955e18f5354a801b779b3c441c76b205cd150b Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 20 Nov 2020 16:21:07 +0100
Subject: [PATCH 160/264] [hri_behaviours/receive_object] Fix incorrect action
 parameter name

---
 .../ros/scripts/receive_object_action_client_test           | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
index 429b3c953..88cea1299 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_receive_object_action/ros/scripts/receive_object_action_client_test
@@ -7,12 +7,12 @@ from mdr_receive_object_action.msg import ReceiveObjectAction, ReceiveObjectGoal
 
 if __name__ == '__main__':
     posture_contexts = ['standing', 'seated', 'lying']
-    release_detection = [0, 1]
+    reception_detection = [0, 1]
 
     parser = argparse.ArgumentParser(description='help text')
     parser.add_argument("--posture", "-p", type=str, default='seated',
                         help="posture context parameters. Choose from 'standing', 'seated', or 'lying'.")
-    parser.add_argument("--release_detection", "-r", type=bool, default=1,
+    parser.add_argument("--reception_detection", "-r", type=bool, default=1,
                         help="list of context parameters. E.g.: 0")
     args = parser.parse_args()
 
@@ -25,7 +25,7 @@ if __name__ == '__main__':
 
     rospy.loginfo('Setting goal for a %s person...', args.posture)
     goal.posture_type = args.posture
-    goal.release_detection = args.release_detection
+    goal.reception_detection = args.reception_detection
     goal.person_pose.header.frame_id = "base_link"
 
     timeout = 15.0

From a5678da6cdb15b46cc6b9423a6c47e5748d6e2cc Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 20 Nov 2020 17:37:30 +0100
Subject: [PATCH 161/264] Fixed various errors related to receive ans transport
 drink task

---
 .../mdr_find_people/ros/launch/find_people_client.launch       | 1 +
 .../mdr_find_people/ros/scripts/find_people_client             | 2 +-
 .../ros/src/mdr_hri_behaviours/receive_object.py               | 3 ++-
 .../ros/src/mdr_perception_behaviours/find_people.py           | 2 +-
 4 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
index e95771c8c..77b565499 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people_client.launch
@@ -12,6 +12,7 @@
         <param name="server_name" type="str" value="$(arg server_name)" />
         <param name="action_timeout" type="double" value="$(arg action_timeout)" />
         <remap from="action_dispatch_topic" to="$(arg action_dispatch_topic)" />
+        <remap from="action_feedback_topic" to="$(arg action_feedback_topic)" />
         <remap from="clear_person_memory" to="$(arg clear_person_memory)" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
index a76c247f0..2b1bde9c1 100755
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client
@@ -47,7 +47,7 @@ class FindPeopleClient(ActionClientBase):
             if param.key == 'bot':
                 self.robot_name = param.value
             elif param.key == 'perform_recognition':
-                self.perform_recognition = param.value
+                self.perform_recognition = bool(param.value)
         return goal
 
     def update_knowledge_base(self, person_list):
diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index 16b560c95..26eb6b208 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -21,6 +21,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.action_server = kwargs.get('action_server', 'receive_object_server')
         self.timeout = kwargs.get('timeout', 120.)
         self.context_aware = kwargs.get('context_aware', True)
+        self.reception_detection = kwargs.get('reception_detection', True)
         self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})
         self.person_name = kwargs.get('person_name', None)
         self.object_to_transport = kwargs.get('object_to_transport', None)
@@ -88,7 +89,7 @@ def _insert_object_in_kb(self, name, size):
         obj_msg.bounding_box.dimensions.x = size
         obj_msg.bounding_box.dimensions.y = size
 
-        self.kb_interface.insert_obj_instance(name, obj)
+        self.kb_interface.insert_obj_instance(name, obj_msg)
 
     def _get_posture(self, person_msg):
         person_image_bb_width = float(person_msg.views[0].image.width)
diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
index c8ca1937d..8831f4e5a 100644
--- a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
+++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py
@@ -56,7 +56,7 @@ def get_dispatch_msg(self):
 
         arg_msg = diag_msgs.KeyValue()
         arg_msg.key = 'perform_recognition'
-        arg_msg.value = self.perform_recognition
+        arg_msg.value = str(self.perform_recognition)
         dispatch_msg.parameters.append(arg_msg)
 
         return dispatch_msg

From 9a9affe6ab1a384a6d8d43cbf565d279a5f15c29 Mon Sep 17 00:00:00 2001
From: AhmedFaisal95 <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 20 Nov 2020 17:54:30 +0100
Subject: [PATCH 162/264] [mdr_hri_behaviours/receive_object] Include object
 category for object to transport

---
 .../ros/src/mdr_hri_behaviours/receive_object.py       | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index 26eb6b208..82ee64664 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -25,6 +25,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})
         self.person_name = kwargs.get('person_name', None)
         self.object_to_transport = kwargs.get('object_to_transport', None)
+        self.object_category = kwargs.get('object_category', None)
         self.gripper_controller_pkg = kwargs.get('gripper_controller_pkg', 'mdr_gripper_controller')
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
@@ -56,8 +57,12 @@ def execute(self, userdata):
                 object_to_transport = userdata.object_to_transport
             userdata.grasped_object = object_to_transport
 
+            object_category = self.object_category
+            if not object_to_transport:
+                object_to_category = userdata.object_category
+
             estimated_size_m = self._get_estimated_object_size(self.gripper_controller.get_opening_angle())
-            self._insert_object_in_kb(object_to_transport, estimated_size_m)
+            self._insert_object_in_kb(object_to_transport, object_category, estimated_size_m)
             return 'succeeded'
         else:
             rospy.logerr('Could not receive object')
@@ -77,9 +82,10 @@ def _get_estimated_object_size(self, angle):
         '''
         return (angle + 0.141) * 0.120
 
-    def _insert_object_in_kb(self, name, size):
+    def _insert_object_in_kb(self, name, category, size):
         obj_msg = Object()
         obj_msg.name = name
+        obj_msg.category = category
 
         gripper_pose = self.gripper_controller.get_gripper_pose('base_link')
         obj_msg.pose = gripper_pose

From 82c01ad0c1738b9ecd3544b7f7347104085b0a6b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 09:52:27 +0100
Subject: [PATCH 163/264] [*.rosinstall] Added mas_tools to the dependency list

---
 mas-domestic-devel.rosinstall | 5 +++++
 mas-domestic.rosinstall       | 5 +++++
 2 files changed, 10 insertions(+)

diff --git a/mas-domestic-devel.rosinstall b/mas-domestic-devel.rosinstall
index aa9c2e363..735732877 100644
--- a/mas-domestic-devel.rosinstall
+++ b/mas-domestic-devel.rosinstall
@@ -91,3 +91,8 @@
     local-name: ros_dmp
     uri: https://github.com/b-it-bots/ros_dmp.git
     version: master
+
+- git:
+    local-name: mas_tools
+    uri: https://github.com/b-it-bots/mas_tools.git
+    version: devel
diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall
index 0a416c874..1faeb2bfd 100644
--- a/mas-domestic.rosinstall
+++ b/mas-domestic.rosinstall
@@ -86,3 +86,8 @@
     local-name: ros_dmp
     uri: https://github.com/b-it-bots/ros_dmp.git
     version: master
+
+- git:
+    local-name: mas_tools
+    uri: https://github.com/b-it-bots/mas_tools.git
+    version: master

From 8903ebd8a6387b4ea2d79fa7cadb051bda1dcf4b Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Wed, 2 Dec 2020 15:58:44 +0100
Subject: [PATCH 164/264] Add a new scenario package and launch file for the
 wrs-2020-challenge

---
 .../mdr_wrs_2020_challenge/CMakeLists.txt     | 34 +++++++++++++++++
 .../mdr_wrs_2020_challenge/package.xml        | 38 +++++++++++++++++++
 .../ros/launch/wrs_2020_challenge.launch      | 13 +++++++
 .../src/mdr_wrs_2020_challenge/__init__.py    |  0
 .../scenario_states/__init__.py               |  0
 .../mdr_wrs_2020_challenge/setup.py           | 11 ++++++
 6 files changed, 96 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/scenario_states/__init__.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt
new file mode 100644
index 000000000..69b12e439
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_wrs_2020_challenge)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roslint
+  std_msgs
+  geometry_msgs
+  topological_map_ros
+  mas_execution_manager
+  mas_knowledge_base
+  mdr_listen_action
+  mdr_speech_matching
+)
+
+catkin_python_setup()
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    std_msgs
+    geometry_msgs
+    topological_map_ros
+    mas_execution_manager
+    mas_knowledge_base
+    mdr_listen_action
+    mdr_speech_matching
+)
+
+include_directories()
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml
new file mode 100644
index 000000000..42477bf21
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mdr_wrs_2020_challenge</name>
+  <version>0.0.0</version>
+  <description>State machine and launch files for the RoboCup Japan Open 2020 @Home Simulation League</description>
+  <license>GPLv3</license>
+
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <author email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</author>
+  <author email="sushant.chavan@smail.inf.h-brs.de">Sushant Chavan</author>
+
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
+  <build_depend>mdr_listen_action</build_depend>
+  <build_depend>topological_map_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>mdr_speech_matching</build_depend>
+  <build_depend>mas_knowledge_base</build_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>mas_execution_manager</exec_depend>
+  <exec_depend>mdr_listen_action</exec_depend>
+  <exec_depend>topological_map_ros</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>mdr_speech_matching</exec_depend>
+  <exec_depend>mas_knowledge_base</exec_depend>
+
+
+  <export>
+  </export>
+</package>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
new file mode 100644
index 000000000..8b05e19cc
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+    <param name="/move_base/global_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
+    <param name="/move_base/local_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
+
+    <include file="$(find mas_hsr_move_arm_action)/ros/launch/move_arm.launch"/>
+    <include file="$(find mas_hsr_move_arm_joints_action)/ros/launch/move_arm_joints.launch"/>
+    <include file="$(find mas_hsr_move_base_action)/ros/launch/move_base.launch"/>
+    <include file="$(find mas_hsr_move_forward_action)/ros/launch/move_forward.launch"/>
+    <include file="$(find mas_hsr_pickup_action)/ros/launch/pickup_action.launch"/>
+
+    <include file="$(find mas_perception_libs)/ros/launch/cloud_obstacle_detection_cpp_test.launch"/>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/scenario_states/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py
new file mode 100644
index 000000000..c88f0c1bb
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_wrs_2020_challenge'],
+    package_dir={'': 'ros/src'}
+)
+
+setup(**d)

From f9a34f72700545a785a3e6cb0058a7e365c2055d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 3 Dec 2020 19:52:09 +0100
Subject: [PATCH 165/264] [scenarios] mdr_wrs_2020_challenge -> mdr_wrs_tidy_up

---
 .../ros/launch/wrs_2020_challenge.launch      | 13 -------------
 .../CMakeLists.txt                            |  8 +-------
 .../package.xml                               | 19 ++++++-------------
 .../ros/launch/wrs_tidy_up.launch             | 13 +++++++++++++
 .../ros/src/mdr_wrs_tidy_up}/__init__.py      |  0
 .../scenario_states/__init__.py               |  0
 .../setup.py                                  |  4 ++--
 7 files changed, 22 insertions(+), 35 deletions(-)
 delete mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
 rename mdr_planning/mdr_scenarios/mdr_robocup_tasks/{mdr_wrs_2020_challenge => mdr_wrs_tidy_up}/CMakeLists.txt (72%)
 rename mdr_planning/mdr_scenarios/mdr_robocup_tasks/{mdr_wrs_2020_challenge => mdr_wrs_tidy_up}/package.xml (65%)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/launch/wrs_tidy_up.launch
 rename mdr_planning/mdr_scenarios/mdr_robocup_tasks/{mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge => mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up}/__init__.py (100%)
 rename mdr_planning/mdr_scenarios/mdr_robocup_tasks/{mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge => mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up}/scenario_states/__init__.py (100%)
 rename mdr_planning/mdr_scenarios/mdr_robocup_tasks/{mdr_wrs_2020_challenge => mdr_wrs_tidy_up}/setup.py (62%)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
deleted file mode 100644
index 8b05e19cc..000000000
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/launch/wrs_2020_challenge.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <param name="/move_base/global_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
-    <param name="/move_base/local_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
-
-    <include file="$(find mas_hsr_move_arm_action)/ros/launch/move_arm.launch"/>
-    <include file="$(find mas_hsr_move_arm_joints_action)/ros/launch/move_arm_joints.launch"/>
-    <include file="$(find mas_hsr_move_base_action)/ros/launch/move_base.launch"/>
-    <include file="$(find mas_hsr_move_forward_action)/ros/launch/move_forward.launch"/>
-    <include file="$(find mas_hsr_pickup_action)/ros/launch/pickup_action.launch"/>
-
-    <include file="$(find mas_perception_libs)/ros/launch/cloud_obstacle_detection_cpp_test.launch"/>
-</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
similarity index 72%
rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt
rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
index 69b12e439..1c6b7a67f 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/CMakeLists.txt
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
@@ -1,16 +1,13 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(mdr_wrs_2020_challenge)
+project(mdr_wrs_tidy_up)
 
 find_package(catkin REQUIRED COMPONENTS
   rospy
   roslint
   std_msgs
   geometry_msgs
-  topological_map_ros
   mas_execution_manager
   mas_knowledge_base
-  mdr_listen_action
-  mdr_speech_matching
 )
 
 catkin_python_setup()
@@ -20,11 +17,8 @@ catkin_package(
     rospy
     std_msgs
     geometry_msgs
-    topological_map_ros
     mas_execution_manager
     mas_knowledge_base
-    mdr_listen_action
-    mdr_speech_matching
 )
 
 include_directories()
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
similarity index 65%
rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml
rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
index 42477bf21..b9c9dd5b7 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/package.xml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>mdr_wrs_2020_challenge</name>
+  <name>mdr_wrs_tidy_up</name>
   <version>0.0.0</version>
-  <description>State machine and launch files for the RoboCup Japan Open 2020 @Home Simulation League</description>
+  <description>State machine and launch files for the Tidy Up task for the RoboCup Japan Open 2020 @Home Simulation League</description>
   <license>GPLv3</license>
 
   <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
@@ -15,24 +15,17 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>rospy</build_depend>
-  <build_depend>mas_execution_manager</build_depend>
-  <build_depend>mdr_listen_action</build_depend>
-  <build_depend>topological_map_ros</build_depend>
-  <build_depend>geometry_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>mdr_speech_matching</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>mas_execution_manager</build_depend>
   <build_depend>mas_knowledge_base</build_depend>
 
   <exec_depend>rospy</exec_depend>
-  <exec_depend>mas_execution_manager</exec_depend>
-  <exec_depend>mdr_listen_action</exec_depend>
-  <exec_depend>topological_map_ros</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
-  <exec_depend>mdr_speech_matching</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>mas_execution_manager</exec_depend>
   <exec_depend>mas_knowledge_base</exec_depend>
 
-
   <export>
   </export>
 </package>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/launch/wrs_tidy_up.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/launch/wrs_tidy_up.launch
new file mode 100644
index 000000000..6169d289c
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/launch/wrs_tidy_up.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+    <param name="/move_base/global_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
+    <param name="/move_base/local_costmap/obstacles/base_scan/topic" type="string" value="/mas_perception/merged_scan" />
+
+    <include file="$(find mdr_move_arm_action)/ros/launch/move_arm.launch"/>
+    <include file="$(find mdr_move_arm_joints_action)/ros/launch/move_arm_joints.launch"/>
+    <include file="$(find mdr_move_base_action)/ros/launch/move_base.launch"/>
+    <include file="$(find mdr_move_forward_action)/ros/launch/move_forward.launch"/>
+    <include file="$(find mdr_pickup_action)/ros/launch/pickup_action.launch"/>
+
+    <include file="$(find mas_perception_libs)/ros/launch/cloud_obstacle_detection_cpp_test.launch"/>
+</launch>
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/__init__.py
similarity index 100%
rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/__init__.py
rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/__init__.py
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/__init__.py
similarity index 100%
rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/ros/src/mdr_wrs_2020_challenge/scenario_states/__init__.py
rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/__init__.py
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/setup.py
similarity index 62%
rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py
rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/setup.py
index c88f0c1bb..6b1510302 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_2020_challenge/setup.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/setup.py
@@ -4,8 +4,8 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup(
-    packages=['mdr_wrs_2020_challenge'],
-    package_dir={'': 'ros/src'}
+    packages=['mdr_wrs_tidy_up'],
+    package_dir={'mdr_wrs_tidy_up': 'ros/src/mdr_wrs_tidy_up'}
 )
 
 setup(**d)

From d9930b7ba63fbd47e12556ed0abdfdc87c124047 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 15:11:52 +0100
Subject: [PATCH 166/264] [wrs_tidy_up] Added SM draft (without object delivery
 task)

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 206 ++++++++++++++++++
 1 file changed, 206 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
new file mode 100644
index 000000000..f3c80a51e
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -0,0 +1,206 @@
+# Description: Defines a state machine for a World Robot Summit Tidy Up task
+# Author: Alex Mitrevski
+# Email: aleksandar.mitrevski@h-brs.de
+sm_id: mdr_wrs_tidy_up
+states: [CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_FLOOR_OBJECTS, SELECT_OBJECT_FOR_PICKING, CLEAN_UP_OBJECT, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, CLEAN_UP_OBSTACLE]
+outcomes: [DONE, TIMEOUT, FAILED]
+state_descriptions:
+    # Task 1: Cleaning up objects
+    - state:
+        name: CHECK_TIDYING_DONE
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: CheckTidyingDone
+        transitions:
+            - transition:
+                name: room_not_clear
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: room_clear
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: floor_objects_cleared
+                value:
+                    table_inspection_area1: false
+                    table_inspection_area2: false
+                    table_inspection_area3: false
+            - argument:
+                name: table_objects_cleared
+                value:
+                    long_table_b: false
+                    tall_table: false
+    - state:
+        name: SELECT_SCANNING_POSE
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: SelectScanningPose
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_SCANNING_POSE
+            - transition:
+                name: failed
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: floor_scanning_poses
+                value: [table_inspection_area1, table_inspection_area2, table_inspection_area3]
+            - argument:
+                name: table_scanning_poses
+                value: [long_table_b, tall_table]
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: GO_TO_SCANNING_POSE
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: FIND_FLOOR_OBJECTS
+            - transition:
+                name: failed
+                state: GO_TO_SCANNING_POSE
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: FIND_FLOOR_OBJECTS
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: FindObjects
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_OBJECT_FOR_PICKING
+            - transition:
+                name: no_obstacles
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: failed
+                state: FIND_FLOOR_OBJECTS
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: SELECT_OBJECT_FOR_PICKING
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: SelectObjectForPicking
+        transitions:
+            - transition:
+                name: succeeded
+                state: CLEAN_UP_OBJECT
+            - transition:
+                name: failed
+                state: SELECT_OBJECT_FOR_PICKING
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: CLEAN_UP_OBJECT
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: CleanUpObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: failed
+                state: CLEAN_UP_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: CHECK_TIDYING_DONE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+
+    # Task 2a: Going to the second room
+    - state:
+        name: GO_TO_SECOND_ROOM_CORRIDOR
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: FIND_FLOOR_OBSTACLES
+            - transition:
+                name: failed
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+            - transition:
+                name: failed_after_retrying
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: FIND_FLOOR_OBSTACLES
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: FindObjects
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_OBSTACLE_FOR_PICKING
+            - transition:
+                name: no_obstacles
+                state: DONE
+            - transition:
+                name: failed
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+            - transition:
+                name: failed_after_retrying
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: SELECT_OBSTACLE_FOR_PICKING
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: SelectObjectForPicking
+        transitions:
+            - transition:
+                name: succeeded
+                state: CLEAN_UP_OBSTACLE
+            - transition:
+                name: failed
+                state: SELECT_OBSTACLE_FOR_PICKING
+            - transition:
+                name: failed_after_retrying
+                state: FIND_FLOOR_OBSTACLES
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: CLEAN_UP_OBSTACLE
+        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_class_name: CleanUpObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+            - transition:
+                name: failed
+                state: CLEAN_UP_OBSTACLE
+            - transition:
+                name: failed_after_retrying
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+
+    # TODO: Add states for Task 2b: Delivering an object to a person

From 58e9a6c3ae3d5554d0fed09ef92e64b6bcdc7abc Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 15:26:42 +0100
Subject: [PATCH 167/264] [wrs_tidy_up/config] Added a YAML file mapping from
 object classes to storage locations

For this particular case, the file acts as a "pseudo-ontology" (this is
essentially a model of the defaultStoringLocation relation in our
ontology).

We don't use the ontology for the scenario since the model
requires a reasoner, which is only supported by owlready2, which in turn
doesn't support Python 2 anymore (and we are still stuck with Python 2).
---
 .../config/storage_locations.yaml             | 66 +++++++++++++++++++
 1 file changed, 66 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml
new file mode 100644
index 000000000..9126e8bc6
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml
@@ -0,0 +1,66 @@
+001_chips_can: long_table_a
+002_master_chef_can: long_table_a
+003_cracker_box: long_table_a
+004_sugar_box: long_table_a
+005_tomato_soup_can: long_table_a
+006_mustard_bottle: long_table_a
+007_tuna_fish_can: long_table_a
+008_pudding_box: long_table_a
+009_gelatin_box: long_table_a
+010_potted_meat_can: long_table_a
+011_banana: long_table_a
+012_strawberry: long_table_a
+013_apple: long_table_a
+014_lemon: long_table_a
+015_peach: long_table_a
+016_pear: long_table_a
+017_orange: long_table_a
+018_plum: long_table_a
+019_pitcher_base: long_table_a
+021_bleach_cleanser: long_table_a
+022_windex_bottle: long_table_a
+023_wine_glass: long_table_a
+024_bowl: long_table_a
+025_mug: long_table_a
+026_sponge: long_table_a
+027-skillet: box
+028_skillet_lid: box
+029_plate: long_table_a
+030_fork: long_table_a
+031_spoon: long_table_a
+032_knife: long_table_a
+033_spatula: long_table_a
+035_power_drill: box
+036_wood_block: box
+037_scissors: box
+038_padlock: box
+039_key: box
+040_large_marker: box
+041_small_marker: box
+042_adjustable_wrench: box
+043_phillips_screwdriver: box
+044_flat_screwdriver: box
+046_plastic_bolt: box
+047_plastic_nut: box
+048_hammer: box
+049_small_clamp: box
+050_medium_clamp: box
+051_large_clamp: box
+052_extra_large_clamp: box
+053_mini_soccer_ball: box
+054_softball: box
+055_baseball: box
+056_tennis_ball: box
+057_racquetball: box
+058_golf_ball: box
+059_chain: box
+061_foam_brick: box
+062_dice: box
+063-a_marbles: box
+065-a_cups: box
+070-a_colored_wood_blocks: box
+071_nine_hole_peg_test: box
+072-a_toy_airplane: box
+073-a_lego_duplo: box
+076_timer: box
+077_rubiks_cube: box

From 2f734f2c7a2d1fcbf465ed172cf7bb50afc823d9 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 15:32:38 +0100
Subject: [PATCH 168/264] [environments] Added an environment for WRS2020

The navigation goals file currently only includes poses in the first
room
---
 mdr_environments/wrs20-arena/map.pgm          |   5 +++
 mdr_environments/wrs20-arena/map.yaml         |  33 ++++++++++++++++++
 .../wrs20-arena/navigation_goals.yaml         |  12 +++++++
 .../wrs20-arena/wrs-arena-blueprint.jpg       | Bin 0 -> 94505 bytes
 4 files changed, 50 insertions(+)
 create mode 100644 mdr_environments/wrs20-arena/map.pgm
 create mode 100644 mdr_environments/wrs20-arena/map.yaml
 create mode 100644 mdr_environments/wrs20-arena/navigation_goals.yaml
 create mode 100644 mdr_environments/wrs20-arena/wrs-arena-blueprint.jpg

diff --git a/mdr_environments/wrs20-arena/map.pgm b/mdr_environments/wrs20-arena/map.pgm
new file mode 100644
index 000000000..756f233ae
--- /dev/null
+++ b/mdr_environments/wrs20-arena/map.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: map_saver.cpp 0.050 m/pix
+192 416
+255
+ÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍþþÍÍÍÍÍþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍþþþÍÍÍÍÍÍþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþÍÍÍÍþÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍþþþÍÍÍÍÍÍþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþÍÍÍÍþÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþÍÍÍÍþÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþÍÍÍÍþÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍÍþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþþþþþþþþþþþþþþþþþþþþþþþþþþþþþÍÍÍÍÍÍþþþþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍþÍþÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍÍ
\ No newline at end of file
diff --git a/mdr_environments/wrs20-arena/map.yaml b/mdr_environments/wrs20-arena/map.yaml
new file mode 100644
index 000000000..5e95ff496
--- /dev/null
+++ b/mdr_environments/wrs20-arena/map.yaml
@@ -0,0 +1,33 @@
+# Copyright (c) 2019 TOYOTA MOTOR CORPORATION
+# All rights reserved.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+
+#  * Redistributions of source code must retain the above copyright notice,
+#  this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above copyright
+#  notice, this list of conditions and the following disclaimer in the
+#  documentation and/or other materials provided with the distribution.
+#  * Neither the name of Toyota Motor Corporation nor the names of its
+#  contributors may be used to endorse or promote products derived from
+#  this software without specific prior written permission.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+image: map.pgm
+resolution: 0.050000
+origin: [-5.000000, -5.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/mdr_environments/wrs20-arena/navigation_goals.yaml b/mdr_environments/wrs20-arena/navigation_goals.yaml
new file mode 100644
index 000000000..f4d81f9ef
--- /dev/null
+++ b/mdr_environments/wrs20-arena/navigation_goals.yaml
@@ -0,0 +1,12 @@
+# pose_name: [x,y,theta]
+
+table_inspection_area1: [-0.101, 0.847, 0.737]
+table_inspection_area2: [0.696, 0.579, 1.608]
+table_inspection_area3: [1.367, 0.714, 1.745]
+
+long_table_b: [1.240, 0.978, 1.693]
+tall_table: [0.012, 0.981, 1.554]
+long_table_a_boxes: [1.009, 0.063, -1.544]
+long_table_a_trays: [1.721, 0.187, -1.544]
+bin_a: [2.400, 0.047, -1.544]
+bin_b: [2.748, 0.047, -1.544]
diff --git a/mdr_environments/wrs20-arena/wrs-arena-blueprint.jpg b/mdr_environments/wrs20-arena/wrs-arena-blueprint.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..74e27456ba694c03b687e58101b6a94dedeada56
GIT binary patch
literal 94505
zcmeFZcT`i|`zIP-L`1+y6RA<@T@eInQ4|mmqErDPFCtxt1tdyH6r?u+0YwoN5NSrb
z)JRtmkuISoQILM37?0#6ne)!vJG1Wne(zl~^ILb;{BeQJTAZA-_kN!JJfEi?cwcz)
zh&{GeHdcsj+YpFt@Hc`tg*XZS^&k8CXOjQe4Di#uw+PXle_{Fd^KCnb_)ByfpXfGT
zHv$FklmEZWjrc#lw*AGooqvbGPQhJ5@D25Q5Pxms<NIqnA3y*0?eN{v@aKr_qWoe9
z4xiW|e$GeW;8h8|JDJaRDq2)`NIDJyN=GhUixJ$lS8AWMjIxTVn!1L*!BIn_W5y>>
zSz1}2wy||`c5!t(@9yDy$<IF^FevzXc*KoJ{LNdjcjMv{5|fg%vhU~Q<~?|r|GcQU
z<VESrvhteRy84F3H%-l*UEMvseed3X7#<lN8=v?xNu|*lU%xFZeqUN<{`k51i}f4)
zv-J;M+Yo&Jm1X~hu>TufqA*>5ZQstfUEm+Ow*3_e4?fZD{09#25Ib>Bz~`#?LA^UW
zB`h+ZRd)y~9&rRDFJ2qkwO2`>rp)|@wEs-me;;8n|5KFxSHk`qT~vfH-!}O0_(TzC
z1edG!JQ?xdJFNU%w7NB3DnGb~;LEZxMF==~8%nK7v(XrO$GjTR2vW@J6S<{7285GD
zTMu&8jFYbJdo8nTdC}Q0-a>J2=ftO3i`D6?o>*yf=_MXQnkWgfX|7dtlmzHx$K)a6
z4KPB%<_7_XT699Id-3_@@wL#%$fqH>lW&H9N#6N-8h1e9l1aa=0TjkVbof%{AEN<F
z9%AwcSCGv_o(CmE<|(_0U$}BDpD&wl18d)sBypx#whh?FFkYcXSEa=*y=Lw$p@-gB
z{z236bQgEG++C>m@bA5Dc7GibU{QA}?vFuQ91~zx;`ZP(y13%Qx~3=S1=mjlvvH;S
zb9o4li3nLURoj}hf^(NzMs*irhG(QF&*~39&+|Kb?KoNk-jz9)p~&q<)zor7Cpe*H
z9?g?(%hHdHe@yT&S2C6?YchWS;Nhpri$5G*gbBVeY`<CfdurvUB6s(#H%;|CM2rS^
z;)utfS>01e1#@)P{lGn?LMcd8E~WjbtKh*Sim$G`>p8#Y^i$-Q6%y5x718oU$P=4^
z7SIU`^G~0hokb=QMRIR0J{&jliC6oTd($u1$~j%%HS)%;r&;__9yQGsWx|zwn(F-S
z=P&KC-oEF=_AAx~&|_i;7O>v{4MP~JchCVX8A3~>+Qu|WARa;jBi$aecS?!F3d+sQ
zyK|zUre;Me`L)QM$fOkIUuzAu62GOW(NA)&h6-8hAMw;uV`fZZKy?8~Vq~5SiLG>!
zwn3iE!Y=OqRzSChlDr4fH112tk{;=}GWA{RR#%AIxKjOkQK-aTn;`!5N5a1w#Rp`p
zQXSOzHiwhBVXsK@3~c<|ZgWL~K9*X@GUSw@FU?JsGNQjWpmw5z=2D4!YergC={D4H
zJI+ytJHRcBjCzF+9&~%1IOkrnX<u#~ykfTSFjOK^m%pVDG|GNEE5%&w(hi|Fu6nYa
z35uKya|3X^>uupK^kuX>-f_Yf&`mPXzG!pcbA8B$h}|_)o2OuuSElG@%Z%-;W`Bx}
z>rb}X-!`d#qlU&L<@9>4N;q{Lv=bgGuC|jENIzYCmiaEdxI(dLhpXdB_3dIJHplrT
z&i#i5N`r8b(?s#jnEct@;4K2qu<aoU(33u3ahhHS%%yS<;3F-^XBH6MN+V(9-1TYO
zormi`o~?+@QM!(`#5?u0OED)B>L`2hy4@bSDW<xwJWAFx=&0SxH*WQ&m3!C}<Q<jC
zxjRDjvgj6FIH~z+-ud3!JAXneVSyH-%;(+eOXTW9_Fz3*mtX=?7?{*qMW$}L`F_X(
zoWV(TBSp_oujEO|#DTy<8=X{VGoPkv=YC1%7M^H-Xo)qXvJ(Hp)e@TGA!0q&BHB>%
z$h9gOPIyH@ah&A0A;6`=cyXAmg?Y7X*j!Mluzh&gC?d*h2c0>`Rld56^7UmoYCoZU
z%Q=zr%^$%na?g=0NOW=0`snaid1{u;JYt&?OA>fd!b9+z!8#NfB=x)5yObS79AJ5*
z7etul{CdnoNVckEQ)i}5f1dXIm~ifoa=i1U2vv;_C%(E(-J(D-5g4|BHxIF-U(Y2{
zfRp`m;QrRRn(-1E8B{x($(Fvr9ci3XXxCNZApnZ0=u<5C0keC6&b|v!z3{Q!GW<QQ
zw@8zZg_Pcm4jWz6?hf9KK(~_;3PmA)W=IEBwOcpdTo?GAVTz>N`L}#Fb)$JXj<|o0
zh`N+}S|LQl_Jd96D_<LWu2kUHziX_%U-@8pBL_hCV3JXM==s7q0E0~8A!K=o0AzwU
zq8U_Vn-O+18+%9yf;s4-lk^NP<S|qyBJ0TXIFAqh+4ULooO*ucNM%iQbFUy>@51}5
z4c5MeJqz#&B=8V{y7I(NU928XArCSCU`~pw%KgwT2+U?w5qv;0iN2=U*@bEHjItqI
zp@<O7$~;vX8C858kz(E+-uUOTYWhoGYqMOO8(beFNKTt>KTrN6C~vW%?OFt*2}&UP
ziwRfo7c-kx!g**ejdz{shL<=Te5<zr=pA%;Vx}S=(#@3vNkt)<_T~5$XTb6i29@+G
zyv6-;D0zR@tkgr75>z9q8oPEC=wdCwlZbOi3wJ6}u-hRQ;H&XK!=sAsdm-kiF`ryJ
z`N%GhkD{%aeP!Q`+v*n{g<s4%trEC*foh|@kZgt6%1>5%`q%%*V|QdvyE4#t*VLpO
zpU~UP%AgerEwSd8w-zWbsY}L|acv)y0?AP?XZ!i<E3XZhUeWB=;M00}d)-gqPZg()
zG@pnD><0O`gA{pE2>NbW1gvMlV8DD~5=ypJfPEYkr&E#?vrmC{o`26Lr_}Yo*RCh+
zS-$03QM#Nzy2le!G-WM2cLE=(#ngIT#!YB9B8Y*0Y-@rlbFc%M`b4S@Mqu5<Ejg|{
zGa(-5_a;Wf&F&5UTcCYU@oO*7emS7)Dt}Miki9U|S}9E0l6Y_l1%x6Y@vK3o*Rj|!
zbTCGNpg$bpY361N$}$S6A@tHjb0pOu?xWPvTyebB;)OlS#?Jn;M&4cLo-7DfC1h&U
zAP@qdVP&!2m{ROR?IX?KFRWc$vtwTZ`%VB2E!Sx#^~{WJ%0BO)S@EIz0!`bAMSvB@
zu)p}@S7mtHF~~;v#&wg0ohmJFu<DU#o^sO(XW{7rc!+eg@O~h#rv(?!Lx{GELT9~n
zOzahVM!r)UV_U;s4Xt<>yH|Z3@Tq(#QhY1tcEimSPC?j#clKQ$DS@Of*I8FNFGzm!
zkt_``Za%w|JLs{I03D;n&JXe}8xn<N9MWiJm@{Lo=zwRl-yJ=sgEG&12kljM-7%$q
zuGJ|?%-!(jTrU<>!L2@GGFWH9>uhPl2_P@&mANoMu?8q1cSQ^lBIsl!ex%dTTySLS
z46r~h!OvNbR+*mMTjdx0L($t{rLFG)_X^>&z&sL?JHp;Z>M$bt^AKNzda!l69fNTc
zVM>55l|-9MF;|!&4tgH~t@`KZq=RP-T9~=ny8>lyJbXTTJb%l3iA=hxSD*HA{3<oN
z;h3oQLsSTN*AhAfiUh>DJ0&BSwV0H%%)ul|NXF#s2Hm`{f~qt$gU{k2-sVU5wn%ac
zx;14V>gK!VtNZZXJa8|ZJVcJc0yrEbzDQH;rtE^;nJ%S$NUBNC?6x-hlW$M59)L3p
za;ky?5dBrm(jaF%gGLeycKogxQQ9x;cz*iAmqG2;M0p3RkWDnJn3H6_o%_X*@|P}G
zZ^`4^THoq7W`H#~1oX27u<f4}tC_D7_&mS#ms$S(F8@h>^=oA5g;+jGx!u>cfzNmd
z0n0#Rol>?u_dTmQKF232RA7yVVC%W}gkBGoXjlp&(PayaS)&%qzDIw%irWR5DxbOL
z`Hrb~|Dt9jcQ<}Se+3IFayIvlAI$}VP%;$%u9B1dt>-LD_nErmaL6x3X)d9yFXd^e
z7JHN~pF8f9%*p0dlP==!kQ%M;m`VaGjD?5f;3u*$66^9qKESHC0+XMINYHu4EWAzr
zR6=8?q*R#=+dbO1F37e|ZHoW4dqVCn)qm#TA@1-HjhMHOD3c?M^@p#5DI(+u9zqbz
z^~4}oeskr<c2vx_S!rWV<rvtT4S(}ZUpTPXX=B6CdWGDMh$3|w2ANAhha>$XNeS&j
zkvEhdK5R97k?Zf9<({OJWuuf8^0`9OVyKc}@;s1>8WClTgllVJhU&ntnI)N8;r*vC
zSBcqN$jceuspyvZKHzWosqrd=#5v3V&>-oe07vB1N)M`0pf6(&sY8Y2LrL-814jCM
zvwOjvF05}ap=)b&od$L1{U4vGA{+svDQ{|1yunO5zNq>0dVVjEc0lFHj=(|ct19$$
z^u>)hQmyXl1lt^xWkqlv8Vv6AA{c>Yw5jOB2LT=;v9938#>e{u5oNHIbu*40>oPAR
z2k-dpL}`7axj6m)Xs2SD3voBI6p|g}A+~3OcNo~ckOk9vwfe;@SlefQV64@N9_@gV
z#?QW6zO~R8ZC+I7-MTj)I%E~5tK<KmPcB48>VTx%U+v|b4A|ExtkzwXB{2n@IC;d~
zQ;<<BSX8uh3z_y6BZ|+y;1yysR5BVcvZi1b={%}<$0ysRUAbyp_1+nm$sA_{@+nuA
zJIX`Ul6o=Q@N@LYE>s$7H*^(Pq<wlu*H-M~>V0mt_PI$L6vTp#lk1ROD0$iYA{CB?
zCikA6h#bCP+$CSSbv*Ogfk7Aw65eir%E%P7e_@ih23R()J9%(r{@Y?qtM#kpcAX3i
z7#-^0@cV|-%+cCnFxE!YyQd5jQ4?a<ruW?&gMW80;O5cB&vUhnkoXGgG$*HB9C8L_
z0XN#-Od2^3D+5{5c?fZWJrFvxQ1U8>s6I-FDCHq8f%%%FW9#c*y-dyyQ|Mlt0-?ST
z{=#*iGy#J@oDd#j$2_Uypf&-EN^xTawY`bawlK3r@y!x2VD?R5So6WU$cI%xq%R5E
z6(hFj+r>H}+qqrq1v>XOdJhlr_hF_qX?`3Bs{e(5#psqoi}=FFyjCr>I;on-9fc!1
znne$7M@ARcFY2vowgYYtIr&lzq0ZZL?e-sy+r&}s2bt((Qb!Z4OKXmwCbi)m#{_gJ
zeO5YLsrTNnqovG0>-3mKOTVVH(hdG67G6mg?THQG1}hj;Wg=MwSTbr~fkIZbAd<e3
zKrQT?@uY3U!&q|{YnAbq+McJA7WFJB=~;AK-AfcE|Fomsj+ns7)R*)NO&CGqK%oF6
z$~F$<A!_pCp+n<nzB*LAIuLaR$Q-Q~q#_a4&kYmutc)uskY(@YCLP8XnCl*^*$dKy
z!Tj53Y4m(5%k>KEEVW^``zDXi8m&|Ob<@PApP!Ve>dp?H6U}v3aXG7e^Y~v@(I4j#
z-M2MLA(_>Wr1?zJE{SSQsUB2XwN-JJVj6)|7MdntY5Bf0bzAnG0pdq!FD$RSU=N~p
zB7x#p823a12itFe5?4>!5jZQJ=QLWc(g?6xbc)Q&{8Y+F*Ud|gHRrjs`{#S+<`C$T
zZ(ZP^bz-#)ILXojPZSdbIZs+uxVylBXUqBP^`(J&!Ogifz_PjG=!>z4sCdfQtFR3}
zL&Mtl``uOil5X_vb3Uua_q|;0`R!f*lS5`bv&crV^6K;4$QwHo%LV=YFG{)`SIRn$
z5OdI6ASGZMFspscNCqhlb_39HbJC`=3?3pCEz)|ZU6;9dw^f-)W#HW`qdodhT8|rq
z?0hk#O@8D^sgGrAEo+yl-N;#Ul?XuPV*w8y0#wagYhx1#((swSD{Oeu#x|`_e-tg1
zXfC0gYK)09*EFppOExqy)^@8wA<r^p>T6E$4}3~G*uFAw)U`VWY9Mt8Qs&bsfHQ7#
zkTt-0!Ij5PF{FAkzVLM?0XKx0Oy3UFZlVO_1uXWkMHw5&UaG@GIvk+OF>&$h`+v?D
zG!&j;A8l;^to-i02W_Xomi9Sp3Km!&1l5roRU|8qhv@d^q@(|gI+7uA91EljLP`|C
zW02bDBFaPby&UH96F(584#A0u^M1zY&MyPA>qnD*D0XP3_1;DYsgZl;z#DFBk9Y_h
z*s7s{Y2qgowE7Y@Uw*_%%H~7#lsFF|(zi<EAwGS$jShVI?;j|0;|eCQy{0pFli>g>
znNv#+UovX0uK}>Dzl*%i#NK<od;=x2cYP=}&R0oz-%B;3tIu&%oxU3b1sPW`COFD@
ztW8+t$(<S8$or(O@^&-QusaEdfn*>x9%Aw4aa8U7|Hqq&W|6Rd*AQnPY`|EBjYC1}
z-}zYAlA$0JaC%Vk4HCAg{D$x?83WfC5#67mU|^=$@#A#A(v8>nJtr@xf34}*Bidbj
zP^4U)L>uyk<G136q>j_DXZj})1$CYkSgn!-$9Ra{$xKy_Qa^W>HO!#?<2=NISOysG
zmV{nX!2*tG$lODKhd6Y98(Y(oXs8N_j@Y+-oIMCHoD9=;Ho{~&&SrCvS_K^odwu6e
z@8xhZf8XVGL8VV_9|^ao)nJh7$|uOfN5Ka_QbwZtE%OjiAG~^mBnXS~?jRtNqlDz{
zf`izkyb2!Tr@S?Eth8fp^Bx-1$m6Pb!_5B}fgtvY2{;OjXDf~pFH+<vw2x?!Frxi+
z7JP;=1^C?0gnpkY?xeX&l;!>C$<d%$`H|9}XT>BQe(*^@`*;rp_=diN+6*rQ&dxz+
zFo{^m|2EoJ&&+y3?cFcb19Bf4yjJ)&W6Kx=s^PdQaL~#<0&fE|Z_SZcg(P4$i;L5x
z;LIJIfcltm?oaYRk7_apxpGB-NV_`^5wroSS$@?1F@Q5%OJeTnx++RP?Jrt<ZC{Cn
z<MAxT`@Z#~v{(wQZ>~m_Batzs%R`9$W?*;-edld#$;@T+VJqvGP!G`<=2iyf5@t&s
z)qv$HZ}b*&1OJg*6s)V8j_Au2$ZjqT4GqEUbN%N5X@#PnlHdx=MsrRk`g<{Pk_6^a
zWJrIWm!JTPjW5R7+29biZHU{zKyPUocbrmZGUsI&@FoP4c=X1%+nurUt-gp$;#tSh
z7q9&{W{|+EZV;>@+&ic`U9NKB+5sM7P$T2tUcHUJ_@CLRRaBw2m5EJ<#fwe`Re1=`
zU+6$AWQV4n<RPB893#!&MFI9xsXPRZa189DKcz6&4K}D<tYT7)`0oJ<3)jOjR0xoT
z<_w=sH9o#g?A*X?=yEpEy=hCFZ#+bM0yx1~gp9CUL0IrlsnU^LW!&0zcw@sE;1vp-
zaN{Oox>1nr9CeY0C@et`JO0lJp1o<QuZvsPc@?_$G?h>3x45<N;TVgUAE!Sg3+;JY
zE1ZS+nheFjTo!@_T8HxeE_4D$peoKQ(w{Bo*YGs~^nBi-4_Sq`z4e=SUjG`5s{6g>
zSvL8;@2@ENlZ?=Tg3j2V8iU)DUrCM%xct3s>pg6t(eae;KA&N3g!=Fh?<Gi`7#ewN
zUpN|)Jx}^!Npyn`Iu)K4y?Oa3iS`}jAz(^uVHPJqVvF3gCpgFqh2kOpES(itPJooS
zGCahy1elW@^$9$L(z$<GK=Kb37J@^tGF;#xhH3|3YKma(;vvj5(6n9@V3)CV^d<$^
zB#>a>DLIaMvBA`&jb47oMFWn;huy51u6HXdONA?6K$3r-*uH0{;&vwz&7S}Z{`1)R
z3`k=W#l?_6ZHMPqBz0(KN}_35rV4Cloj<KEOqH8hKG~9doQ!GByBTjAudg?h3mzsZ
z&jDXEK;=e`j3LZhpae|eAqLVAL|8&PKY}<O!cw3E1-g7kLxt9m)*u|yZk{E;@E+$K
z3p4E3pB|VV7_4GENkc_=SYLKw6DiP_p?`bzv%vSziC*+wSG}D$q6WdJ_QA`mk`w8s
zhj9l^-rJ|t{QT(fiEY)fv9Ud_@B(vyCpY<bCk#WnkU9_19}Mb^aCg?iPIN<EY99|F
z>;vTf*1~=o-#_uI|Jl7AV=dzb{VO?%zTe{fMAfxE+^!W5;mW}`Zg1ovij+ZEJMMA@
zc?i|d0yAi!g)2lF@&@7Kh=YI7e?qYdsD@HV&WnAJ+ktB6OCb4C(y9Ix(ZEIz2}v+y
zm|!wcA_gLcefhcxxcW9br;&C2ISDYz`c3vZJCg%{=jqLSA1s?K*8942?dTX9N9uY2
zC$C`Jw|R-O9Ss!1{@~XwNRIP{GCxX+L)TBN_0tLyNyrG&cJ2q*T~kfq%Hab}Gm$aG
zeSz1fK}N5)PVML~y8^$}y<Df_z&+EG7u-D?$X6oNlhSgv&1|)b`L;ZPMM#2=rO6C<
z3rlm5mjEjtG>Zbvhu`DK?fVO*myS}c9;c_Tgud*F)R&1%e}3wk+VFZMebk+ixp%Z#
zFXJ6DmI7Q`T=inx6X#G!^u@W!4-+nM5{6*Kg^l0cQdS%@qFZ4b812hL>|D=|Bv#`+
zCcF=Y$WUjdJ0$BOw+db*h3pSfs<(RpCHZaDc4C3!I8fR8F3CUiB+EILbpco>0~;Pu
zA)KfS<G%`sKj}KvbQ2^0<dGgXg<~n_8ppLGDo$KIT+1DZ($<?)&YCsT_ZZL@<7yLN
zZHhh3MRGsE_5hVYl5IUkpFuM6R&UCJ&rE#)O@>Z^c3*P1?wZ-bAKH?Y5o?FKeCQRL
zLcbeaq;hY$K&OR&*oRxa-Np%rg?R(ihJ`tbM&e3wC3uK$^{9U%1494y)X=n#B*2ce
zrH73{ftYC?;zu+hdW44%qRcmPBsX9I0<oYI50N`c`qyO$@UpvM=EZoz8lMI2fww}0
zp)dP8tow&yq<}yE{+_s#L_@<U`F?ZTZw#aZ-#xeK)(%@T8i|JCdcr{RFKdkfjnJ_S
zt{|+UR(8Ni7;4ysTujmbfW%w>>q2gwbHD-`RHksGbvuxd>N@F<c?NhL1&PgZQ}gB%
zxZ=NIAUKZtU&YX|;1LG0)9!cRDp8$qtTj_As-!dxI&x+CVei8E2t&UM_YC*!a5$0=
z9{IO)kJ(YcgME+~l?fwZYP-bwwa{7rG*n`p`c&%WT!z)e<$zpsu5*oP)a}1p;sg0M
z4Y_**x=34U<~!kIBE$CO>lXT7@l_YRW(|o=anocwNT4HR3HxHb8UN<mZM5~jpmWi`
zt^m<tQBZ>Mn(}?96+ZJg-EJ)Ra^Szc$hR5!uM_<LTl@!F|2N}e#Zk&00)MnxOn)zT
zPwV4~L-7;c%?8~i&VCQ9C0CkU<f?lSE7(Wqe`kJAz)-LqcJX68r@JqrY7!DI|LAC4
z)9fQjZiZB6&^%`}`*I_L(}o!CF{h0pe0mVXLRen^FRlv;$Oc2W@$yJG9WsuBGO;<Z
zb!Q+!<w1_D=^S{=91Z*M!!TUHe$RH;HWdZHaTwAXHkaI6&fB)0{TtT>uI})|CFGb9
z;s6YX8HuD?Yyw=X5mD{VZ@f(kG|5nKzW{qkZ~gnat!Zn0F;ktMID<jti7WZ(&r0T0
zlP14q+Xa!UTsx?;vRx`2L!^J>T2Um>^Oaa2WDa&lLXe55nnieti_xl`g|!*CP0{FX
zyNrx>V<tM4t3){7&gTCE1cQzfT~B0~xD<mWT!fx>>nZC=oy`z^e(brX!q7S;zO777
zK0FzAlEQim>?mZKur7lg^OO|g?$Q16J|tSGt4T_m*+QD~-u&@2qsDQ46R<nE_RI44
z-pI4>AAC*nvdws2!OY_d!XUo=7$*ZgpJVJ4f)dn*+!%#vGUNA}15$~H@FU)%3T!ub
zpF241@C?B((^H_^i4lds{L_jV_AO&BmjYjmy+}Q%x;pm%p5OdCz(~6H$ehB%&H4Xe
zD)c{^O8sB;d_agyRc>5yaQ<wGZgy$lR>XP*>FAMf?n|uUnHih(BQ-m{n$rs`N#^KU
z^qL4PgVL}JzQ#xOq3d*4huD?`6HX$@Zvailq(J<D5AyAX2y_LkW}09C?^2z#ay+G%
z%$V8$ty&FUEX%~Tq@nCSU(Wrb4NbH*<+MIWKQpuV0GE;4MhtddY<fK!s-vYF`eNJj
zrn=U+wO9KyJ>zYj=P>+^30GR*;$AxluWi7cL{&F5GR$yXK|3}nn6!f{50>_NOk_M_
zIfBgjeo`WqZ*u_aksC5&+t>NKp(ZV)D>uJjGf_E2)AQ!@WTR7gh6?8^pEKmn%pR01
zO~{ozi8R(L)RC3`mG4wQu$OU)3kJ68RBcmz*2+3YE0<~I1E0&R0|MgCJzuJ{>lulV
z^i*})@fu8QcGejD?Hp|C{Q8%MLs99i@U>l|6Lo2rR&zn<9H)WeFHJlej0K!9$&{M)
zBDNu92;3;3lXnuwxbi1~2(ogXQ`Usb5K#^9>pk|UONRefNei{@#j_%NzZc$i2R$w2
zvR?-PbFe=L?e(0GT$`e4u3!^Mb*R;fXHYnk)Jsk=7U<Ny-7XAfKb<1jkG4zTG0sgX
z9n=>k^J#89&E%wC_K_~1H&@n(KX?d-k=0Fh!J(e05FcDv`r`Zmmb<HpDa%TNxe7Zy
z9d-mywmm@=u!dc#K+<IXBHRPf#4tmhaGv1q1N-S6@kkN8wFhwDW7SaUWK`8kzOQoJ
z@U*>ew2ZxQ5y2cgJ;zTt##VsSzp(F}hpr{}Y^0KQwMqxkk#Sf)q8R=f4}k^xx-de^
zC@I2qreQC2M`lrJLY;^OwRg^S6mnvq_5}Y};vvkdgs*82d~D1+OjTNMwt8Oj3vyK{
zk@;e3j=ks9>C)BHzw&diz5e0Z?jhEpV0>w8=eJGuk&^g`06YCsWW}JQtJ`9Z;rqC*
zvuWpjL;R0Tbvu#Kfq>n?cv;<UpK_al#`xSL@<~;W4z17D^cIcW(q;CVv&Z3W7gGQX
zNd)8&CGoMW8BUHqCMly_M3lf+PPhdI_|h4b(u6xDaH9GGKFjPh_RrfM*2yrHpWEyo
z#cO__OF~H##R&31Fu@WeGmpG26oL4Z*V9U^85mix!V(ya`$}O-jvh7Ue|&45d^6}p
z+o$@QS8*XF7L6))e-@H8U2<klaBbjyJo6~+)5L+Q<TVu2;wN|K2zxJaun@5T232~`
zctJtTxwvdb&poE|`;Zba`{ZZ18f3(@uWwAH)c-Ou^Tdq&!bk_X-SdX>(`H)BRE!6y
zWEwir{2;N91RN)WDwysWYZs<@Et5h=t*+5==|>HlA6J!oL;9d5FpX#T-tzkPo0_)b
zd4ixnxLfw!t1Q2?tkajoHMTjck5}v)&boWm&XCqx6YO*@HvOiX)UWc|qMH+){hFQg
z<@5?G;@GFh6Z%08_m^ramz?SY2MdKvE6^Y3=N~WjXZrvVuYou_i`)YXl85U6=Gm{#
z<-H#wd|mYtHv0OIpaybP5K!r-^Jk=yE|VrRPBQ0iL&|0NC%@l%P<jUw8lGsl#EebZ
z$77L{L@5^mr}hv}S?9f-b}1<jp5E23Iav4*=3ovAa2<>(Y_wiw0{vcK0{bNQLp5>q
zi8ip93Pm!IchJJ^N+1C&eA_NMOgK?#_o5)OLyD_CL2xg*@V$L^hIJFuW%lX8#(0+9
zBeKSa@zc*T7SW(Kr2{)z!Gi0tG3N;vfs76w;x=jrq)4YE8=_!<3@A*<3Zx)SQ9-Zd
zed*YAeS67)hRUsTql=>lhkC0Cd5&K76V&HYDp%Eh3Y2lDNJyeET;PQ_oQaZ!^TBg8
zw=R+(WDA#$72g9DU9+0N+#TrfbFkKpncg<fM*X!kWcMn_b?>eEvEEv`>=&h`sc5CV
zBQ~vnUhTM9?#9}u^>g1dEzFo=TnA0Pxa>oNubhsFv}?}3*)SbD$;{5?u;Qkx5)9jO
z*@cGo#;Ef>m%f<eY2Q*Su%d~N!fexprsiLVmyhSYDL7{D|KoYpzOwmJjmOKPjpL}A
z-+gJAPV`^q+Qgx`dh8m7tp{BIb*l)T%&;b)i-Eo!ffN8u<`=u>?nZii^^m#?36|q&
zm#T*i)^L_%srfq3$ic=XYXj>qhp4hmVR{ME1{qTnmMZYO8;2yICP*Q;$v$hag5Gh7
za2Qy01P1_ae{VAn!Acg?+L29bNh6wh{2`yqsmh%lw|cP0S0ocV(6{yoIEvm$>Nt#^
zPlNp|+tYg^MkzrXNprQTciL4aC%pzsmm_D906O6SSi<y%-U|a4thE=KO1}uL+|hXV
z;zy*bc%0A19N>li@|)%GPhxI+KU#<=OdO;LK`5qpU!gQ{2c!!ckU>(Xj5ax4i?|>E
zsBg=x?Ny6Z%J66KIm5{gAGTgE^m{Z+r_@P5TdM!i8rg=nGFF6S<?nOY-vzvqoiw2z
zk$G^-T}6`a>q=yldAA4I9<Fd1D@*Dgdd1vrplLkBc;>XC5B$RFDBxG~#JllHF3m-G
z!`<{F#-r-S=YE&i=4iD@YSfiAO8}e73FpI~VZKn7(7onHKtLBoc)4Al`8ydhnJ4cI
zbae_go=7%zsa)C;NZ5QNgr^3P4dk)TZ|YyyHYO_tDY!4D{Z)4CE%`n{5w_|ousBD7
z+?`<rBs&al_tA-2?A}7hRFwdWeHrojAExn??jr{6akK~K9dw(I&Fj_1rp93p_l5Mo
zcP<t+5qA6LmS)Ny!oCXI5yw_lwkRPYf2EgH8@Vda)+A_yBMemht(oq|CvxB*lhNxK
z)QpL%$0p{V>05oGJOrs18}wEMl`e~3pxV4T-1H{(o#JE{aSl$*OsuZZH$aU!NKAa*
ztt;aWY9}OJ`Q2O%9AKXY8QuV?t56tEa>{s1*#R9c_QaE{n%joLR}7;}D02ANQ_bnK
zOCD0uW+h@;)dx?%?7)cMzFS=G^Hb|ngA`K3*v0*ZkZkg^^Z~hm2os-c(}U7>f)e?i
z=ZcbZ4_+(N(vn*?i0gHB{)Q31^I^$1q_e{NJ>&30e_8oE{WP^#xLKEvdXQGNUR^*E
z+0<06_fWbDX+-Uqv^?@)t7;#*UTSp;=<D(5yVI@+MS<vfv@{siQAAg1qha?EP64F}
z1{~B|dxCxsGq<#Od8?{qEib<CoX^Glf$qa!3?|vepZxxW%rql?umkAT9JUE0#%ZO@
zSIz;B8&nDd-9;8AxP7JsVB|m)3k}-%GNM(GaIxKPtX+C3Yq{NcOgr4(LAoxgWeO*@
zY`FJ|*9GeI`;gadwW^OlfhV85(%Bbi)T=3&-b>f8Ic9vGp!Y1{E{$KU?1Ou{iSG0H
z#od7}`A)I*r`OCq@E#A6)NQfu9X@JK_LeT)7lS_()70yxB@XSoOgvy?6>2Oro!{!m
zo&nITOim#>C_`ih>|@;kr+^AZUYtIAaYPrg1`7M0NHbX}elL3kqh#Ccx@2-of^avU
ziv6AMoQJqq{8vZ}_?~@@+l#KHByGa(#4Sz*DWEXc`_Q<@j6PY2U=LE}(c8h;vr$dk
zT`NM0@d;id8w(*xJ5O62Ka+a=%vZOc0Zco(VFJkq?lWD4Bva^vt8Fw45<J;~7NE>O
zLLs5+%#$6nf+O0SnC-}n=G?}YRLEcyw<(WWrD``&I2o)aoKj4$$$Krno9lPFY_{Oo
zZ97~&h>0K$Y++VC=TRswoU;}oDnJBqihUZ=A2vV29YXIj*8$xun0+axzI5H>CknAW
zTLT5-6uODMEo<ro@asuEC+_)q>IF|~(3am_n-B@XxLuED2nYL|zLi?!?lxfVsoXd>
zQI@|r>9tGWPvZH@i@seCpq`7%R-0Gq0nzh>Qx%^|4!r!_-KucVvCQMeLFeywNFmF!
z@$Q-aIpt$<HVrFh6ZXts!MeuTxpsF+o*RF*OXEHd;TbW90<8vVz12(=lq`4>1Nf2S
z$dEm<Tg2J}&ZE(GRu5l%ox83pI=9k)os+LGbV=xD1VJb6O`VO>Pm7;eY2rtc%r8tF
zx`up*sLGu}H6*MqF&!7_s;RoDP7(@Ij-nwG&^sW{BJgK-9tkKXXwO&7P#rR>W^Ib>
zN`sCXp6!o}rfXXqdl4Vfb?`9PFC3S$1t<66)SV%NH3(p0bR+zMgl^p_ak}+t5nJVW
z*h2Gpt_<8jI#PnSvmYkttc3q85U?9oW?uHtI$G%yKTMT~QM{~2ghSq#cxXF7>c*i+
z9TIROnMYj89KlpUb%d)-fE#`vy_{LXL;`HqRq$rOo*0`-ovN_;+A)JaVWHmV{oQkI
z?sd-oJrHVZ`s@D7xk(1o^zfZ7Yw`6@IdX%7n8fRO9;nthVgGa46a5vgRhBknxcG0V
zq21{|#+f;0)nfZ%hupcOZex)*qh;9_m9xG*`Cva%Kv2QuTgzL;8CO|;NZa)~xlGE4
zxZ=s3##R^1^-Pg&@1@iStTL}Nd)J*r$en8Dhw!^_i4#ZSA`f-UgTKZn{2pi=JFJ{-
za~GEvi!Fq!$Qhs}oEiIqrc13h0XRFbjcts-PRI3P(!G%YT!)!4VIWgC*9^?12!d@e
zaVRn9%*3i9(=cJZ^_ZX02xHjPj4pD+`SZ6i9gBVBGq|0g8&@cnt<L?VupJD71)L2Y
zV4D+=U<lAh|J{w5+F(V0Hs6QOqy3KEL<@tpUDU_~1J4afe7_@R&(hu-TdS|%KMgXx
zF)?g^{Yv&=lw97aYNU4|vy|4?MW(iJckckD07j2FAK;b-9b?OKdlhuR90rZ9D~ylw
zw#W9OrG8Z)h45Ar-6>^Xrp-$@g~fq`FTa1D<&<doMNNkK#t!Bbni`rPY&iey(u64E
z^@9?l*(a)k_pWedQx5Cwal95M=b}!!L_eTDNUzX{@KvX#xo~=fub1b{A$_ifM?c=s
zGS@ZwRB@$O{`!YqM+)TZwzx835~F@HS1KHeeQxm(t(5ItG_;#j3`wH8;qGbr?=D;#
z1^I0h7oG@(0_0u~f%H~Qm5LW{QqGMOO%M!8FPEyKd=+=DSH->07dYVO;+y=7`i&qz
z54WD03}tr}A|Vuw#0TC|Uu?>-0OIeI;BnUJ3*SEG84A7#9M8RBW7d7Q^+)AjHrR#C
z2S2@Bg3Rr>Yc|%oC|UcT*Q^U>)pgv=1NcK;8oijZZ%WScW~UH_Ym<Yn{WQ0mY5e%?
z!;6go9sm1IHgI;p=7goh*=_&3jT8H?IS7<JZ}nu|8;uZAWJQBB<x6f8xBN?AKx#}5
ztMKKQ3j+bszEL5xXve6K7L$s|^r(<WH#3!E@Rw{ZW=am_+!xKcRURiZmVeDT$hn+e
zcg=ZzVHkIc>S|qd-|y$Z&&#zMrH=Xm4;PHI+9gY+)Eu7}irb0EWTkpsOO<y~x74TF
zt2~@1HH>Efv5Z34J>N$hoISv)5NBOx`q$}eV{Ms(vF7_hw=T3O0agBeE9X@UjT3;0
z^=R_T+7|7H({E@zN}oH|;MREK(7Qj-TUf!mGC;MlvHHeNdlRluA^R|x($SoWg<mSE
z<bFa4>ym;@QiQS^UTfziX%ST{=Y=xFTTfYvtPqa8u~EetRDOF9PhOB^5jln2L&S->
zz1rq`3A)Ks-IP5Mp_*>vbgUQ{FQRv>GF%ha5G-b2u{^R5dPh?(O8cs<DfaS+*W#KB
zTa^2mQf&bD4ekdSMc<#G+L)`xzc%770UOD6b9+ovXg9<0zmBOqvdKwHz4yLqX#sht
z!mf>ag#CitrATkMYO14ow4_t~$+@*6JISP6U2&%!FA6jc#|3fRmWRk{V<lGhR(kmb
zMwomJK@vHF<X?LCi-Yn9(_<gov(H~fC*fDiU-{$$owSb`$S1ysmg9A$J!S8F2)?RR
z8~o=Iy7usw2c?dcL5`ga)m`_p$c7v&!3xG^9m4K^!s<r~Ch#9bNSWD~2nndBM@*5D
z@)P>ZycC&vsjW85cI4jk;4(I8_GtWaNEB8SQw~1D#CV-Nowm9O)>SLym6p`kw{E-A
z4p&wWql3xR1lD0DYL&+J;ZC+oFz32C;yrMgrZo~k(ib~%cU(!ro8-7zP4L83+)KQt
z=Qt!4MIRLJV}!JwJlCjA-x*eKBBDnh4~Xw_sE%;goPqb4dTYIrt_!MS-&UQ(jvJVT
zaZSic#@D=;fB*JKgN?Qu+^7zl?&N82JN^Ek(l4H#lkT1gKdM6a$Ios*U33anX27;`
z&)|OPKh#vJ^ipdMnZ58KH2_yeKeTzMzGmx<wXHtwE;+9^T_#tiVfVh9j-?4jyKMKA
z?zDzVnW)%cd<0!e`ijT}!V!;Yfu2^+&g%m{3Are?yWgJ%v{v6l_qhr;aSuFUyu=u6
z22H(HrBESBmCMdmju9H8+?%ApLQn~=ND8oqK|2~UZmzx%lF06QqV+i(orpyePSQzy
zkS^zO_(nW-w<(E+-HDIy^B4=ClcHKP%&zy!<hE6w@R6@7fnUU_+t?lSvtRpbyL{`_
z?N_+B!3vgWJ2L^2u$#ADtEj;95ccLfI8R6c<b8w#0J3u*WEQTgZHi^sTWl)>ms+hN
zu3yw1=^AGSehyN9E;z<WUu2&Y`i2_H*BnW+v2b4G-)EbvDVY|Vy1#Jz>O<cThb5Ip
zaVGPA8sQA{z|W5hJVZ+!hq1etde1&q{>X3=vKCNUROJqwl$Yx**tfE7Nq;8j{PkNz
zS>Knj>&4#${=lVWeiKe?JrY#YUHw@EX~2$mQ2Q=|C4lO|ARvFxhq(_7o9Z<Vi)Caa
zQ13TVokE^SwzO0?PJ}-kr{mYPsLMNM4@M9)H3>^_&x+s}+lx4Xsv*P4umB!{TFYt!
zZ!*ZD<TWXvmfo1rYB!&8dt)9g2z!RYM49C+2e{6?tIHb52j|?{M>c*9U~JR5Kf9@U
zi3Tp7554tA_6|6HAr1|>58I|bit%6oO@&&+Wjh(KBlNcJU)>XDsn&^OYi@I>Nj^um
zih1VlaDUn2)>v41=|^ff@TO9%z;JchPB5i3rPMXfX5?Fe?bR)oP6@xEY698Y<-2i&
za{%g-Zc5?nh~GVwso!uzC;1m9Q6`k!wK1tlYwV`%#yib#;cqOhlc6Z)jxP2NUFnQH
zX<k~Pq53$H_g721N{%L&4m<qaWcU5l!6QZNFk-h%AKa#MlBLE}p6Iqe(uLZM_8ZKo
zVlMW!Z*K)YQpaGeb36UHco>p;;lBL7{7V<@`=%y)z}m6tNtC>^S6#z<xwHB^tT@8Y
z%k}P?m%%BMFIOp4qbT+#qp=5e2I&VwXGR|%9!?94vR0O~O;c*i(YTUoR2F@4$y4&q
z&CpBm%R7E|a=o<l#fA$E@0PhDvvYoqRd{g6ix+r^#~V{Z9o`7I*8zu>A&$+}U?E8)
zD}V#HP6W*u(q3f)N8oaN6m!HHnCLDP!dG<N68FR>bU7|7h>V#=&uimOF<sMw!+-ik
z7cJWhk5^mDv3^RI+Q6a>Yf&dDk)nXc%~4OXxXih}3@PFW39iTjeHNU@<~yMfrc``5
zc{_;dGai^F8zvF8S784a_5A!7_aKz8e#=UzVi(oy>u^Y<$Mn#^O_dgs((V;@BvXnd
z%Smb11>bWILT;QaivL^^X^+yI#+K|}t{JEdJ524`S<9bKX@_CEjejTT{_=nF<;2by
z*B0G9$%j%~q)@g?GUnMd&e1`u&vk8R%VGWRiuJFDH!Dn{(GJxONftf0Aip3vR~FFw
zRKrp@F!?F}S$-`fTTAVe$3)lAM|GchvSYl0d$Hzl@8~nfMuXt+^5NbE(bCwMIO8L|
z4yE;F@AqZwZLe8AY3a~*roH55OJ~*jZ#Y3yN%h`GX~&wczx~^Z?3C}2D<&~!)YLTc
zwOs?;I0JeC{dB4Bg5XQBHh@$NWX}+>a+f|QHrUl{woq}*|C)rqiDS`KMAnX*<Zo!M
zpd?!j&J$n=mTUz}wl(q7T%&FmI;L55t(JL&9<X+Vp20_O=j5#tHI_Z(=ENbpn~|j<
zTw$g`MmmIwlI*Reec5cYKgiUkBx-4|dmq6C=PsNt;nqQRw8Gu^!LF@|yRF!#-??f)
zW8&u2fNZyd9COE`pvu5o3z;u1SQA^`RVBfDrq3@MN3XSJ@Q)Qti%m^iY{*qNAKrly
z#9Mc^{{?)CohS2wI13;$7Sb+p5A>D>$npA*<b}H0QA7Iu;dLW*X;M}r`|C&Q+Vncd
z)z&IkFIUkT8e;U5_hlKlh+6HEWJY`&P#^209m0o~s`W~^YP`}s6n9>w$kC|0PA}<1
z$OqtB*g!|VLs5>L!M>L-hkM3*U9Y8HT68t?i+|f3+VjTL^sscEiT!o0Q@$J41?W$t
zJ8*i-4ikeGp#<QleDmb)s>p60LY@%7_Ufyg;f`TtTm7`_Gt#-bA=FQgsg79FTc>Vp
zVdLQxZV+7-Eg0w_U)50X{3K4+QvBVIS}2J&2&y0<3D46Ywv_wc5DPd9UE-vLlaN!8
z$?IA;w_wJ1n)4Dn&#9^j1PG>ui~vYEQilIEc2^nD43^r1(Lr>)maFU+1En!v!z%WY
z<%Roxj!yU0KNzUd4vii9^w_+_33=N^F{W#fum6|L*-ob_k939X=S#!Syz6)NoU3pY
z`=pt*C)LmVRs_@NrQ7xFo|B@f_f=D?<eb8q%Tmg0bIzP<D_I^ZvZ^FI<{Y#Bfo8xx
z1&=yVdrXDce2@@mL)Vt2>8>uarJxX?w5Q66cE4S|g{j=vooFgpg0-eaAE<AtPq7JS
z|6+<TLYmvFNxLhz9%o$1@{G^%Mqg~<B>alTad%-$xO3i0U{dL_Hwp+?9W#83OF->n
z0Gw&JcG<D|K?w{UDFuFDbYBbpF$wz%m#1_dy*N?Y@tiskBLC>!?^?lR<mY9LF`M#o
zifM_vZ>?NE)z-l&_lUs<pG-!G{rmO+H?6yen5pDl7s{*DI?3(o7J4t1uDu+=Ybjvg
z^S{d#-<FR#);Vk|*YNeDhC|(pD&yD0&dSmR+brli=Q+hE8q12~yx}2yk<`2kU`h#<
zaEz0wO;lbvN)q%3x+n58l8FaF#)&3?n@KNb8jU?`7+!-6L-d{cE^V28osinIZujWQ
zBI*bMZf}SQ<c{7W&OI^bG*f)BQv$G~ye)zRzdi*C0Tf0?jJXm)IF=!jTU!E|HMQW7
z!Q5S=$Q0<XZoI*xsL+#VkB9k=ZnB<y?Z56Du9)P@dHuZhMO$`fb7XACc;7@34E9M6
zN2;t9;7-;-kRIroBiPw!>augFL~`HR=g%UPmMbmFg0viD1>?#eJK3i1C@04{k5yUW
z2^RSLh@flEE{l#8**P{%P9sm78b($_$zfFPkJ(RDp%zxUkq2u*P3+h@bzjcKq*s1!
z5Bg*(122@To#nsqJo&}+qp*q4#!Fc(kvD9V@-AiBUCKIm_-yLMSECo7KQ2*!|FS|S
z$fn6=GDO`_<yUCqmFLOo6922pt^XtINJ6>9X*A%8NteNOW2pkH>j0&*FhK?vQ#VDH
zgATTiMm%aGU0_PCUNtYHwOM<|tPd|*RM$kkle++CdrQ5mQe!VQPAo?vR~#s!r1?@3
zV1wJv9aWGUH`jzunVU2}N0r5d40v-*)JE)w+cgE!#SP+HGc#&B=w=N(1XFXo`>Ou%
zI~KKQKrYiB$CQh@OTXZH=3AFh)7kE`Ikpcf4GfUD^e*Etu_7zSyFA2A*{1TKE^0-O
z&47g-_sy0J)9^OIj9y~wZ4S%`&nbOU|NFJ6$rDC7omBJYq<Oq~?!owwM`yd}z(j{D
zD;yZ<vwfoOS97)7xky;6+Tc4^j?}S3zsmMEi}Sf&$V8~+k-RJ|p*yO;V|}Mm)|`D~
z3!KJdyOaGa)p3$NzsA~%b=6(6==kT3UZbcc()`zGRj2H;ob3AU*DcWs<Nb72-u1Fd
zG1+|vvMU)0jjlKJrsIBOEiY3$S@`|_Vm6YM>*cPhia&H&nw%=44yofPB`^&RV?2OR
zPFvb&QODdqb9sD*wXrwFJSnu?>US(A`)p%8+ipU?qQdIU$n@!4x9gpuWLM81JJq&-
zY#ba{yr4F&&5*yNM9ihW%cQ3OPF)`NJ<?NN1RJ|lwjX9@B)_=r`J#*fGd%3g>-YiX
z{J}HB`cron|BPI~J}Q$LH^Fotav!%(07eXH%V7TI&s3`bnUw9onsbD!qJV3m^4GlA
zXFIM<Jf7BYEK@(>m>u3$y0y|{E&-N3=9okJ%tqCBdXFRHCan>!9fK+Ai_Wt<qzOM9
zl=`cm%vjtnucW_FO?J6lDwt53*x?K4m(l$moQpG!>UhtVCUy7`^@#6CLR>jWWP~e=
z=j$mHHkEo6+?ani&)Tz^nOjnV&*-_8kyPIw<>hK->!mXM2kY7rYyT$O@=d(59jWfb
zF|i}0uqS&bo-#4<1QUAHdB28wrb1kC$oTlroFg4Os!OMq_jDb$c;}aF5@)kZtYmae
z>F<f3aHGNYS{SGFml~<Av1|+O#O&Vu$Ts7!7GQCnTmCy2>mM)l0Jmy-k}<XYkimz!
z;&W|<akw+NHRmxDkA}<z&(oxrZ}!KG?+gv~NuH%r)#e);|FO>d<MtXoi&xr}*&`8L
zCKp(qm@Ii5v5OPZrG*lw)ua>HqPk_H&+eAq|69{i%L*xC@%ZWEH3z*?hbrBRW#=B3
z8tl7nW8oN#T_CM(qZbw}km$jk_#;`LFJSHrfO%5%$bH`;$_Ewc1oqJ$pR3p6cRTf7
zRQIhuFHB-pR{wb-6E7>|+OAFme)bF($=v_;JXv$I3*7nN`)@Y>ceqb;5le+(80cGl
z1jzQ06k4&x@OwZ>^C_VQUxzkH$`K;AKhKX<hT9(<mJXBlx#K@-AQX{L_v+uS{7vnf
zh2fCPA6PN?U-1z4anMa3f}D|n+Nx<)_kLx0w<zONP3rCgw&w}{elcBc=d$k>Xn^_Q
z<!bLk_R>`D=awyJppSODbbPy+u90HfAj_Xjwt25^>0DxUJ@)D?8+A)BGljlIDygH-
zTPSnyX&%B4nV6o=r&yX=muuyyC+!}tw@>ul{(IK8k<cmJ{M?rQT;d#fJ_rjHk-zz}
z^1up4VJxMV7)PH=h`i;Jk+e8Q5TLO=+XBvTCtf$Ebi@5J53g!9paScEre2+vI<A@;
zV|1`Mf%bc?N&m1dItYE&ZlO0T6z(Jxz~9(7`(+T)@_8H}{dTZ~WRL0Ez3Y54sAY&;
zd>D6GSxfoM8312yo2Hstvi7p;k?fPd7mpcPI5<d5yR3eT3;^|wc!*>LnOvRrtM}*2
z+{npA7DnZg@*Ww28s{o>F6f*~PQ6$Vd}AU1K3UUh0=HEJzY>`S!L4FekC^apArVcO
z#o?rGD=u7_F0u#V@@QN<T+jGm%#E(Aa&I*(2C>%6-zfz2b^4mc%;n;?Q?XsI@!4(-
zw+EtC(uyw3UHFjlt-|5*v!<3P{E2{j<+)}08Hoz~6Td<V1Wq#Z;tkTm2E1NvrXBGb
z!PRT}rNz7psA*`*qVG?&Xv#96_6h80tDo=N*3_9@;%M+<+C|#8+I#HNp;E^yU(b(E
zGX$-!pKB{J(2cjDXmSr`NO?G=7HM3$@TvGpoxHh2B)@sUUDvxMQj+Q(KOA&to6%3P
zVB%Z{WiyB(N&#LEQ~p?6|LZ9ZVmW1mV>B|=8x`X*+c+Ps*cAf6S*RX382^RcL;Zpc
z;wm<bS88cPCRcQD2W#RMtDAE-JwI05>X&Ke+jEha9NYIqu$lYe=&TMf&G4#i9t}Ua
zx4zVdL-z4&S<7n<n~G&kc{--*=xDdTd#2H8<tTFPZ%74Ll$WXLvKjnxvD(PE8ozoW
zKn6T>3K+XcXFeW}csZJO%rp17!`W*o_M3E5E5>ErYSz%X^QmT9E-J6T1X($UIJx-f
z1kyY(nPm>zvob%G%UF?+gb8;Sl8xrh5M^pc#aV95zOK2{S=5hz-hb_W6pgcL2^r9q
zhc=Mli&>G~LRFqNQ~KHGU%&CJGG243ntPW;mnH~s1%I>8Lc*MTtszlNPkQ9)5?nBt
z6VviL4+&l~kUxR}HewG1dg(-XdYPOX)_w7YskEke68Ew$u{pH-P#wm2!A9|i9WIHq
zdqcl}EqJ8QTeN9tv!7fYY+S5xV7O#tRrhdV&8)Zbi09O_Z=s62*qQ5%l5oue_R<-u
zPtbz+zAgootWotQdw~lh5dn=Nzt8mhUO$#9cjbccL>js%G8U}-5g%z<lXiK=O0vw}
z-)J(*ZTp|Y7$G@Qb(O2Sq423P+jCb_?>Cio^s!bOmj*Tq-$5FEpkW?!A>=NwAyMH|
zE$ud{H<8#iU+tnk+T*wH^74OT@4cg%YPY>nd=*hp5fSM`ML@bJ9V9kDKtO8f5s@wg
z1SD!;z0#$MG^IsAq!Z~<BV9y5x^xn%)C4I3l6W8Q80WkD?DyT@-RF!u?)b)c?;i|?
zA+GhTXU%7p-<<O|-&_2|95#B9?8=rvC>ikZaVE;h3aR(D4(Ax{9fFZO@qyPq(c=6C
z859}N&Dx&?up575(a$T8P&D9jp{NqFIJ&{`^wot3s%VZ4^2ZMS!nZ=)W-Zs2uHKh*
zlhUf1{>>5dM9CW)ljePvu1eh8hV%BftqK?)U4I9vOfVudyieAsB1AW)mZ8crzx4T;
z>w|?pPLd04>%z_t9n%DpoR61$mp`J|h|7)%<YN*`|8glcf8&eI7sVB$yiPs;?nkp`
z?NVmOkr@iE8om)OT3Mt}ViKo@KyP_ovGF|VqYq1$#$7#qmd^FoHiO|wT{#~~$R8pK
zLWe=U`+F*IU7DJ;LA^acjLe1)gcke#1tEfMHnb6qXHeCei8+i`Iv3a4AWm8jv8#5l
z^c}89gKL?Ocz(PwJbdyf^O{wV2sih##n994*j(q1LNk|SzqJGjVd7Qop|ztcHsew=
zm#x(Wuk|wSMn3=YyhAZl?U)B&j$x{;lJVQPBF5-er9NHt98+<wIbGS})V`X8?Fsnl
zkJTFNkHUjBopcEvOIKVqGDL#Q(}wcoch-6)vH$nydtu=kAZLJcOj`Yz<RW8(>u%<n
z2e_}4TR*!qu(lddqH$$xWV}n_nxdFaCgC~%_2Q;9X~vId$^{J*#1fqAbM$#5QiWV?
z`l4l8SGN7k^3gf1<?jflcXQ-io8I%X9u*%cXfrb&9850&1;T}_-$CD9&opwZiWnNM
z#_o(gT4xRrrAa=L&u~<ho2zO6R)qMfSi5oWX3FQUiIv+%7S`F_)ep$Gwl^+(+r`bF
zyH>PEK(;4QPvYo?{A5>p*K!MX?=|KMZbcrSqN@J6l+H)Qm?9F5(o`kIP785$zguc}
zA}ZS8@+x&z<n+p%xcthB|8nD#$}7sBJ$6HsJ@di}3^U`iju=@LB(y$il8Nd|D9*&W
z*53%fhzn6G{9nOC_>VP+zasC1Nnu2$^?4}yU$~^CRpkiMY?Oe$$pvU6r@FWNS(nBU
zchyQQ)fe`ax92t{E^LfcDpwv^cI{G=1sxguloHT#-rw#AUqK8)9ot18$dSw6f)ZxL
z24;jNFQ*z=xaL2;QaJ8x^6If?jy%>Oa}*MWRPc<@bUvJ95;H2&*fj&3NPLNOGb~Kt
zgs#QTO*dan6kEHKoZD5&+4@p(h<vyklAn+&8tF+WJ5@X+qPW#Val=w1aWuiIPg6TP
zrl1?_jZGXV<pw$Hj!+wD<RKXpYNymmBda-fsMnM2l{9?c%*89}pUcx^j3@}rQ7*Xb
zBXmU0QQ=sV%)HBvXQXM3kljov1~2pVQQZk;9Ge5FAiTN1NYMz*apa53s=5x7sJUff
z%H+B!bajd}27*51o+j!!etoS7gwK~_{<<FIw<r$qV1?Dbv{QU4I}Yi{Etw4niv0oE
z&OnRry6IH>FNW6`yxI!=8J<uHO;_SKqo)o+Qxle;)zBXH*PGxocmOJA{%C#gf}-zQ
zmnweAZkGOw;Viwm4d@cBG}<l%T3mPi{YI!eQ1e3&+_*tM_#1$t!PfxnTlEY4ueX72
z$QsZ~=cYDqd#q0+KY<3?J81Olv{2xOWX7rNgQ8Tr?FMzfY<CUtyN!VNH2RC7PXz;<
z703rgf0953=8tnb6Z|2Qs!oen6GCflN75px;v_8LOrCTw>kF#y`;J_>`N-{b$zGIf
zaG3r~)P2Ei+I<%Ot2@p=sYw!=iJqVw)#_h)ob9ZhhyQ5sd(|7t@<8#)VArEP&lUso
zq{<9YVs5Kxc27*|)ol4)z{sHaM#`x}9Il|$QQ}=1*<Dyhovd@7mhnp9!5v&#YLgY7
zP)>cV*z?NyQD>q-LQ0wLIQ<V0(S5&ALyIkEadLlOnmE%s#r!sWV_d^s@<LKe@&jit
zhJ*`Yod+)^%A8k58C07+KopMByYQ!ZhDJhHA@bbK>nx0x$dG+wu7O}~lDQ)PVJ$_D
z4*EAbe<>hI;nkt|5T>}D1O&*Ei8<2~F5<Y8=B}JP4-o=914VKoRUdVg2NygAEuTO&
zXZdI(rNZ30C;OnP*i*VidgNiDQ*0iH5er_i-3ai^eV%&4gGr8dT^X(D_@r2bH?T`a
zw#4jVvZBR^yIK(sYU#VhOB{1-vjbYQNAF!$k@lm<HIYT${s*+t)%IL`YMktUHV<Y+
zb-5t2V;cL5)U0liqJw^XsXTZ({zBeBP0&ofp20}!`l{nJ3o9GuIxWH<MyWL(!syz1
z&fwX8)C}c$p}i-N);g%eRvofc!^+Cv!^ZA9U2BN$l_VyM+6SF)J!cWP{f?pqqQAM}
zm0dDB0TEcI0-T^$WzBXt1I2Son%P<2La41M106v6?vm_KA(h5amt0XXSd$QstY~gn
z=;U9;-XRag0?(MW6<Zb>C5PvuXiu2|4DmRkML;H0ub+CIKLqEMkA6IJ8(pWux82`$
zNYwMv8^qldkFb+g_3rdu3&&9Z%ZurM^lhTJfAHLCI7P{x#tT??<e4ww_y+-jQu5mT
zmvKw;TFJqtmoLlzyv3Vz*5v-98V<zCnR6kHb<J$8PtL#(yompRwL(jr;0&4(D<_}u
z?@PUC+~gqR;d6d0*r)U2z|L2B<qD-)Md#s`LXKlevQ0CKZCo)0Vw5_Yr{iJ<ca4{n
z2THg!eU3_*8`bKiWz6`CQjnznRv77S+U1@WJS&xx$fAqb_8F>}&>OZOwMFHPEh&$y
z$@D%X4zAy;FHKe{(i2CxI<3D)RO>u;GBn`jP?{2;vs2Z`BC9_87pY2k(hwFkZHf8=
zqbEZ_+>Ugr5e?|Ft&eUjl*N@^r=<vWKlu3JNaZlMeX@e)y=Q=KfzDAktR_K`o~de#
zc|ku-T8N>_5Q=mfEfN!EteLC_sM18%<1R^}e&?;!;@!4*b!v4!-%C7r*wp&1O55od
z!YEdoWjq)CAO;E8pO8mv04<KH(2N^<>o;c}javJ|xIFZ`($TJr>sRZDlWpBFG0`&{
zUMjoxyAE<a5;Z{s9vqjyUb#DiNG8g#?kk&FIUta7Ei*rKJ50UuKWr|yaX6X2%hofV
zjkmJmt2q}{#Hho4%m@9ElrAEfeckV|Vx+wNvC8*nm}R!LPx`oMoJepna?Weph=`1T
z+@RLFFNfhE`G%^B<YP|uV(wr(XH<@0gh&OS(ocNrn`xzAs;YW9X04yCAv+;Ocvuvh
zyta>cIo5o{K>VaJqHR*Cge(%^e}(>aQlS@ez@6~#_E<u)Pm@}blR_${PK>(Pob9Vv
zRYR}KN}oa_n<_ti?|{X>@Q529pYTg3UM=|<3b*(FHpv4T2Bt7dWc;6ih)du!C5eP>
zF+ztnB_+>C<Sk9|516iXTk_x37+P6vt?#q2X+Dpw7`<XOR(`J~s9Jm`T?Wt<v4%HX
zq+BPizd-Ue2tcis<ZUc5uMS3l9YM$4AyJ2hV!y;$Y342&xjZe}F1qL)ePqFsSKO&3
zX9st2B;7N(Ww?tyBREPfh$zL4OC0R{F?-r0;$q{>qwtR_dRMqjq0xrPRJ+&T&{nd!
z*}>Tw%u!ZXwKr0}l$_4JcRzn9elG0G&l>YcnF_BLM77%Rruowzo0q;@Xx$Uicqeco
z3-`hVW#NNT_7Uw1|2EKYh5{o(Ag|p%G^x^ijx3y#K|M{9&vwmRI;ABQ>VqC_%Q~f%
z|3j#uwcr9v+#yb~{vr3T#LMl1+Wn1_ycDH<G+JDp;+pxR;W+Tzpa3=T1~NQERmJ{r
z>|2|qbl#zdNxFlG+a<Rp&f}v+&^$yHAKw?_r*vNV8L9WxU48H0FK~rIkdBglA*;`h
zQ6wz{td@J14Tj{Po@Kcx_{LfH2%1M2xrtecY2=-IdHK~B#w#t!nxPf=QA@&9+aks}
zt2Gm?LRb&4<A0Xb=~%$8x3-QPki?y>sq`t6??nA^twZed@;L`aPVV9rdBlyNM^qD4
zHRfo$6LLLXe0pN1nlw*sP~APTH9kv2k+f`Wxl*bP?~c2n{}+#-x2b$~9SDBI!sTyy
z2o9q|6r%&rb3)@~E@y15sF^3xmt?^g4AYf^MsqWhDgu*JEbtd-ul+eGf-^H16Us0-
zra2k3JhHpu*)i4xYpM3OGiN5x_G8Wwutz7O#C3bi-9${HmpXOs=MEzbEgSiM>Yh!&
zdL_@|U2w-2VTV`LSTJmWjSgbBLNgJQTCFur!6l~{CdVw6(nb4MyqBXacAAo{k}C_c
zM@D`;tr)+1bvs>pb@~~XyCi`*Qm}ZA%1;EiM1vH$7{9MXwINBR5%6J8$0sigQ4f;E
zo7IE{%Kcbd6r2uc7ouGaz7<QN&d~?rpX@06e1$n`g>1z(GwPyFQB}c3VoB?_N+?m}
zNfm5F!x?fi<{HVd1$msJ?ItzcIJ$D=hhz44r&1l!0q=`%Us>{gTq|B(7;v-q5)Hp;
zbH6i9k3S>Ps8cYc+XIcM;eP)h`<1qNQ?o<0gSb-mO*SNlTx8P8cGj@AeN}J4;M}>D
zE1EanO(=|)xN|pu6>~UMlrUTnhUTkom(`c_xujUb?Y%_Evz_6See`57Tiv+H*oT<4
z>LD;FB)EVKQ{x?$T5EtU;}78Pv%bI|Za9ZlXyf7EstSKt-V>4z>tCZPI-Z$4-&dy&
zn!l$a=4*x|*S!}!80YFckDji2o&5q{B9rLU``Yb(xY<*gySJB~@<rz?xlX$&nA5Vh
z#EVpP$@=Z<K~EJzSKL=0)D2-^0CK!c!C9A8BnVi#zTWaP@J^G`y`f00UMM*whfkj8
zxW@pu0gC%D!$Dlr?SmpK!*y6BWgC}INH()N<|URs#;#@O>0R)Ji!w`5R4ykCkopTk
zn(QA%DT{ezY0Kr9riz)S$;oE(>OpX94}0WZu!43lA~@S7IWH*DIUFlwkzWW5+Ps)i
za?qTix2@izC?V6#gm<Gzeg99`({LB*ps9-C78o171ADj}7Y$=`Z{S?A&?_M5S{I)g
z?QHT|Ry$FRA3izentX6~Xf*A8ti#*M+u71n@@v;X=HBCPd{F<VAcr8QBI!tHK1fCw
z^gD9Od(nGrgf?=Fy%A$G(jj3}65-L8zBJDpq<XeOb_vueVu)`*Bia9Q)SW<)uX8I-
z17>x|VS7{0l$1zRXq&-1!s+CQyY*>fPhr)hGbZ6Jq{&n7u0;0yQN6$v+U#dy6!|tU
zPX*CzlAA4DKRf5T_zHkeJ-$@=9_pE4dhFOVYJa}~`uR1B&g)2Las9<`dzGX>qYdJB
z=MkHN>i$pu;xcQF{O{z}1Dc<|2_NV|PFXbjPW<9a!c?s+2o3UL;`XV5`>X)SR2TN$
znFCxD&2zgujz2;2^*G23FjfGzp2rkkI(=!1`V+)1e-}Z}j`V8obtXZ*=bG`4oxwGX
zU?MxaOxpqq^zb|D>z#l;B((GATF*l5Utfm7-`z)ik1aE1D+x5rW!?<+U+M3pzgN>=
z-(X!)MbS>bnVL!hoxQ9{3lrFX|AoVrrK|y6OXvyONKO*%4hB0l3;T|q+7rEk$4i6u
zc9?WeEj)ewuR);ZUqlMGs1XhyvZ|%OIKjVPrZ2tsdKZrW4F5pJZyGv6{zyO*q=v*k
z{x3wCr=fq2F<<`wviN_=Vs<XncmpK)a8J(^WulQS@DEAN6XLCtmt4P7*)p{E2-ucK
zKTc*(Rwj1ATC~m^<(TUnZ%Oz5X8(Yf<>fWNX7P7IyN4q0(Dll;fslPjYD3a)d82-g
z?O`hm2HF>tOnI4J4Tl~f?We<~i(cTmXgMfneYvk5uCWrW7HaJ*dOs$9DyfWic--5l
zYpN8H%~re6{lo)3Dl2L}Ek#hRJrIr=Zj_xWZ_%|DS{q&3LH&#1`(K_?ob+FhY4k6K
z|6LCNH~*EM?EeoPQNHLm3w{4<E0LoNr_c-!u-E<{$E*BnLAeTOZ%KOVlC&pPv9Cep
z>)UdZTC$YX)T<}&gubmUt$iM@i}GZdVhVRk<?z18efw;9H#o%$G=OO>E~p)a2+gDg
zz;lFn^fmRB>z|jah2wPZ{j5yJ6v+-Lcge0L)9u|u#T(V-v0U><a~#se#dPKD9nw$j
z)E8r+D;tjgt{K`&q4NU9dyv*kr3eAG64qucHBFPM0AE4<V$f6~=>XyzSSWl~8M_^c
zU!0orgU_?70;`%kiQo5q+mOS$Cjp>Uib?z~G$4{QwSb%L`HMCAI|pSMw6Xu?9x)Vm
z==&5s+G{$A&IdmMZTP@OFN38)37Y_gRQ=gW&jS4xtCP>S6ro=G7?71H%;}_OTvY?y
zTL7>9D{%OC_@p_WPz{QopgpCY03^kOsszy30jSxF4)DoMFYF;ai9qoJ+&;PV-Xt0?
z6_-P8TmY~r?=XPjHd*tU=@AkTox@%Yxpnl%&hGN=mA^CZ|H6&f>jGcy*$1vjZ#U)?
zXb1=;^w9NiMif`_)O35B!hjvch^+Z(>f~(S4fz~ckgC>uSKPhQnNHX84<FxTAU-p9
z|MBV?(*;wXD^3Q1d6s`}gd5MeyCis24=Rd(%zo0zmsvcKFu9@SEtPJw)Nvz;)4@bU
zI^;B&+iFf+yjDX|W8zh9>FDZOPYdeb^#f-A2F(@B`hUxxf`hbEz<YoaUsvO$qA83d
z{B|BKmU@&_2HL*(Fn*a7ah>|gk3*S5!q)m0ecqOxtCw#QX^%griRs?&dqS+O_`~>K
zY+6eKJyLuZpnL&M;B`-f>q!DrQwy--r6;6O{M$W@j{ZR-X~Ngeqe-@pM&CrU^Q-Fe
z@#0qaNZ*%UKYbLAvncQP2%q0s8C!jjsHti!^JbEXJ*ven<9Q3=+>K%(!|i-IlJ|0#
zR&S2Bq*uq-M6c9prGYxfhDhJ<!3+YNfSc=^z#&=!odIJ-%TyCuy5yXGl%hWE=hc=x
z(X!FXnVM`}AeWfpZvRT|)jnb%y+^x$b+B$MWqIrwI#B1lC<#ju-dP<a{iHK1?4PGz
zrsezd-$=GkiaIw?4oJ!=*2S4M^(5bjP4V{9A*x*N>vu1+yE69W1~hIIT@7AM8^tAf
z@+!;&dNlKW4*yYXRn<yV4u+NX8iN4)(l~W|b8OX-h-@#wa#8|~`g~Z~of;1HpC}{r
zF#o7b_QyU>N<`&0tCc+2ev=tF>74w)-v}fQVy1BvIoS4j8mK#Z5ED>^KdE+(Qbt$^
zgB~D9t3D|+wZRxDC@u2g98coZVf30qWtmR(=Cdee6|T%-DxAA&=y?6K)0%9WQi<Zj
zmIJh41NGAk|M7!J#(<`>S1M#XdPffM4YoK3{h*#4xgS3BSRVLnG}R2W_BzktY3f_Z
zpLDqfPTB%1;D%TPBNjftLC_gdds|?Qw_|r1|32&ToA2xJrf??qL&yi8qeTt~$~u{`
zkg@#oE!z@$2kB+Bc$^?U#Y31~Mf>^5thS5;hL$@P4S`WG1&)Q))^_^o5+3d0KGVg~
zp<#=ap`M%VgkX1pjxBv!KwwRMMbv7(-m>HSAEV26L2=GJtM~uNW`$CsNd2GSoS1X@
z0POG$D%a;jOIygh6`&nkObwQJG~SN-&GH1t)|4mBz8phZj7{9cR?A+TNhl_U=jwUq
zFvl;5`G|hG5`0^$$s^Sw-tnk<o`!+5Vux(M*84YCg2XiLrrIg$NnHsdW^-$3_eEy*
z8%F59Pj=36hzXX1C?Yvj_L*z_D4#U1?faDDSwDE(>XM#@<mICswLX9%Nn68>aqCHI
zRnzTn8gI)wWy8V`>oB*I5G$gfm};C7OYjqHn`EG+{iZ44M&Cp}FD}<-N4s9@t%zbZ
zt<-w=yjYf|*QQk@MPOH#uz7f@hEwA}v^WLSh6cvL9rY>FN#UwqWX@KLq!$<yGIIMk
zeQXd%<pjraq<G(~qF+^#AT4(y&6SKIY$t4^PPtmlW#_8b56y?RSn%N%I$b^Q$6bv_
z0>eYPjrs!!uN^R50T1b)B}u3m_=*W3CIm^!EmsF9D_xL!j*^j&p3&+%<(eJd(=xn=
zig+kd>~lL+d0Y$S_%MiYNBcSl5E(Z|*rxnfa7jD|AS%QpjeA1&bJ$I(PY^*E_KP85
zSNbFEJFt-M{-AKg1;z-GqCS9L4LD&8`vD~SLmQD)!H4jshTDK~kA==s2Fxc5*e+nX
zngz)K)lLbA6Tn&nR)F#cN~rNFob;Irrh&{8V^#}i=piFmIwujoi+Z&mxGyaMI4B<<
z1pG~xK)8zQ@M{O5KWVk+y8=Dx<}{AZRtY0$QdPXRL5eHuE=3&N(k+yN=zx))LyeE2
zMFM;}9ZPxO4?O>$o|@FNaFTV*o+1*wcsjRq#1wVhfL4(TeH*(1Oehuci$QV&>G7AR
z-J7EGSMP(wTyHK0Y)O-R^KUBMe}4X5LHbE)ic}AcPnyc!54u=lTG#3B;EVri6DrGM
z_IMp23NjoN6g23pv98pW(SyIUQZE3FzJV9ofmwnrtAgK~+LL1qz*987L&30rdUoUm
zs0o~DMxej5g2A3Xg$S_}P$=02@)m!$1KUHMzZh;l>;V}YGLl-=;nzq@N`$`E9stXH
z4bc&^4KvFS<?+G_u*@16B(Nh###l*Ne|dg!K(l!bny!UydH{kKed=W-Cw}*1*5B>`
zeY6|C==KJ5M*D;JlL*8IUb@k<$9WG<iUV5e3pHun4+XrdaT+M77*JydU*xr=b9|=g
zZ-Z3?xKLJo9)lxC+gGF%q9HRpiCznqMUQ&B)RpC$Y~7NW937i48UIr*B-`HW^h>Le
z2?+$-?Z_goR?G?phz{d-RjBy}^bc}|e+~{HbOM^LP6E<o$~s!SzkRBX3?xia2RRCF
zzl5;>69@Flrq@TXM#Jna{lX>EVXJl47&>PSsb8;f;r2lGpwEL!liM?|rq$#2T|rq{
zMkQ!#3Z~%bma`4h^v+KBqV=CZR_?%mF^F#<J?ZJNBcOH;K7yZ?D#=BMPWLpTLZ^;b
zbXn>vn`RfF^_#bot!eYgj^VT4yS$ipqg;khUcYjKOHk{W-lup&wdc50=)to7Xn6ZH
z;&~DNp`Q`Kw1iBTyv#@jue-58_i@jsewzriuV0AtNmymY=ag>F%k3QX@1iP-iimfL
z{P?Zp$Q)!jek)y<kKsiZmJoT$6+GePMrm5wx<Z+{Ba&>x+EF|wjTa4bG?Lx&z4O{q
z*`HN#U2Iht9Q&=d1+Qg>Uu5_<e-0A$V?upbZTC5ERXeLqkTc!hJ_ySqrH7OPC)3vA
z*0G5L9;^mPH;_Hx1GI$ezzv1%ZBfV+kbP7jDDOY2Uzwt-kIMkIw1>d)?g~^vX3Wgh
z%y{hpz{!GTt>bdcPr!U$$Nrs}&IQndtSR;ec{G;J4BOX1YxK<Mw*V~NhLy~im`0#U
zUnjfQXaA^q{N`}*+uMnzLp<);_O3inc81x@(=D~vW^2=BXx#{s*S;^1MqRw?`aaLw
zUkv?B!Q$KSLm$3lkR11&?>qJ5bmx4oj=nV1qM?oy7vne`2K)883P#F)N^^K}kjAgx
zC>ALEp=(E2zoDl<YxUzVcP&aA+#26)?xBnU9mq+J0jCas2Au7^82Ai|#s`|VsZvx+
zpk$YRIN_F&TM*FR8Sb3F7#N_1{}Y*u*`9a%#c+KPNU5~Gv~=r^67HY>CqU_=@SC0x
z*alK98Jr{Of@}b&{nMpvY1JvvHy?<~(%;!h_Um^2x7L(JLv}!mmO!z5g6)Dzh3}BN
z!_+1}K)wFh+}NNZy^?>@<$+pkrP={eUvI@z8?yd-;Tj8|1Q8p^y)y)e#6<T8co}T$
zO%;s^0j^uGT8HNx|4!i`Uzq{Cd%(*X$#$cwQcVZX4>!nSRQlvttZ%k8T&O9l?w5mc
zEn2u|*?T!YDX_Mfu$(fRw6fIa`&Jt76_auZdfN)6iv$310^#BVFt|h-4$RILoYV?s
zOhA(8<-n%tvby~AGr?jf_tKu_dm7;DZ`k~#WIKw=Jzo8~`k-e~`lzR3q^Z$s;}+9A
z3&<?XQAq*yxb(55cy8Oa_BNtE02Bl@vku+JU3w6uaQEzV1aIk)CDvFGxu>~H*p1@@
z&~h6PWv$s9{^5LTt0Xpy8uQbPq!2*8N(2?!88{2wXXx~E_((kz?*3!L`J4ETw1Xv&
z9i*nKFU-YotpUYnKw+gnzXDD77=q`>@1|(|Vh~URQc;q)zlSIq-jhzWy;SzeRzy|q
zXh(0vqXe->asF<tRi#tZ9->t<$BPgJHm>{P8e;Ebqs-rxNA!xCyYB5cCWE(=MS{`(
zZ~&?zJsUXw#?U`~VDiHl0S$bBHjjpY?|@RF-QOswVncPrFNU|Nn)DNS;D0><#VYf7
zQZAM07sItTc#>ltv|$4Kr++^+Iqj$O`wOp8RCfJe46&df<-^{*G(Er*Mp4fB#US)s
zwEw*&8Ei>_Romi8x^TKCfR&q<U*q?Vwm{QMSZWdgh0|Su;}P=-tU3iauuqe~#SzQv
zRNh@%w=DVy1lvrzYQw^U479hcy4-p3E|>$B#F&imeSBE6S>*~t%fWE!SKI(A<t#)^
zU--?&_rM#(K}G}%>R4;!Rc2MzRP6*F<FV{R6UrWeuA@f1AK#7E=#3$*d#XuG^-t@}
z6Wg*~D$eH!$1jLp*tMuJL|Mu=plabbpqubv`b#9BE)V|Cf-O1*KK%i|fd&Jqpna)r
zXfGWjk6To0r~;?@)w4iog#{e1!3w*D%h+9hPdF`jZfdXSEoged#BSOGt^%fbD$rT4
zV1UrEL%&6zc1tdtf}$S*7U5zpu%;^$7*>F*i{Pr&RKV+FrF4Qv%pr=<GkIDu3cx1=
zURS&^aO1Y!8^9auW&g(XWAGoR=RL`3i`f;#pMkF=qk&`__#vpj7?d|_e(FcTH#Gev
zpb=T}Fg<fGXLqNFgkM*L_JE=T%y_rH;7Q`pa6WeLR?KH<io!7cj`B}>r(+C27A6Q!
zX`gY;(BFNUF@xL{2fo-o_kFl5dR!XNj&JJ&ayVIN^ChrBb3N$)2pxX^eh}L(i#nbA
zs`Y%S7GF4yFYFVT1NcW_;n8D-N6x;y&Y^#&ue|Hb7k&lJ{+03~<?o7BANq@I%gU-w
z-R}DsFghU^BJ_lR);{Vqr^CYsafrpYClKvw7pg&rBF2bV3uj%TOVpC)1M^+w8@NbR
zE%|C9OO#rl<!f(0VNq_uxVr-vqTJj#M(%crxhr--LVI}XX<jhI0}DI~itu_sgW971
zX<FPb28XH9vDQi1%09e(6c^=LTLV0$v4OYm9or<vmRvL$qq?0g2zag}<&Jn`na3A9
z-d)ym_I|~24v)*HBk|J(_>DAa$~hP<63haCZ$@|j3D4!(72F=FpndR>e$LYtwSRmG
ze%zZi=IMOJTW20bgqpaem5UlX2^EU-IYJ~$hr={0(O8Rt@-&GqwKv(dVa3JNidgIG
z99#98vb~nA5<ffnPTu70l<ik;J#C<oJ^RX>FJF`AqQu(o0i<CJ$qaNA{KJ^ADUt<5
z5tL6Oxo&@?Oaae|k%U`aAhFm``lkWoIIr6AWh{d#Cn>~@=?-}<cc{yE+S<2S;b};L
zphehhN#AUJqPe7d`sDmJ%f3S*uvi7i(|`t0bD9Mnf`GBrK?KwRD~!!RIgN?1l@u=b
z%vuaOO^LhP7hM7i-L&=Y^{X;EBPuaJc?s6}BIRH<Bo8m2uL384afDW@#!deMV>&hp
zLc0EKUJ5>gYDvp&kheIO(un15xJ+T*!hfJ2p|D#&clVxRci!gYl)z|uI!YwwNBDS@
zoo}?0Y{gNOvIq_lWM}yFE9kI~wZ07i1VZTNpvG%;BBXtj0gPx~S0*K(`8t_3*xmc;
z&3%}0Rm%{kqh56~SFzykOQq?}x2%)v4E~eYlF(LUghk9JEYPRr>y0tIRAirS-xNlT
zG!<QLZPcPA6kco7dc3HjGkbQ(m9NP6czo5Vu_N!UynDub>>GrN`Ciou<8cRURIL8}
zQ-?vTuDov*k9~VHFDu_-m4KMFUVrz#X%c_96$jR+^3okC+2tqp^}(63_RqD!_TZ>|
ztl)r;{%(qfjr`o^BI8!dNO*NxGW{$Cxm`kmgV`oTMB_6j%yIImpG^W+pGEPSC7F{$
z!V_z{Ym1LeD;T(1baicKZ09*VPJI`}JqdjAZKzQR4{ZH+Kp;Cv(wq@l=_NTfVOjiT
z=-p5`a)stefJHR$_7{O*@5Ly!CMNt|zg%^l&&)F?;xN_LtEfwHP>v<)(e{Ssz<KlY
zGHpG)e?kw+5i-+J8Vd*#GRViZ{iie-OuNVvHGrL}uJ0?7a`ac+%}z)m7pMQp+}=yQ
z`j+xk<zqQNr*>}^zP@Gv7pL=~;)7+E#<pqt#@?c(I_Id=+I!!`XD@s^B_hqo<C4Gs
z_on2i2~rjS?hNe%IUUBe#{mC@Dndukkh}b<k!(~qUeL0N|NcdO4*mh}6!9m1bBLem
zlPj-xw~75ryN`COF;(JI-5jNBoIg8cSl5Yso8*n2oyoJnmRG=Nh7SnprxAZXJAiEL
zSp_FaRGx;zueb#MVz?u48n@1a55$h)>4t9mNa*_@6aBTSD6#?#Vo#yRBJ@@t_?Gk$
ze{<pjHO?4XQG@Npgu#^{=a^^#F$&}}Eggbn-keZDP#VduW^tONQWqkkm#Xr9tj*Gt
z{1|u4(q_I#^TsPMu7`R2^JDHywY+bz)2LMg*ZVcyZ}zR0cG9KHZf!Uv_cWsz6`DuZ
z<>z&7m+$lHAWvDEmD!*3CG&T+2%b_(4G0aTbb4}z;8Dnts+Onw5(2Pc>cJTpqf?dd
zu{>v3L@~~EiNz%LvwD3)<%94;7gV!v2<vm~@gw`5_;c?2vSaUie*gL<?T&y?#21g(
z4+R{fKN)?p{qXvsV77rlBk^KWvEZ5IPQGQLt$B^lVt;UMe<W)Y*(@oJ@W=(NDt}mq
zGx6r77NU&eF=2Uh%^+DZ-zd8$BFnr#D<tX&VNmQuT7FZJmV%*H`_Cz&ev33e8a|`2
z@|?2;Vjn@@S+YR3Xf+6rIVacEK!G@E@2<mr>glsqR!4+N1MUx>fgU<u;=c4w0hxng
z0*Q;`lr$o53^SUAhzh3a5>?C=lC5F-bx`H&qSx;>KE<^%PiV3oDjO~T!oJYmliXlO
zdtWC<=;8Js0B;<Of3+6TB_EEGTxcn%>faJaqY6dRJJtKwB-1UQ+-y_~K@X{Gar8fp
z`r>Iw|ITwJE(5)@twnZ-uyua7TnXowg&%9ob>99qYIU5+q|Zmat}ppE8-JnD$(%h=
zwcCQ{#3NW<J5>8UoI05u++yE5bLRxYFb#!L#bmr`4}G*?5#?jZ<-*p~!&Ab-y7jcT
zMb1?0J$IMf7?Lq*=7;@hKJjnuCWJzF`P4<_ON~uvRgEW+cMCL(=Pb&Kj$O%Ey;?7F
zLoB88oRrDt0%)D(oz7{})i^FlgttH^C_2W~m6o3Y(Dj5qEL2dU{_&)x<@oJ$j;1D$
z@4hRzm&)f&XQjkWBSGU;HJP;;&+XK3q3^NPbs8>fLh~6%RNu|{`}5~2&4i!ZC`Hw`
zcvj!YaW*|fts-J0(<RBswsjsrqRK%EZKPi$BmVTJ3G@?LTVnWaG~YSZ+^Q)zv|ilf
zleV+>EpnAQuhVg%D*Coy<=OaS{9$D5NM?Jx06(n=a*<?e8){(etI*XXECkkR<vT;i
zvj&UmF6>1O{Bg13$m*7N`0UcF(aNqHGhd98AFTxyI8*_=-}CZU62&Rs>}%QOR!44s
zUXGgwqZ>7XF=dRe?f6X9DuunvfF5ulUK$j;w`iNe+|eE7k{=)2UCO82xr~U*3u&Jw
zy?Lxw#QgEAu+U&eRO|GmA9eg<Yy|;&+`-4xr3+1qMEgtAE{esz+Djq8TC91SkcZ_U
zeyRp#KZ|;oM32HPXBkq`M<s0d`z0ikEZmg&d7g=Lyt(PLBQ21)bHIEdp2_2=aP2B=
zJD4IugmIZs4&I<75d7@gcoJ%@Mw~3{mu)8WnXkE9tyhKXI8B^0eRbPDO*rxoevSqU
zYHk?=aQ7wYa)F=Kq{y!Yzf(=C4&yE8tmvCflcy-CX+MQ#HCXSaI9J`3EWN}%8?38C
zx?gvio8;Hk*;~O29}`Bk<{$NS310cU(voZ?m+D|1zwuQ_k#O<eBFUH0(PYkirsVr*
z#g$R+Ok8lZAX|hFtTe=2x0yF0FS4cED^KE<CwodByK1Kg7uO6~&PJ?KIERfajJP4|
zpBL0@6pf%^mC-MHvx8@?`r9icq<FZVs>X(ANhk+I(!jui4KDzEL$o9;h*9^jl>7K^
zYxo}azyF+(yieEM)u%ll{>3n1w=K;C&Fa5~zN^E&d<6Lk3im#P3BM8jdQA)sJpq7o
z)r9{^R2T4{BeO6-UD!`P41eK&jFv`cIxfumB3-&)m<$h7H7T}G%g&}8Y&^5WYA^M1
z&rrv`COzBpanTv9z2tyQnMUgl6XVrdf(S_JB+=U}*MA@doxR!#Sc<4ZI8&JQJ%G#s
zghL>LGr(QS+)yQ{SFR(Z=)Jj0s$5fPhmTMV>{4OKjo{lN`wK9enol0@k$UwHC66sA
zWF^^^D7w7L2+Ii$F;AXXYl}XACNDHEy2YM?sW*K`uwiS}S=tgE_FmGY{AALIEIYQ(
zX&5D;d#6gWD7uNTeBC(9=wu0(gjkw|6uIb_q=bo;UAF9x&ZXrs4deJ3sh>s^>3sy+
zePB7Ag_bYJ%0&s8P~BCPBy)ZeIDtH}^y-*wrD;FKrCoDKHYjOc=h+Kfg&@-!buh_Q
zX|@9NVOG(P(MnaXI+G8!d7=@ZKCGKFc|A<FbHc4iRg~!kXG_h4Q#Kdaze>KZ)%|HX
z`sIvHQqZoGr6<$o$(L`-PUauD@QQ)K=R!IIe+RA`$4N127634Bzzd2TMl0io1(ip0
zg_gIhY-N%5j&pmvCRI(mG<=fI<*0RT|CYhH&g>V1<jZq2=RVzY_|9DI_Ao?APlRn%
z#_DFs`#cBp5Zmac{@E{A+Lh?^dMlmC{wVF&=;2_oL61#|McwYQ!UEo7jSkpR*-L9&
zm^fazb3o(04+-<>A_qEozh!Ah#PeC?n0jX$zA7-7eD}l|YeiL{?2x>Ow4p3J(l8Mg
zib49l%k0P<arV_iN#1uDExTILa`(=+GrO3@UgEJENr#9fsy_Srm;)3?oIW9jpK3(H
zhfuFSJg=avySXFz%v~f>=V-dPrE82xS5#omYs90Mnui-Ol*V_?drYgieTn5!$)bj&
zTuSJ5N)%zj{TY{CrSkg(`}eI+oZT?C#tNctaxGB|uRlfzI*cDv8zzG8N9k~owGv(g
z6E)Wxr$W7cq@N-mL?c=OxF$HsuiS1!J|1*lrB+2(<<^JlAvv_fNy%DkE31q8i84GH
z7J%H5N&7vUKGQK`2r8!2FO9G4(pTH)?B~t*N_=F@Faj`Mjp9?UvR@MfZQhIUDsw&U
zkCJvZHCfD&>A2+XU82pEl!)lOM4`Kkgs?KHURa)yQH0qcxh;<>o6IiXE4iG-!*zSG
zNUvw4)u=#dOLr%#PH6@i3?j}5RTeUnW<{SA=4zpC4jOQ2$?pxNW#}-b?hXksw5bHX
zyN!2F?zWD+BP?q2$hC)hfPAy5fr$bm?YBaw8srz#6-x)F`@Fw5NQ^rJ7RH7v16vV+
zCsMpn??*3u7?FRw7JKs)Q6}o^m!pCN{7Cm$ON}(})KKfplvmQn&8&Y2oF&Umw8c)B
zH-t8w;Ylx;ej+@8yk0qwAho($Y;n<g=yq@N-Iih*8_$_qUR{0q(b|16=V6kjj~ztB
z2PFC?_C@ttS&T0yl6sCsP_-oVKF`#YaLnpQcs|NHHWb1f?`iVUOSq*_O^_oB?Q~zJ
zPV3nFdQpSOL9Vt_d4?V>?Z>woOpX`>4y$dE9Q}|F-S};{J0@DTU?gsKh{=z;P5Zu~
ziF)MCe6hawOVy7QHH`gAo3*X_jc4TF;}KL5^33#3G}RKCMjfW!AT7MWAl_T(Kz{F>
zcJ!Y~*1tP-^H^C*Sf;DU?YWO3@*BlgAs$D%J@<bD2q4CY8>eO9SE8{rZHzcF9&o+P
zKPMDPHS3VJBeI>6eWKo(4n0;j39g7LDyqpRa-FLTmyN!p@i>>E>gyvdxxL!IUd0@p
z$9D(KByUy|FE%_|mlKj@7|h9OA(b4ZTtQ~nyEGR$yHuehBFF5Ut5J6~Q2=9`q{G5I
zu!s*)4F6DM(xCrT&v@}$xS_9SK%j?yLbsb2e7OlgcSOOxy5G3Lzd&8^SXPRJG&P;|
z_k5tvdMrH$d%R$e6<5>)pG}{h+W(^qzvX7O`!$EIX$3$D+b6#C0w<8uMqcpa0*TrK
zb8vnD+J#c>fk`qVvje!~`27ap9Rx^2U&KHbXI}w%5Y)81SYFeQ1|aZFI;iLv5v9BU
zH6$S=NPs>ya^mMWU?7K<f&&SP&0CuX_I&=4kkZDPoJGIA-WsznoUZbVVE}1@)4|D7
z`T<JfOKhjb>4W<(N$1a3fvj2`;TOX)4MtO97Xv73(Juy@eCWq1_*wk)6ms{{a?ieG
zI>=1)`vx!XoX3NE|INn@p;6FmVGk$|un(j5&UoOdaU*crpS_@^>;~A)q^r{a-ulOk
zz`+!90Qx<W866Ej*af(wJ2tJ=F_NCRb}#wR6LmwSQBu}6;tD!>-m`TZ7ORpI4o(LP
zA94jEJ9gUf6yX>uXUz5sS{`JN8=Y#0@xrIEiC|(OK&&jmFC<mEycZ))zTe+m1^gjJ
zB3?kKB%QdlL^H5Mp0$`Vi&q-A!GptgaHEQ--vm*_L|S?%B2c6nuO=ud!8PH>fIFA?
zeG$o}cIJn`g?>7ZQ)MDB(mqTfeLYszNQ<hv0gMPL*O`5S>wUC%K})h?Qx*>aLamRm
zZ-dE#@4y}<H}KOE=`8d<n=>ToRmozC>!)<i-zEQs^wGXTs(sFgv&<OvrnSJ%%?dxG
z+i|VR0i2hcQS22#RcA(mx?=aa)fpfFhL^FfumR8lTs21R7+Pgo<P&Qoz&NK4Qk6)$
z0T#}qZrEo1FxDrz)|ZY$MawmTXR8OdR&5si)(Y~iYsal8bS^}HI$5wMwEq}tgu|WI
z$24_;oZVl3Iu+f(ZH7^&#X`;ikb(It+q9#K%$)=zQ<{?{OdEvDNgRcbcZ@#mBqyaw
zrJTqmwIr@&nwx}(ZzqQ{Pyr|#P)Gj+wi}v$i)T_}#3)njiMXQ_ePfFCz5(?-g-~cs
zsarbVz0-_m-Qq3i(%1b!@2aSYUrI9QE^rbyu99GVbN&Qqk1Pdp3`%P*;#VyF_7yNv
zWc^?ajMy3TVv0o-B~UU0TZOG4KcIPA&m?zeyR_?6*3@t+$ba~p`0j??<zRDe<26`8
z{9PtELGrxc!KMc8Z|)DGEz{VWN2OvtB~<G29Q(b;T$LU~cpRUrds!wt&Fjp~K(=Q5
z?^C!kwP1Phvp?~6@O@6W(-chx1IJY^6XB7&aodBm$Z3P0tVij6Ne|-MlTcY0#i;iE
zmT}?UZ+|}F*q-w{4BXK3YiB^D#}Ck`lrvB9vOO@6y7nWo?(-TTcY`{nmIA<;!Cw6n
zH@&dE4q(?5KwP>lcl{T`dsb$|PSoGx0Hbvv8TzUgsFM3N*uA76xCGgYu9XkphOyE#
zTVT8xCyL;7OmxGUh-QH+Rkcy&`-?s?tgfT#?cXui2~m%MdpYEN^}4}m-`>b)`};9`
zuURDA>ky$;D^uj|wN{K$p8xQqYD&|2HivJ{^rNJqq#4nN#*A#bGeLD9KR!#kUiz%@
zUE!;Mb{F>KeYJ3Uam%cdjWipA!;;ldZn^DbODAh&+w@FCl;Xw>#V=-e)S1m%kNB^_
zErKnLw3ByEh0}eS9l1l!L|)n~FiH?JOkLDu3vRkUDd6SYZRUJG*EF5GTT9!f$#`9^
zo3^T-Y)?y(4FfY9fa~Y`V#`d@|Bx9vP`|&NromolLy5lnOfRHKYwN1ZvyZ|d^XzON
zp9B|mRKL<NJjcNEqu~nXOdyfA(6nDoy)G5x&rQA6*KoEe*101Wa_!6>owSrn61+N>
z^F8aeb&`UMlsPlwnblgsaq(7vRq7Lx!piEjxhga>KQN`u6P1{Nvd<iqq!;-;!_3_e
z@9ugZ*dx>SN?+}QfEAn8xkHkU(ajH@xF6&WYtQFWtx+78M@1A^RA_tKMm7yL>ac{x
z*JsXd`+QIoV!m4;s(d#vTS%$xsja6{g9>j@PEf)_J(Z=c4eABa{$TCWjBqnNN<(HA
zv-ZyOb&gn^o~MbBkmAPm_OEqD`&ZDh{vs4C5!(cdOzvsAifsS(!xOkTcOsf<G$S@4
zHl56tp*{C3#^O9*ds;&NO_W@V{^O{}r4ny<Max6u2qG)xfW`VQ=;6^`COW>L`V?Yr
zliCvAy5Pa!8B48ajNPd>wQ@CcEv+<<RwezRVl8%DxsT-t$9b*yV%<|WDeP@EtSu-e
zv{$<t*GB~l)^OPZYoyMbfWjy@@00{#bw{&>7gg%Awke$n{6~dc3-hv@pIneR?OFbz
zbi3SzNH#Mq`8YyYH2&&D6urL?+^l-%L%Cphc4UikY=*|^IkAR8%~1{Zh=R2F?gT3#
zi(YMGCtDfLCfjo_@^l!DSS7AXXb9O3i4`>W91V@+cPUL3)Rv9AJ0C$Qg?(RYgN?+s
zA<Ixykt#PwBF)ll@BtY`6(mKZ93G?^!Cs7>2G*(~67dI9q<KMQ*3sCbsrhVkFq}94
zMW?3%F`0#JbKX%>&37yV0t6LHD#8cu*c~HYIUDih+S=)~+7xc(0fak~Xy?W-_q`8a
z0z)hl?b%$i&BvrVvR;H(hS27;CT#5@Te1x%E08V|E(v_LogEQhJwMLQgcmR6*?Cq+
z=$+2MIM{7L#Y435^Xtf^kI$MZ9@|iDkWx;U(cg!u|0;Y<QuPPPG`bN;hxq(r*fFCA
zl6>j6c0*}F2GoLJ;D$WMeqb~hgwOKk0>CnAiXPX2$d3kTyR#FT^g4IoP^4>nPCUhZ
zH5}Q4fBwawC>?|)&k&LO+$emuEykUORplo)Ox>s%M9IY*LpL{ZPL28>Bd-S+8#cGl
zdGKr7zWRKzUC!c%6igM^>gzvU;}YF1@coCt)usx$SPVND^!3q0c~0IN9E*Bsq93)s
zNXdyBn~0j^7@m+5HO)4Z$<wY7Z7LFrjr`GlSqn#es3FZ3(B(?>sIb>47rBtbtJ3DZ
zD{{0=tI2!mhGDR_S7e6asdy1?^>Vrtp7y3ehLYb~&I+Rssi_Rr35q@8e2NIBo+GRu
zbXAVZ<<%|DBD(T+X|JK@?+A_Ebas=Nu0{k^BKlY82b}kyc1flgbzGQx(*8k$7xhvl
zX`oX4a;L*+!uz+$g*Ba%Pr5t;dTm|39yHxp6}LLL0ehQUbaD2HJ#&11Ta-&eE|B58
z;g%WC?=?&H%!+kaxwR|B*)0kQ+R-YU%-)Vl2BB4{S-N_CMhK;^Vr_SQ@{DqnIX-M;
zhc?|Km8XgQ%u?<!Y_|PjWLO|^l~XxitiYI<x@&?BLoZC%<D(iRF+!9)kuc%@d5W>>
zopZfLdOBehIEt1;W*H>Ei#zuWR(hnab;Q+Y;mjze&|U2gaF)(cT-$6e4>z!39s_iZ
z^%(W~&>UGLoXX1-R4#q$+UUgB`{JuNn@5XtypGiz8qPlycfXr@^wUd^KM-kXBr$Xq
zLG~hswnN7;DrAL5JPX>3pwM!d<QDHQL2`Uik!DC2%r<k;@{$~SL$u`e`((rQ-7oDj
z;_LyNC=z@orlkupBOQZxT#tOJ9~=`&<s_u>whpO@lCC#|ULP*^vv}}lVX{M(T-VSW
z-$QXthxC0ed6e=`d1GHU-&aH_wA_2JG2VYvVu3%VD9LWlh$A3cg^Q#$lV{X2?Ma_Z
zmEn>|FivM@Te<Y{_4Mib$fYKQ9kIJTm-o2B+If?Rc8SxMN?NC@FH!PrJxpY@D+PQm
zQND_l7KoYE^(|G1SkxcApS%zyzR?5fWp&Caj#(JrK6au2o)|)zxjNCcT}0&NDwgv*
z|E%kjwZM~wTFdF0uV4GGj-W*DMDkg_)8aaIaS2~Si_QaV{-Ts4lPWbK!-U6W(?vEg
zc2?G=pUaZdofI4~z*=IdlRs?Wn2qr(p{?F3rl;?|k0Bimkk?OefBw;%6B_b|OqR~k
zkWc+Nt0j_}EP?G3oyXM`%xewXS`mhB-MV64@p;ceV`%u4eN>^}BbyPTe^t-Qhn>Cl
zK~t7d%aKP@OPwQ+#$HYCNW<&oI9<dRR7?<R9~m6*ck$C8TG)7oEmxq$f><C=KiGAH
zj=u~4>&LUIR=}U&1%VsR133RL)#94%-6cB>N3^w|X_2kpDGAaD&FMF(BBTZ*nIU&o
zVH7YvHR&0grmx0}*n}Z_LaAps>H2^G2?ffTCP8#y)DtpDV;UY>z;5V!plHt>ruHfe
zG4nuA2^NchhMT(DUkoPYe*o|2m0INQ@M{D6?}a~GJ0k7S?m3Y3gjV9Iw?M#i(+mf)
zY10u?8{sja0<dcsxT<^SpEA&t|1lEzn~>-i19r|P3B*r1|090d`Ni-CX#z&0O}JvD
z_q$<qJTMjgv6pdF=Bcfv9D1za1`GZ87Km?8)PaC*WCnQa3TPYrBXP>_OxQiB(X!#N
ze=DpEY$+2c^<^a@!nSt(&!JOVv+k0zi$moTC43631z6B5OUe}q?W1;X%0?GA9y9!?
zh*6|*0%9>x_Gb^<yMX6|lMXgvso#bd#}SR=`q4zy1<s(35+Bm{NI#cy*Xu^Jd3w5!
zz9{6TVUsmmKt_`NfudEMMDzVCs&ZnR9`SjDIU;IKdScyD-&~fK9d<M~Ilz2DJedu-
zk_Fn}l4e5b3~2B=l6m07^Y{@>`U~lHRw@H22TX#O`w2ou5wMzXp}aew=439XgkDLS
zq8%q;{q*TKyZjdZ3m21xa#KNV&<#G+gbpb53*8%QhBg(N{I=k`ta3ZpZzdd6fY!9A
zL!~jaQ^;r-<p3B5?Bw(}ThU9;^n_?fJn=r#bPL!r3IwR%rjG7_RLZRC@FaBI81Zu%
zlzMZ}zrZ)O{1u^TCNT6m19zVS0&K=FwAX5IDv|_mtcNwiI55&U%(bC=Bzj;TI#jn8
zrmb905Vv=I<lSYnJ<n17l55E8K!|)}Y&_qP&qtB)51aYJMX&5^wWE{<3*_@En7xS$
zN^(3=qcXa+!Yfgtp8Bk&d3p&F!z{`Pjy&GPY}q-b3a(a#L&f8-5Y8?!qv01b8_af^
z!Y{rBGXLNB5&x;P{aeDas7$nAj2!{V0OtJsVu%Jc30MvaL_d|{r_#8sjOnz?sHbhe
zLHYW6<s;3mSrB)0>x0iepL=slSkpwq@Yy43Eq^l6F0k{%peOoG+oLEREv1i*UV0Yg
z@%Jm-%4^(vJ0sf&kM>Tr%((V9e+!JXH_dVpGjfr!5FT?Ixsb9e?4Q_+?7>lQA%4?K
z_X<$}r0}P}X;7#e5<>~YZ5x2n5*>0gs8&-I1vPrd{h0P%v;sHdTpMiwzw0~$71aSv
za}y&n1%&VAPvjFCqrrdWda(Z_m>K`;%cXx32B7tEQ?w}j%1v4>d~=Xg+Axj-N@@i3
zDj1Lgx5+f13>|e92suG@K7?m6p?KB?wNBupHQBknYpN<BT8lSo449=|F~H9sAna#(
zE`)^$g?3K*&g?X0y&#zt)+C*{=Q-dQ(n=^5ZL4QaT*wxCv+ZlcY*nnKWKf`&Ik;aI
zA$_jZmRxbA&gN91E1_VXl)&>1FQk4f<zkk?nLam-Y^BCpi~*<~LVUqq<Dl}<L9$~b
z4mG+@Zv#tiBxuvg1r(*<3!M|5s@S1L-GzPxHYMyMo+CF9_n{R8c$<c#(7ALes6$SZ
z-VCH7mlD|Olo<}jOuY`kU-g%|o<nt^mH6{uBnEUVKq1Ttii#;EZid%+Qoa-r^=u+6
zU?JuE6)#`gJg7{=o#DRbdAjV<=Rfe*P*P|5`sRNmOt?)aKE6Geu3;9Nnt%SV?;}p;
zd={Bc3~|UeQw)%UG$Y0X0=OBUzZz9-z@3U9^AeFSim57gg-dGUq{)m-G^Y-r8zglF
zB|dd1$m$3!E9c~{`Lo)YrI_s#?j_Ob{?B6$I`OubUO#-GR$`>m-q$u<6x=w_G<P|R
zPma0b$GwtO<Gf_MONz<f**6S|^bdwbCWpr%bzXKJ4{DyTP84UeWbFtwcQ}Q1F<vIx
zg?}BqZStF1+mubQzjJZMRxG^KJKr?xLTa*pJuPTUlqzIPcHQ<GbWhaPoQ7hg-8OGI
z4{3>z;Nf%CLv>1C2gWg-=;zgyl03VfHDalp477b@vH|r}A4o2uM!^ij7j!nXf@ui_
zMYd~BKSzSKu(-+VLN?`naz*f!QGBRRk>0vN?Vx*6My-8vmqR*wH3Kala#No5hnu%Y
zP>Jz|g}J-(39j+*3ae;5Ub!Sq^_Vjd#i8J*@wxhJJi8`VjLrye=b8jZ!%e~k0^i33
z+a(9or6zss@sw{l$=5&LNS(2$O;hz2GYHl$$x@t`^>mj_(8}%e-gQ*0Py#N4pVH4c
zeSm3o^w2xNfI%C7U{#hN0<dy7YXF_qzxoqh=Rc+8_HH&#?Xu6qw%If{iosR=5Bq=O
z&@=`L#2pwj8wF@jU&tn3IfokP1KT45A?<r2+U~Q2m(g)t1Re)<y5SVm9`gcPv2UO!
zEc?u2j{<)47X0?*DM)(@tV44Hx627m>GD@d9%6Msu2ZmkyP!w01$ZW#C)hVo)1l3%
zFf~QWkA2$-w89J&PUo-~!MMurB>RTeT_spM@c$p=y?H#8|K2~YeUU9>D^npgwi3xC
z(`FA@$37tmAt6J!DqHr*Qo>}JChL^7go&|)BKsO<gzPhxX)rVW-ktM3_kGUiKIh!`
zIgj7t_jugj-yaik&E>l0x~}(Yc`h&Gd(_d`qoX!@6fX7UTTa@8*ZDrS@Lk!5@^=U@
z<e15Xvr@r?W*83!3=%vVO{s>;7BWlT1nTycb-GQkkd{>a(16f@V>soqSR9sH?1c6F
zfs_M%k)E>tK2p~*CrD$vfE7p>nm<^DkwbpSI8wm4Pw9`hZZguEaO)7!C~z5fxrexY
zuhGXMyJDNuvxHiIgXyz@*Lf~dRX)NVg9zKWz%zjdUb-FJZkf_lkL0`=G!Ii`JVSo5
zxn7+QLDArTn=kgwcxu9|Pg(EN;kZ#%;019GQ~5gX(s0D2s<?h+j$ycjmVEok=f#%s
zzNZ+49uj=ACE+`ZZku|Gn%;8oO1`t4sj>?-U=Mnk)O?>g(=Z)_*yHA&t2U(5q;^XZ
zA+Akk^4;fRfj0xU<wQA87RBL0NQImQR^bPP!w$;Z?{<)3dGfU&1YAv`GPHDM<^up>
z(j1A{%env}-!5<yEe3xbqiY$maq5DEwvE60<Il`LWV0(I&vlu*V>G?xKQl1HJLZ0q
z*8lHfV4By+S9ebsG84w4VRU^p&<c26n`cdiMnOiu#*SyFM<zq#<{-m2L#{@`rhTcF
zeAP#eI2l9b1XVZ>v$S$eaIV${TKR%pN(SWFWy43<<>NX4&T#8s33=Mppnoi)*%M)4
z2Pc{FWTrP{(*RN}1*dhw@@2Jmz`8FD8k1oQ_<I2h4y9ilwJ7F^Rg`^;ABZAoYTKg7
zWHWf!yV>h#YED}`+aJ$bCXx-aAADE@dt^BM+_yXd{PMuk6G&hf{pYs$&I`6bp?;>~
zX82$2S>^*Pr5DBCyJhpx4i2cXm)gOx<rl{(qnZ9WoAZ9il4)k{HaY@i=LKXe3qe?4
zU>Sa<r81w2jIp9=$c>xMU++{hlSELg@N2)z9VGGhZPsD3dEs{%^6$K4*0+NDbNK<}
ziGFzD!9@nos~da?VA)A+kZ&RqhS=cl9k0eN2aj!q=0j$%8_xqT5xSW$P;^bpf%CVB
zZ{1bLcna{E%yaBoa7<leKZ0-k)d?T}t53&d?mIH-bnrmhK{go86r!d_-3h2w^t8C5
zT$j-!)XLaJrWPy1{>D#zPsq=n!s)JV-F!+rTl;zxuZUvKcI-T}Lop6@k>Yxt_AdWg
zm{l_F5ze3>%OK`_?yQ-ajIpa)UU^H0bZftSKil%-_+(t+>t?|)lKwkQm{p!}x~qZr
zwV1<X1Ki}jS}8$~DvOQj|G1u7sW9e>;pQY%PecUo47|Xq3_im=OO0scLZVseV2(%Q
zNwLPRB{hkOL1mfyw+K=dX=l=4{O8|3PZ7E`;BmpyZ`&i8r4LxX9|79oW7hqva_Xy%
zuor@Tc;_2dmqPrbWC}u)G(QW!x_Iu6qP~G`TG!`0osBGOp+{N90&t@gb%EFS129pp
zMTn#%H?wLRQBQ25PK!LM`|3Sw)D}AnXEaY#k%4bsUpTj~3_|d@zDeHRRGh&V{j+%S
zTeVt6?>#I3^Va*%+tdb%U27>D+k&ZV5S1F?^2TGVZF-f4KUv8<+tHZ$+}GPp@rm7u
z1gBUrnBrOC!;jonR)8~5u*@L)BS4cUch=k{@~u5r+pWxHGjos2^JE{>5$}R{Eb0|8
z?=o#MqPKtkvA<L0E8(rG-UdhOoRm)`(UfS47}<Q)8kDBM_sjzppT?8yznF8I)fD!;
zw~9HRF?OESZL?E9`ty;9j6I!E4L47=9nNp}a^pkr7vtnDbbESElzk^i$;j)u60b7<
z?$Ze`&QMAf4GLY(=<0BCQ;L0{q9e)oA)R}tW#q?kAHjPy>l#hQ(W7#W!*2MS{(HV1
zdr_)Gs2{_R1Qh{f)}I5ayH<S$2vX_`Y114W$nSO`ZwL#n#3)aOL_ZV9LxSjfYJOl5
zS`}m!_R~<Y(6Uu8sQN{b8wt_K{AzI9CmN@+!axM?xqTe)Y}Q3KPX1l3&1Mr}yZ|6=
zy*>-l?7fbgwjFt+!viGgXm^T^ypu&juDUq2;I?tY)?u?a#5n&D(S<7an&u(Z@*0=i
z^TAz`I7EF>nT?dAxvSp!CMfWIRQ-U?nXyfEi5un4iAFZf%ww7SKwcA}3-Hi)#HM(L
zz8YvxT;>L=;6pDc7P7w|09)BeW$%kv$9}GDs$^~yfM<P@ZRfUx2#|ROXgJ;V(CWz<
zG($*;0eS^{@PL=|!@i41l~<tU41WbZh8`mI2J-XySkLt}6evX7nAQEGKC7rxf83bO
zZ@33kv?LrfZiAlCs?7Sufy{%(KS9s#i;k_#64;!n>;8o5nIFZDf7tYoC*gUS4S}+~
z?g_9Gr7IJsd(liGg=J7{y&os81Fh8{lHJw;;9!;^^A!4rA9~uJI%n183k^YZ6ZYpG
zcZF(fJd(3|tn_N)v!56sH$3MTChakQ(7Bi!f!dJnUxwlnV@gL}86_rS5OgmWymPpr
zneTg+*f`VaV9rTn1BE#OB^P{oaQttL81?cR^>1@C_?mG~9pc_g4#^iXFF9s)9$n!0
zc`V(}u3&|KaN{&{Jos<2wG~hnx#Ji|4fnf<t`!%6WRfXq6;i#&DJ3-bm2{j_iiMr3
zQD?}Jt1;eD^AOpaahdJ!hGuL53#=4sLndY7jSp4W!jNC{bgVF5NZIFS{}MU?8M(4g
z9F7^el69#td-j~NkaI3-dV}s<WMl`r6r2E9S!uHZ_rz|Xd4b0>Y}mXYKw0$x$^deb
zPMEVD8#)Wy*b6SvL&9ACX1l`c3FHsaN;bzYj$$JAW<amtcOvyY^9YD~<pd^2Jh<Oq
z10i4snP+{n2pZoGF_X3Eal6=$UPI$<8?&|4jkwe7hXv3$&&J&Pe|Hr9?B8+<pw_uH
z4*h^fuy+!R3zm5Pcm%;Op*qm``2G`l%X3o#56Uy_-u?*%raZJ8>X37Tt&w5No2WIy
zz!F1+vZhQN=AT&n;I!jwcX2KE!-L%yIv4KIP~nd>j_l}^@;s(#I%=8B$-Sj{iY-HV
z)60-J8!Ngxt79AjCMn_;TndKEhc^4Y7XuL_hvPImYj~OKCHcELt#UHUm7ecamV@Hw
zmVa|CPH&X`j<ElWxfb~NkpH86S)<nG%XG~F3uWj(9F(Dr=-KjrD70t$VRs<EUwg`~
z@5gS)j{>bw#5kTAwI3R=S^@T3!~F!*#yL<Xydy(%pOF#_2*zHI1Q|jJ6Eu#BNl*Tg
zoXAwMQjwE~VQ)KF$%b6e%m9khwtxOTAdGXc;-L9_j2lqfkeDmz(@p5*tIiNzp#e}A
zpJOl96PK?!q3I%*f2(oWtk>C~4m)qS@Yi&Wf9ZR|cOEup3*f^qP^H<Q)SrVsb7}*e
zO_W&&ciy9AsAV${ENLY?ub^2c+Fqg|6=Lh9SF&pykjG)WD2B{L%+khs1ju+gK39=z
zrJ)uT+7c^rR1)N6=O_J?zc8A$-qUu4R_nhUtUmQ^&WgJTXVr4IJ%u)c9_Vo6Z0~HA
zjo#k^FN;a|MBW0Axr5)-Q^pzBqMjS!b|K2&2)CM&VNt$t``JspsFLd3F1y=ovybgc
zw0F8s6Jt(L-mIi&cE-5Y8$VsvIn$9s9LIvDhYM^Maefa7UU=&v8Zq)GC}xNa3emNy
zT|Nqf@fNJyT8SU=_?g->z2u5WrZRbRV8P1;r%t3;cV^F^XhX4G7hhR7=xx1uFT)}p
z1dE$IVF6$*MnA0G251w}UycEvWNK~3`3%-*1mK6IDkYANjL2B&2RY%tA$Ct)yIOJQ
z3HgCBha<&Gx(+!cG5(7qUR`RGDcGydJ3G3Afupnpx<En=el%NrWS~Nk-LQuHYxy*_
zbNFwTZYjpXDfcl7#bwi>@sgif8=G6EEYxl+w+q*qwPi}|(c7Y_8jVlAep>AwzsO|;
zk`c)`E8?@6jk2J5?%R_M4&Ovan+GqwxQ4g$&0}c_358h_cqdLcnjw0<o<%*|A$w0o
zp`P;E{4?CF0RApUG7ps&D`kL#?=2ECs#KOn7~xFnkxsEFbO{x|kU^ckMqMl#&)}>>
zRrQP>qV9Z45mqx(cyzr#5O#jDFT$nBy3K3fP{3!ROZg+Qh{|$HFlkEN-p-4HT-VSd
zDQwJ9K_zc~EFNtiD9(COdx9Z9O@Gl-?MoR0aw-eP_|$}MPuV-q)gR;Uo{~yuWUEp#
zq@s_#aOY~sPoWKdn_UiyJG|Vwl48<6ODyqgkO2x6d>y!)p;pbIsHR#Nw6lDwK#E?h
zcKSG&?^>wb)=_F({CMcab9cELal@ax9L7{Nk{U%6720H@)iSlr!>;a>qrkQo7H1%8
zTly@bY_7~1)wK=kcbU12I~wRJVc*7{#p!G+3*7fqY3*}1C}|}3sx%A+tLKi-ZX$1%
zSadf!@aJb(UYw9JGVrx7IMLFY<9+w(<^)*HpBRAW{`#B(>sVBoD=w7B&C$vIh>27U
zuA1945l0D=aL9e2n__(N@cvUz5N1`Z%mM?PW=41VP*C?%7gqBWpLvYQHw}f4n`rj#
zHLws|2JQO9j^EU;^E)6nWu^&KCr+F^9xMhPM@wJ{qV<xnaFZCN23Cs1KEj{)TRkzB
zEK9N{Gck!c5681}>@R^8`%_2fBceL{G*l;tv_}tZqbNKEW35mO80<0gKIOUlbpP5g
z8WyNe9iyky`#LR2US4<%y2<KHGX0_Z_ET!Kf!vbEJ*|r$MOoXktEd>}JzI0L5~hdD
zHmhqslc+t93&Qi@9$@|qUh~e=kR(Ux1M?#e3qEnT4S4p%KElP+lWC?CQfIpxI`MXV
zY5FIclqDo{pStKs&JWeRK*Aw%3!q5xY^dlEsf$biXHUm#F;!LAdq8B?oKayP8jBrc
z(&pBzqE>%#^ksr)FFQgFBs9h6esQD%&jB*~<0s+|{bYzt<ODMB>eP{7kdu<0LgQ($
zRgb3-`5Ob2upQ#@jJrU$dYK*D<;grp6eF%r^v{+HKt*c$VBO~S2fhzBV8u}Q?GthR
z272`vP&$r*YI)L5Z}DF_NaWu+MC2Pqg$;fuG{X~QY|8{-0FjKOejj5Ti(rZ6jxqd#
zRiKsT9QMH%ATbB&W{wZ|hI^@LiH%v?DAIt<8S+D&mop)9Ke!p%fU5#*KtzbOr-Z;%
zAw%n^#Lf6GNU&Jf9frm;(cFRdWP&(GYkcbr80#2bj>klkhQj(A2+;uzXGGhd`U);)
z1o(V&ZuZ--9UihJ3DC6eJA3gw7cJEs5ZtiT5S(`OmTgu{l78k$d$!&c9i}>QO8KoK
z{=nkI9`p2(qu*W_xtw|G>>;%6(Z{B1b>x!++F$Yv&bkV`*1W6R(qbHAnr3xuv8xmC
zCvCK@8TA(dG3$#FrWoV$eJ|y2)AfY#pMEIC>V6>=^_|p)pZs4W4kTK>O4ak1N*+k@
zKlJ>0q^yqeF6IeX2y9imRv03016Y!2KfcpC$eFi-U}^0++M#DP=GGGQscpdMekTq@
zDKcB@8C0LZ0QvZpXmSLbS6u*HIDHs3H+Ub^d6Z!fa(o^DB$?P@HIRiA=*Lkan=23G
zU;Ya3&WG`=ujqlKp2Unl3GzD$sv|p6h{VG@9C!lUPH9N8Hd7efa~f0<FsFd+g_d8{
zs7I-nMgsXX2)&FmAT5&u&Jc6~<n$K>rV!7v8i_3sUl+e2iQb2#KKzZw!EgPY9j|U2
z-)NU+KLTso_z+M%16P!RYoOLpB2sOTpH_Gn#>kz>9|?irpz3!47%{Inl~lW(sue2v
zF16l{i%JNa{9vY+r*@9KAaNyel6P`Hd3E^Q&F9ya3yxgJKiN{8AN3qnq%?u(brPy}
zGQMl*-tB%qb>suLQXcTqNj_U%8gn#l)df)Nj4VaLKNn#C5CUEygU<&%3s1q)`^Xv;
zAAY5v>RTa~Ll@W9-UGGTh!|M;EGyYX1u)g5UmTz2z^5~>AP;IjcaV$#PmvrToPMB&
zpCB@y0QYkuT{rkBG<&2FMS`drOCVJIwk5eaHx9A*&Ih&7#@O0W6M9n$$%kCy?UNxB
z8ApFIvvYoX!Ib(^k?hBr50C~R^ZAcfkIa9~;Qr*G1pSADQt{w#SQsn}stbMpghu;$
zf&z<TYK?mi!YE+clSF~dEj!7tWwAFmwR#u!?~#{P;I)+X<CE$!87_V4{ZL8qh;)*2
z0z~bmr%`!Aau~7Wgdrrz)b{(z$y|wVNM&zhydZb%Zp&drwQ}z0@ho%WG9;y$hXmz}
z*)+v+#cr0*YDqX~8;s95QYvB#F(GsnMrZa9Q3HJO93niT$a-c-GuGZaZ#FHg?tp9s
z-TZ);zOV1OF!v{Q{GSHMhRQoS7Q`mN%n%rx7ZwJA%hEJOi_0T7Qb(*R8x*#TG0#)6
z5mQy&srsqbJ!{DQ<%FRHrM{;N$GQ!|XP18**3LStQ@II=s38c^u{P#zda#2L@Dv?A
zg@&n9d1}sO?W*7QiHkxeji$biA!ZZNIp)!?vJUEQd`MvCM1=W;lUl=J@&%ffboYaO
zn4z;Gm(D7OeX5BfL^d=JlV_|frgbd>%4WT*+C%6qhiEK9poCCFFy|bM8ilE}tY!3*
zzqwgZ+41C9<fg+i*JTpb;*gehiqx%K#h~J~qU;Y&ah*G=pW3wT6>M+EZ(hMv8hmA*
z4+bL%n(njwY}q}Do_mI@FfQf#`@8i&Y%1E!_7jSW<9Uh3m1nM9hU6NaoSBN%eIt&3
z87SV%76k;_IHm%Az}2cYfx7Z>Oi+oy%LwnPUv!6enDi=*aepqlAMo~k&EuYOo=V8M
z*;m%{Y-AVj)R}EE^z`?%&sf3L^D+%qP;<f9Bfm`kD<u)*f^j(YJpYMEUog^$M{w+i
z+<|i3Odj5UXZyH~waeU*gy{_J>*u_lXZnVd9KFt_n^~Dy;tm#v$;$I^zhT^4W*(4#
zb*vU%zR#v6hGxhYinmje(Ml^9<;_AYhiTunr;2FY*5r=ecOKC*9n$DHYue!8B|chy
z_NHnaKTiZM0%y&$xm0j@Ph^I)dHPsMo>_00I7V+ksbi@5<3@Z)S%+}TO9oyql6`Cj
zbSs!9IkOd~EM3{VXm&40jhiXE#Z5%Y!l(4C@4bJUI3<1moz*cP$v8GLE^}Y%gj4^=
zWEF^-YK75PvzBU3Ar6Lrn;IIr+wno#IBuXhkm^q9GY!nt2vnuw$Kz^VGq-?wV*CoB
zfqvwI3`dG)ImK}60E50{)K_ifC!dP@fgnbB=JxsSS(55ogqe2R#}W?L)WnSBK9OsP
z7c-Ny2J1|dwc&#^urfJp=$aoZ$0N{YjJI52uZEMqe3{aM-c|Hge3-RC6i+#}gki;n
zpi??p%!R!4Bp==wB_+o1_Y$}>aMkwq_JT_yH?1=rv-EaZDHenm7+40|r`Tsn<~JX@
zd|)EoBxa_ngw$OSl7a9r9jsoy8s|Ir6&DpNZE`58mFn>do{<%qmJ{7Z!$iK<#r90<
z4Ufz8#MFSCv8gnDJ-36P0J%cjh8SjQdhXd6yv6(8K!1utL?#!tFB;Ev8BMFGB)5HN
z12%Aa7P)+lxKDjTcVo4#rF8XiVycHDCHB2{jv%^nZ8PZ(3=Y&8j~B!2Y9w-3%d-{x
z0y!yd=fvFaEs=t4hcD;5oE{vtoe2o5E*Uhuy|800C~e|~-&Vz0o;I`)STPs$E>d~s
zyZsJ?&~`P6s?_TvHuY(X5mZtGf)HNqkHBEHE*D^)YQE4WcKga{QaUWP(thexRBf8A
z)%3hhFlcpDW%DxpslS2W{-#)n+)>{#6x`NOiQZkUM}DSEjP+*BPE8vLdFe>n9sKHJ
zC_L*o)Y$vElEX$a<;<~xH#aP=mT(L@lord>Q@jQO;<p!EzdLVDhi9PFY_F<tS6h8P
z>Erky-A#4+l$ZHu_53%Tc_A3zm>Aa%T@e&Fiex`$vN1xAZ0t73&rsf{<mzn5xljI>
zcVoS2cWaNE9xI`r-kW8m92DxjZN4K6TedCGir9(0z_6rA7*K;NUp5z6F*ecNvOWYJ
zrIL#RB>gzCXmY`qQ!e|wY_;%WBA?wQPgiRks{QzXs_lMOGI}2<q<aIP%=d&$^}Pe5
zGuYk1A}&M8xG&*Z_T$LCR(1oU7p2O-WqhlRa1?AT@2zfqdeLipe&?5cuS>L!mLS5n
z^JIeo@9WyT=Dh*a2Sq||_zJEbG|{aQ=Wn-<Y^XN#d_5rzZ%;<RyK$<T=_gFgV-YUP
zq%<@5S=<7=C;2<G`jL-R!@6_t583m_Cyrk_Y36L%mY8qX<^7`1+x+hLm=j~;_#N5*
zr@BY-%-;cM*S}QN|7H9CSE_r^YS6p?i^F<=8A8))1e9T9GmM8V?&gPj!gxV(Zz^{u
zpTo#XzpJQ9z0iH5xBQ5ozwpVy-SgRgvG~Kf<C@PM?Gn(>kmXp?H|K`2^2+(_6d1pn
zH(7!r6aV<Y8DmVqgkB``)S2hwo0}hEwJxcoxpy`D7idJwT`Ug!?CyGobkNvo?gvqv
zap~tr7^l*LLIbfJ6;;jS>`2hRUl?dWyO%|l@!aofjym4P{ctB9a&pF5-%cv9dUpF*
zRiOXH8?vWZ?qMCYE-YUge~NNM%L@FN&3UKjkJrLnuhmbQIqofOppZqq-NaCW@LpTE
z(aRj&4&L|D%(2HUZxZlN;KIYGFLxOZZe7XyY+WwpzI~s0s6G9b{i{iR*+cNXN#}7W
zbS9oEiGvl+)mQJK<*B*To=zy#UnKW+zaSqJ-}Ah@J1-j750Oum^m=YB&Xm25@S|h8
z)k^dyr-!Z%KL}|fF%OL^44qPw+PuV+Vfc+N4aqbh%XmW6C2d`Y#EH%mT!#$u$O{Hy
zs`6zu)|xcA;)gjFwu9Q#_0xkSsdl5Z%OuT-msG+d=KTq1Sb_eoTkQl*!3YfVh*cr)
zp$Yzs@cDLTu2E%KJ({uw*NAgP6qPT$O6IB^?*EW1cjH|~=I*cgGEYh3GMVbdh9+=Q
zUxVf=-{+Ge;d^6mSCnSlKBk2}6&0uK;1uF>q$9MUYW0+^zFlbcZ2mLvA}Xb>^Gk}o
zd4|wF?3U-(z0C3)f8-kxu;X`U<6a#wJrZf=v@4pVnQWt$_2sNtOwvIat+k#0fzOV;
z@K}ONKlSkX;E>Yf-{ciDK2gl0OI;eAp0ElGpdRXo?IsnG!zyWpk%7vr^o?h|(O7QR
zN>N9<pw|{NWR1a)c3%JO^<r&`ZL!m&T0u-H1r4(FxJ*S>dawj$=qJT46-ffqWI$Yr
zS^Hpp&XXT<R$iGwWQG*AIPRz^V)pqNwW+Bj)W?1o*!Nh(NZp5Vh?Z5mmE91m#CYf~
z7_(7^KEU+MzIAFgm2YS!#HybebSBl=JJ04S4Pa^y-lblvzIjKz!{z9Ak59gsl2}>a
zgJzb!q)8vl{m8fe;rnrL1%AELE#99aWPJRyr@vTb*chmA=_+~ga7Fsg^cttz!V{tb
z&#98#uI1)Ac=eXrJPO~&Z-BG-S`hN(jYy`PVM=y{tmJF*v{_$vKKUE`iR+oyxQ$7T
z;46$V3T_OX?Y1$d8Cbv<*2c$Rftp`s9ceprsO@NBreYP<F!HL*!K&179|fM763ES3
zPsz@`_Jt(Md&GY>(S4+Wrw19KezF2g3FO&<S!|c!BE$sByu_r9?yrkE1uXcAaR)V$
zO5VnLT$<F`C9<=oI!B#!bB*@S!o<hRLexTcEicG<M|4P5T*@)wfG30p4U}6t@5gBZ
zv)$fiCk)a&<TG^5q$j8Rl>;l{#Q5P+6)-{L8evnqaJ5p7HIL}&f<4Af*GRpScN&Z{
z5K-KM{E7Mo>15bf!r#T9K%u@J^x=}S7@q7FXTUXG6MsZ3Ph)G-2y8toB7GZZ!Wf@&
zUQG=nF6#pD<PESXiVXvm2T=m4`MUu5cVEbVARwG3u{lA{K{^B+SS9GnEIEVj=`_ie
z9WpoP-LWqHPKEuk)RSdxuI>$L$%KA>(7v*>{l$?#v<0xVT40*L3Az4;|1&b|=F|U4
z^~4dJ@fUB|-(P{hET}#I#$OidC=amxFaA&F|D-5tU-@H;`w0^i*a?>mH#zN~A0GnN
zW&!`^F3>I<RABi>e1~`@esPSQLax!#&j2tuaQAn0`KOm3`p?k5D)_MS$OP88y=1pP
zG{SqJOk}3p6sC=>6SKr_WMEmRPyaZJV(acJV3UqRGoP?y<v<fLoyYoZm&Nh#yabsK
zLAS&o`JGj}9Tc;>x{+&&j4*WnBm^6y+U1ceS=GTi^}S^8T*7WCD_Q<LckmWZ4Eyi{
z%aO2aIvh9Rv;{YyP*JaT+aRRxW9G|1SR`^;XM#941>h<@WF}#VM9M^T2TKLFW5t+K
z<1FXqG;}?%k9}TkOCH<TDA*q<rb#n@cqWq0k~U1yj?1Rs?WlFTWpmZUZQo%(^eM&&
z`zSCa>3BdqNC@7;yh2kU@obs1lc}I-)qim#^${qmnah2nhlHAT)d``xhG)HoM?5SR
z#4Y6=wL`OvtYtQktPk1HSM*hYe$AoT=d|dHpdh~(bQ-J{MG#{FvUXtHO*!`Jd|`VL
z;_OJru8d~x8M2#6R@(K66ce1;eG3(9PK3X^pvf*&ntE5cRnM$R-h2jfYW~c9<It0E
zi`}LLk3LdL%5BZSgM0OZh-~1jk?Vfl3{1~wx7HN+i<s;jgZ-fsB6;ka49avfOfXoD
z-KKt+TKN$vhLIsRhXgw3q0w#u>`41;^hJ8CxVDNN!&^UXSzk>5fiNX*&M0kX+Fgez
zu6(<yL(&aE8ghEmEVw)Pbgw#hpefBcM9tHQS|umlpuU?rkLXfv55c+J8M(}r)ZeF~
z^63U~P?D}5V5KG{e@VpQ+lItH|1_NR!f~3}4PEVXtoxg8f*IBNSJihKB?sE+U6#lc
zH$7sIp&TAA9wV)DIk6-ap5Yp+Yo?SEm~WhNz~4+IBn71<<0BYooLZ8TUbv`S^C2l8
zvBaBn<J-);Z}~3DJ7~`Jo}8E!if1!gh}~7INDVIntM!$5G6k5Qc#?T|ALr1_n$i?<
zdwmu7+$v7$z+LWw>3*^Ys=T#Va*tdcdHA7TjcZc{GtoTu)-#$divCf`@NPZ@Y=x1r
zfeNoM*}V&L1O+AH!Iq*KBiECLB66?X%w6ON&y1e7ebAw{K11fAhpVJnJ61cG+~vPG
zCdwd>AJ|o)ZX^}a4}xW<JYVZ<x6d4<pwCQkWNzEzyc3;fZj{p_{R-_~mhp-aQfm6L
z_qJwSl{;%<5dimkm51=j+M*SCYV}qAHBwjh%*5SvbJ4xZQU+maSk9<A@|%>T^!%=j
zeWnw3X2)iGU0PaHTd-J!bA0vr0}1XqZa_f#_yy(anYY&8X3oGktGB6yEGjEs6w`5^
zcz#3wq(#SJ#ln)t+T$;7=LDo}uGHsA28&QP^s|OB4o;nAohlKsVI@Pi6r;}uemOc|
znQ!GU>OIw^P<nKD{;}|d_*(`4^lbY}Q^s)*eKTx))l3RsH>th5%FTV!v<6?>w?A3c
zw7|00pb)R3a~GVc@ckZo?M>u?w^3>M&`j}@-cP3*CQca`xXL64Ipp@7=$;JzqN(21
z72H>3t<k*L@pnVkMGzUh-&IzDz8RL`>gGaz7Y3%Zt;)X9PHfU;1ao{u95B8({s42{
zZ4*uSt{|Eg=w^iXp(<Iq-K>2Z)x%DuRvHNAX8z)^%KOd6{f)&hpI*KGm-PPM`1*er
zX%4mCkL&*%5A_XbfRGPbl?(rvI_FO{VQcI`&dY(OfA+#$e?fCCXPYSI(uL)t*af#*
zktEpKJ?8imuO++p({%t{%?50^ex@7iP}|T(ijWyoI9QtvYnER7Y=B*+qtk%eLQg~E
zFIy5L*-HLYsN)uKi2@v4@7emH`<aN%c0Kiu4S+GA{pAFOu)EL*h0O&h#aj!zi%E}W
zv!xlGzTY4;b#96CZ?^mW%S|*brz3=YUjqz#<!9ciS|kz}zCXjT&Mb&Ts80ES9rR`!
z1123bXh!W|XPCc(sq?Ha2(g%GTJZ|q$_E+-tX;(M){lW^)K9H%JlCV*FE@A7mH@OY
z2Nj5n5?B6=EkkMQAz{StFI$Q8iV^Jehb(5Nh9Elt`#V{=_HVt=0E{i~XY%XO9at4=
zw&ae3fv(-mn}K@dplxiv4CFOxd}PxDr_L1il5tcQU8UQ1`#WAc^tU$7hhhquvwHis
zY;4i+9e>OCT*-IPd>g;@p2$Z@j#i<Q2H7$FvWZDn_Yymco_9pm+k5pmMKoQ*MP^Bh
z=qBGfQP(O@3oKH`)R=sOJ^sZ(;{omHZGgxDaDMV9kgYw>M^Zqr9@xdk{LYU^3A=uA
zoHpC^(zp-Jr3wN~#6@sb$+qZ~-!pex0fKq<K0B1b!<3F#|JU{h`!BnD^S@_r5y;Y?
zzdaW*tQZK0!os<zu47HsYuHB+zwf!{YL*nU{!E$I<uLA<hecNd?qwb|FROj0WxoH$
zo9mjYtR_N4p!IX$Hvrq+i`i6|-=xVqOILMJpuLwK29b_PpB;}ujhNXOMm0D>Z@es8
z*SyZ=4|44>FS7FRh3n0&$2gv7MY%5@FIc(}_JsVy9xA6YT(4zaO7S#TnlXurIb4f-
zkd)+5rkBw|JW~&F&j0FL@vpQ@{<p7-*!91$bM0qtc>hTl_XlGUKuDYThbv=?A9Mdt
z_8ti;551q2hG#U>q?@q&S9$m}Xgm#U{sZd##IHqVIriB@Ke~qv9D989FHI~cp2<3%
zT>XnfHdOz0?m>1e)PNW(k!A@j#i1E0RImalu6={*YKhDC{aCuS8U&0HmN@$tHG*Rs
z1MM3I#3kDAJi9tAbeSunxeu=yb4Xp+hte<Wh*5FsT3|~z-U0JuZk#QAD%(I&{H$!X
z_9iLcEbnYvnzxI}2`e4jK_4V`j1gf?zXI0s3|bp-3uc2dYsxPE@BNVY5-u|hMAWqa
zn#!V|TLR!=D-(?VrXzdh0Rok!3EsK|oBETPz+?kK+aFJ4_3xY;Ym1TA#?QvFRrThe
zRpL4tm={NE;DA2vcs(?JdKv5{@n1Z`<3J9<_`r@BSgK9^?J*BrmVa@WzVl><2^%tZ
z2BXK8wN^HNaq!Lsf5NVs)T%<`WAzZTt-p}D7o-^?!EMl%V0i}T1T2E-N%OSn2c0^4
ztz@Uqf*<OuL9#sFv8G6zrnIoPbyhnoIkeW;Ci~N;0~bE&Vu#XX;3IC0W;144)@n`d
z$*$fwb8geF3`D}6bZgV@vh1OIE`v3`PTgb=@f>48mq`l?Q~cTX^du)tJGWv?@>Fkk
zj1>_iC&14)@GnHCfBh>_5*mL6OwWx$!d!<)t(TLohwWa4qEKV}WSZ>Md6Nc2++&|r
z>Wa$`<8MkVm23C&<heE~+a>r44Sz-ouih;zIF#Y|+UdPJ%rmCJJ6|b%rhD3l3{SLK
zliDqFX}{|-%C&VNmr=Tpv?rMipLkmy6ek)XQDBnjJvp83()<lCqBH6p<t`(+IRDgn
z&R39`3+{!>Xb>=dw5urqD7Qb7xSp27J}S!i1=xa`USx<VUk$nwpL>I_%uB_fy3JBy
z+g&HFzg&kHk8Pv@8x;ia&;cylY^ru;qavG_gk?N-1649hq&bX@rDkAWfS^>!E`$Ew
z-#LOh2uP)13x$(t=2sYv_njd-JQ|=mZ;{LbpyXV0*MLqSX}tZ-Ks9?h9&{wgz7$XP
z&DTJH8D5J7;Vd1428>7oKeVE(^|uFzq#U_}WpN3y`5ge2AZ@c_b|{K%3{j2{0O4`f
zSsb7Y^>Ynl%rC|;##zq^>UVatx)8@cETgzuKZbjned13C4WGbn@I#YR;7_M*V;>dT
z1CF;7BA)?go(%Y|tUj6oGna>)%6frs;9*@=JB7Z}{~J`^zQ!Uh5k1F-x9QS?^Qepc
z<i3J&!$;~0UzO#~kSwrPZ>!$*1e#=zYMsaRe!*qbH&uMSQ2g}LL4&Rbx;vzShm%A!
z81ZX{mTsf<)Wj)pCfM6sVb$QrTPU<{^9)tp{RaDEp2nn=+Iqz6h11^MZi;h>tv98=
zyRGSIXNqq-?kyEoS2udvD9I}6Mw-j{t^X-z{@3r@Kf}@gi+Pd%HB_|!JxvNbXzm{f
zs~Oy^WXOYO*m>OVkuMJ*43suESul5uf3UTiZm1NM9pz;}MG~$<U6(!mFsY-G)bI~^
z2Qq*MXx8TS${5XkMo0X<tw}S1%4Ba5=qRZ=gQR1Q43f8VR?J=o*DnrOtt8-5+MWZy
zwC(RQV|@!7uRZYjb)kpK(N>yp;g#9;@_={yq;!*S=Ig4hNh9_reeXdj;f{?K)fj%i
zjS}?QxwW+3-ut%i!Sll1vG8k+6p7s$^@@`*j%J#t^k;-~6FcQPULna-SgM)lhH4v<
zse;}Otq%cKIFAxoTnnJic1!ShRWUCA;%M)yb<VCrQpBmwEdJ+OY_T_BVj9mRqjp+l
zW3CaliuepsOF=T(Rt+M|h9g_t1+XZ9fF~!t6r`NK)QDdk8AV{!upt9p$vQZRWs4C)
zk<10RsQ?fG9^bFSt{OPO7#<(N+&N?_Wi#UctDSNJ^%WpB*BMu<yc%&+87r$P>#K0*
z)DwWX)p<*`B{R=HT7YpoNa<;-xF?9YNLh%xZNgabwZ||oz-Xe<O=O@2Exn=?G~;3Q
zo;!f%!rRgshszymD$lmEL$`Ul@$wBfN;MSE30&8Fu%Kba!87hjAuu0Tjdg+WtyfgA
z2@0-9-cU<8`xB9VL0uG@Rdz&&G6qTSk7<RpikqZdf3v?JD#_K5Gb}42L%T{DEjy{1
zp^h2lbJ3-{l)HUke;}LtrA;wn+BUUdQH}XNC`q^e0;`j*KJLf!OR=&=s>gbZGb`WH
zV?igeW7opPf4ji>jAR}Ji=C9=Z?zHu83~AH7YH*H;xmYGmY=PQo%aR790FNI=0kKg
z#sqA+2wDQ@0zm4MbDU5E-zQ!^3*KppT!TMk#A~2Fd?<fY@vf%!ePu~$d6Zss!H4%b
zl@%3_q!eWP`9HS^oYH(ymZ&}Z_+{xyjx;BLSmqB}&05(myu#tpmk)hOW`%uLNkQFP
zetipHQB~=GI8#I*T*tvrYZtr$^%Je+pRSs!sB55N(<)b%+c%>kBDrw0Tkj@xWVtOh
zts`YotuL^;<ybGhO8-n{>D5XN0TbTeZDbSh*rB91V*%$WElAOD?Jo3gY-7*MKe^7Z
zKyV82`-m*VH!NR}3_RESMq30%Q~j#7t8#mM7Gd6w7>8x3Kx_j;N(Ad^jrA`ML#rK-
zLof3p<tH<F?58fc=<w#Xi14vx13h-rSt9G$<KJ=;ub<?9_p*TZ?fPe&B_;!^vxmM@
zq45+@8r#kTBtebgj{X`H<N;a&l0SEX((r&gaItr7Zg)a1BPh@&XB%<N`nSAnUE&8Q
zHeT|aU@U=6krN86MC@bi#$z`@>KK#nKHJD>6TE$}iUHDvD~9O1fqvi`|95j7Uh;J3
z|1~}V#xTJCGKOIcjA1DHQ^)=fDgHm62FLb-Z6QwlY{0I~;h`UMRdl2<=4ZX#mJp5i
zw0otV*>w~cuS>9C=l6GU{yXTzB7J(0f_{v3;|WWTsT~sjTd&|#6SR~kuz$0Y70+Ra
zt+DhFMhBh#-N;o&FZ-cdkWJ;0tf-v5-q`*(!X8#p25Qfkb6gJ#6z&-Xe#J$m&&)X)
zpSo)t`@0q56pD{)pA0WJQNKUO+~b;;am~IRSFadP#U!WaY3Lb-eXtxPRBUu!GgP}*
z(KXWQC0MPX6w!I~(jv6O*lirW6_ZNGva-w0Yf*@#G2l7PFdKi7;cQz9h*t!)UAdV2
zi3ZV|A$i|ip3K@w_m%HdNeh_s9#rdW%vo@=R-I^aHtwBqT(P+RR^a55ddE`?0AUK_
zBI+*lz>}!YEXgydh8|41!2zr!obqOMLN|TZ!A%#xc--_FK;N$4otZe^+h8adT4(>G
zC+m_K?QUUS8((CsOJ~k*@(%~s_XfcB)I0fXCtldD$U^m^-s;XM<}i)8phwf(q~D<~
z#Z@2LnLs8+-6M+m7>0_=br&y;6FYZSy62C$Sk3c^eQ-;O(C-nETU=O_h+nAa>)sxB
z<+&cpXo4UZ+%>jMO@lhtB(B21l0C&4uqO5MU#kTvgdG+bego<T!;QV)az{q;QiSh-
zxVxjVo?cfZe6}Pt;o!-u9)nZza&5Ev2;ti#{*dXG3ANT(ILsJ#ke<2!724dcpI=W9
z?rkw7K0Ije+Cy4V?ZfYN%h-U;PAS5T3Le*8YZyBjrY5Lco_VbN>f0WR*Csx1)=th4
zoRCiN@|kXehm}dv*omK$f;8vIYIho*($VAz`~euM6YdgZ&xqgymbZ#1OQLoRls+x4
zsE`qDQ=#(YmZ)8F;THVR;NYN=_n2)<ba+Cx_CcAGX8@<A0~To8uP)90isoj+83aw5
zTj0qaCLjI>B0QEnY{+w260lhF>}*GDtUrq1tg6Bs_D|G%{_>HAQyJftMz;d42I1T0
zr+LEgq0MlMD#pAz5A3Xc3a21Sj^1`4?=A%wZ4^pf%i5cKCy8uHN=w4)=f`JO=vl?>
z2NA4LEH9iiPCFYT;QSa<rR23jYUVltl@^{(>VwBj?{;uT7-8is>a%@1vV!4&=onqZ
zk|nNI4H2sp7}6@tXP)!Xz}Ipc8E~)7mB9=sGXRqbS#MZEGrr}J51MM=xqfnNP>B6$
z7s-&NS?ESx$`#s7(E%!5@KALNhpZ8HB1cc7H&%edD>=UG7VuuMLk9o5xaD59Js_8k
z8?M8lxBV=CrXf3E2V^M7BmwyABX$|EAsq=DD-0}ibuWv*9=on^muG_oDg`4qm<WZ3
znJCiC3djPGrYX;K+XR-GK!`%pk7BUEhgZd{jSc!@%O1!b(0m34Bx?FJlrt@*{--I!
zkOtH5c9-{D<6PQ_iRx=@Z5NT3FJ5%+mUj#v9o?NgndWlCNqZvJ^4nvx?P-j*acDs{
z7zTzIdX2~4ALm&($0{d#?*1`!3{OWs3WmQc+jNi3zKl#jCS~5X?wNn;uxNJ&fueIu
zoUEIy){oAMD}1v`6X8Y~#7ysv6NDRo_Vzaob=s}teM#A+)u^T`9c>CJ+EX#<_b%_X
z(6t-F$&T?Mp2UpfgbjCv#z;<_&{%FYN;Gt9ERJ}?n43^&4DzOAmQwGJ3q`02y`Br^
z=`8!=+$=7~i101cA{X(kg~+(19V*6V3Hu(>GWk#$zhJDL>UG(L53Q)m8DmOK1uA_B
zcUp8E#22WLbqgjO%*)WmRNS`}U0enqJo`i^p-`DKL)xm->HR5JqrvjVkfB*~mHp*k
z(u;(JxTa4SBI40Bs4hN6T!~SS-5V^0X(MBs4Y@JrY!YF6F*k$2G#gHuX2vT{&8}*V
z@gLts8+6X6K9Ra*@6)|FktRlv3)erts~*i8EX9bXM=;ilF+vUEoaHoh{SoRInGoXp
z!3Y-PdkE3mZBkv^s~jY{Gvz*Kk)Cl{AD7D2bj}{_$(5txH}2O4R5+!-b~v=pz)|f>
zThsyPM;OZ>l-x)1N!5|%s|JGAN@l7(4PRRv!}9i)<DO{=*%^mj!Kq1??&G$dpUEG%
zz3(JEPDfT;@oXr~qPo6TS|;2%qPX_WNX}kgZ)Llr*IjY@&j#F$>7KNB>_Z&lq`#-Z
z>TDGzn&PP>vK%37Ay8U6+BcrtHDLPj4VvNOvM=hR*55RzNm$2(vTytg2i{!uX*+Ko
zWJP|a*<EpCS*w6SqG$9R0}}-tsRi})S}9<TxKZa>&6DMZ>ax65WtLp~L^;;Yf_7-F
z?4}p~7JX1NZqOl(PR@CB;)u(Z{ejBVj>m;ahgh+Czbf$p7Ye3!&n)6fE$sc9_Ys!y
zra@ab1iG;XiBeV8{m;9kDsF|o(lp&`@gVqiw;GspMkYRj9C>2Q(cINW6yWQ@;4z|N
z8<1OE(gJgu=%VPT)o3?Mh1G?vUR;xF^km4*lU{R1NhSD7oH>7a=J~hED>1dcC)Api
zib9JXkz)*0M2y2k%c*>gH=o7Gd!f1J@8}BTtex#n#%%6Q_*k#lbV*>{Sz77V<g>vO
zvfQ@1HT&*)hu7O@nd9IV9geB3wU%0Z3tsxYwTf_FY#Cn7G+$P2R+nn@(^d%Bv<i}m
zvBbUX{ejlh{`&Zmz)Rgs+t<WMwLSh6R5Mee@03lW)6VBbx;ILaC7*CcKgZWb%?R&4
zT5&^m?6wZO*zl(qfRvTj3HNCdmW+%|yWlSYFPKU+;zJ)XLvE&_`<dxxbKYA4efb?Y
zOWbf}c+84xm05XVrj6T~yhCr_`)_$D!r{GPB)#z-IpWFM-rsqr9gZnfzcDJqAA`Gm
zwt@Vn{H>Q)F|~}!Q0<OM$}>vO#u<AEO6%+99&}YToia&{#$JovyY9Jp2DZn%!tzq>
zu{aUDj)PQmlSxvMXtJZoJCj#<gVVrJnp^wqJB2RLj=akYX7+eCTKe%iFue2M2+w6e
zq=X41RgEpkia^moMz<3rgDnk#R)C=3nFq3TEpi&WUS^%AacNy<ECmG`ax=<?Ge}U2
zet)s{`BdsU!X<dm;oq>_%F?5M%NFX-R#H6wgiEqc1@%xpI4+<g`{PvSjgb|$&LTLR
zH?qS3S?G;VZ-u`5Hh{ENC>5HmcmOi#Wv$LIK)?_;`5*A=Yrp@G(%K6!yy;(&F9gA`
z;{T-D|9Mz3v*1s<N^sPN7;iyL>~0PQqgrKViPOVHo}e}04}wtt26E%>F%TmkTC;0F
zH0qLs(zQN%GFUWeR=~{I24eBO=df6vx;icO5nJ4lI#>S`(``j{Zh~!LOif>?>?vC~
zcK?e4$!~ddR8pm?kZPqR@gO1Vo`VSK_Vrwp2~~`Z3<SZ|uF8)c_koaGz}ksV?f#+&
z#~_jm?0X7~38D<hX(7+7Zi=_*Yr0^7*^gSQfdCJqJl$L=vBt(%kZ@m<p+dWzyuggP
z&qV8tIX$yISH|8}^vMgHaq^W6M50c8u4G190POfJtZ*#?6Jq-2YdJ{&^%^t9I(;p>
zNT&V8>Jpg!Y_4Oi--YqDiS6y=;$q|N4o$}eT3NjOjs83je8z?x=^oTY95tj4WY$Vc
z(c;1-eaf5IaJF&<Ei?7y<yS6Ni+JC>K?_Xx)t;=l6@={1B2USx<5enW%;%0C&tSgY
z5Ys?B-(gjL!a+Lo=9pcM9)EZog10pfg^{(cH>l;R&$K+KAW;;OW0mY1VNs>5nyhxk
z{=~)4r=v{C#xaU9m9xoC5d~_JxM%H88^~su&W5};aT?qeBz-50&DaFv{dQe3c;PR>
zo^Hfa=>~ThdJq*AvXEUy(`wKI>c&h_29d_CkC+m(J#%jEi>tdyL!wIwy5+Xh+pdXQ
z?(QKDvs1^fJUTRgEojH4X`njIxdGUT^XpT?Q+%zzhykvPn%?UMdAXwMSTT)0@`s+{
zpSF7Yk%J4J*w&abLX2k22&#M41W!_R-=`cX#p9{PW#_M!eB40JC{D|8o8B&iB>bMX
z1kx4_b6+WAZ;u?4=tLL8^G!VsMvZ2s&Q>uid(+#w;ohHd=9q}%^+r);1%`u~X`$DX
zP%^q5V5kadOq9JZ(2o%|u@r&imaCP;NK@8ch4>ZBoC!YGt)@(Yabij}%WaiLzZ@%3
zZ}!i!v($_|T;cp7>iYIOyu!88^I^!CH6*Bz%bO}Ef=+96mA@Dh2vo1ADxofBohiy4
z7#U*RH}>o1H^~&cS6%bA;(eKDNKy6i(l4PgM~ufVFpXImtFS1$qZPC(?DLGb;~C~$
zi1YI14F8E5!}`AdJ|i93yhCfak~YuNyOLK<#!AVA$(uTSz=n<OVG2<pQY;yi12)!H
zBcmPpvsK|)L4NsjR#R{NDi;DO%JawC&QG#f?Tsr^Df;>OZ%GG@;Mwc%_C+7MteRzK
zlBWl6D**kH1ER0w!;h+WpUy!0@fc*HBM7>wn+8YAQWQj@Qp62}ZIqles@^O$-W-5G
z*SCTz&E5nK$<|>F=HUm*_vHHrzFCPbGRL&`!1ZON_MXDr1{VqtYKLCGcLnJo`--jS
zjMFa;TwgD%^Y^^5?<D93dh;-h3%MrxmO%FV@0&mgti!~S5<ojkWj||x<uXpOX#lA0
z$Fhj~f^q<*Zke`8{cHN$zw$k51vDCWZG~D)SaHNTfK#q~mD#`$reJLD?VyCm1Ax6B
z3l`Stm#80^fd1BU{|VGT3p(!67K{#F==+m)<XkTgOZR241i;N%1JIEj0w@KQ*uWE>
zlwssn!{Pyr39idJ@B0g@)s_?TY<|$aapiqk0_X1ZeYMd}(B6D^Z`;8hgCc~-UYxdx
z@69}8lXQn!Q?I1tvnP~0RD=z5`Kr?$rQI~jjqs`~cQ1K=56M%vcz^ezQu^eL_3JNm
zS_Y4Gd7AnPP9X)VxhT+~9~I{6`x(g;SlC8Qcy=nytrqsS+gIhCM))nOCZHbgHJV?N
z#kQ|pH7twFiS2!8gTm>_-qDa;exscgJDEHTT&we(@11}2S~Bw^zlAtI4GItR%^Jd_
z){FMWKUNZ(%8*mmN1zd1<f2#i{TuGu`w8P?DZyubCCVKm!(oCV*9%Nt&0IFoPI~+(
zu1LC%Ygdh{H>npF8S2uTECnya3yK)<-u>AX@HHb)Ehh@MV>9oycTbi<jUeJlMwYQ)
zUQd?vS<Relc_QA&TXNT}?WCyv^gwKV@R_F+&v=mC6;P%d1Uhk4dU7G-QHOSC8Ik8l
zCBIZ-Z$$)o;lSv*b28C0j*rJipXa8`I7Dh+!25NUKm|`(;OxKZt3Urs>d_ejNJ|0|
z;q}IJ)1+Bbmv&!iMDop9vjX!XOM@cUa%b>gSMbAQBe{bXF=l~IPS$+t{>$BXu}c}p
zFJ&lY8DzjsbM~6%)t@-tjo*FIwZ~%^!V#$+_2+=umC!*MLBsHk6gmgln_{@{c*_q(
z?QJ7b?7D*WJ)m>ugZy62bOGsx+iM6+D220Y3K%(!{lFYGwEs3i9v!6Gw&J{I2)bkX
z3Y)@?#JvgVm%rO%{y*#ff7Kpi`w++TvD7=eLO@BIfrV8|ZK`8n<G}@+7cnWLusyRd
zG2d93hEunyc<Sa9U~YW~p+~a5(LJR-2Ml>JVbj$$Ay?C_O029K4=ehY_#+F^l}kY2
zzDeJ~N@By=-Duvo{%+$zBKFkEa0h30Cyf}d{NZsKw1c6d6E2WKxxd7V316;nkmb{n
zywjpIdFje$BtW>2SI*y`AUwcQH9r^*3DIMBnXpQT?h3;YJ(w}MdD+b`p~<t%vK-6n
zdYYj^#^vN7tZ$8Oqq2GsJ_pRF&fO~hUd1Uqu!n=a>f^5C>n^6<OZVr@cS#X_5c!N>
zXt>DXJ)h9WeI{Q9pX{V4?mAl}H}m1<+P9d65f#T*Ug;C(Y3MIw%!BT<ZZNWu1^~5E
zhC2;5ZWte^L0xL@!yZE3e8G9%la$tn&(imG^Yh<i!Az>UJmfrzmGm{Wv!YGkDz2mM
z)#W5uc%iW{swr~Nn*Q^5`<>l-4@)(*R$!daTHv7x(MuVY6P^(p{f*e|u4$joU`x?a
zN(3>?hpQmGjT`P`o+yu#lhC2Qdg@y=XgNqyLMe_847UX-B;=L(8G-rFPG3)6nmrg3
zDyzabE2JthCNRqea$Wb1K|}f+<NW3>R~Qu~2Q0ymn^)R4?HH#?>ibwP%V80<0{sqO
zL0ia8Vf$>!3}ThId(|_Xw@Fs|YoU_c5BFkau4{H9ZhpL1hm!b%UVb9>PR|$6T$d)s
z=Gy=g;n5?PQ%Yo1Qr*bJ2J1PCi+t9jzyaOL;DfKg#birIou%B}IMc9?ADfPE8~g87
zqy(16n(i1Udj()OgjHzu`&H1%-&)BE0Hs2%Hn3ige=tVdq!@0jbrV;8zulk8Bm4*u
zbD1l0Lzq7qDrX|hld4{2xm@>^GbY;>t$tIp)#s}wW;i5dl~%6EOIeKM-WGgJMgGlB
z0_Qe^e%K*;ebIwiOu30@Pfn6bGqiW6!po5S!uIwMCUL66B1omdU8-^NZRVjf!iz_O
zjwD~NR@thRd777twT@OUczTBo6Vuvd0#kYB#6T8wWr%uvefCaQ-)bCF(3@kJDZ?M&
zY9=9KMt9B4(rM2{#z~}he_m;Qru*)h{bA}r+G4qOPr3v3z3)|H1Esv)jNBhX`WMT&
zodr)EZ$jrJp+4E2j-JpWVMCWyqL3~;94>Z8(7d^oM1Ce}+!lGITZy&P!i-<9g!<j8
zv!uFLbkciY=_nzsq~vks(U5Uy2hla2w}UqJJN;E)D`Cz}EXGgVm-Ayw4@o~%=Go@T
z7U@;GzpE!pN6^E=#}=hu6RzsO<FVj<@FC|(o9S10{&DJh9H(p}nI^1efWWvaCA+TM
z;0_wVd6Q+;-mIP$9T-$bM8#g!I4Eax5Z;7psPgjNcjAU{x+bSkTzx<2V28vVD?&SI
zoz(eS|E;IE!0cl;>5}}Oo-okkq)3S=IN**qvVM*eBJBie?5qj*(<DC?6(NX+Y2-b=
zv>(@)+gIRTY;{>+rOspdwe@Rr)f^eG1C9>byN+hiq$yfqwfqcCplxI;zJFgzWxqHd
zTl1|Em)qZ+n-=^1%z-TTm*&Z&k&!DgriY^RDqFdN?=ugBqOTd+&%DeklnoZb02m(4
zRfbVlcl#+MI*KE!syP*p&0+`Xj`ODmMrH#%QXOB(ZatGYnX*WmynW>@{Lla>NaQd%
z6vO#w+S7}Shi<Q|nHs-^bEMJjw9pN>-huf1Z&n!WmFn*D8pNnyikpS6zT(snMek<f
zMFIA$*wEXrGX*dtUPdsc^_dQNMfJwJ>TXWsa$#AbDAnwEcSE6?3uV?}KTb`5=M2R$
zBE!nt_ZFgztlHe2%GGIEju4=oBs{VV-?b+SuUj~JnseW6)fwGFd_<T~N1HL+RLa&E
z41Q|mPcmUyg_~vy<71v4?9_tqKdGyo)$_!Edja=3({d`Ri8!PPyr*eX52;%J4|(qy
z7S*<;jbcOzA~}PIfMk&zq)dQh$w?>y5dp~{g@jThN6CtWEjb9KL<v$#R1lC@k|}ah
zs)!4yTKi5<-|l;QpWWU2J5TrX-Fv?uBDJWM=bU4Vcf8{rEjbLq+=1tZl0QA-5hLoi
zAtHNRyidNg^{X7z9Z->dJ>_g^U0YT@bt`yObn4D4D=W5pCs~tjqGq5Yhc}^TM$i{e
z0-_zkq{<!Wo^YFx+8NU%MdZY9%RTlf_8Ct+S^gT8qdw?+#k6H$fx#spPAR}8lJxy%
zu3BP!%w$o!g~^=}MH6|GUetuCl|Exnn2*qlyK@$^m$G{m(&J-ai^-i&uPu2N&H^N3
zQ(U^-sBG@N54aSL6l>9<qHh`O)Or@@YjU@=D<fQJOo{sR#V;mBMxz>xV%G}<O!{vd
zx~%0yV>i#Qp|vH+61AA0G4Sc0-zfw+56^>$k^%xwLh*1Mni|WDo>v=z(~t+&uE<GN
z65}gaM}8L&Oe&8M1_5|m#z8V#IRP&up}*h3Zg-p`n|(^yi^ai!qxA!_|G@$3VbT+(
z|7oz|Pjj1t?dVH@vg08{N=1BMgPUL>z?(gay-6s7wI`mzSTmp(VA<J$0>=A7xF?9n
zfwN<BY`>O_xj-~VRwcBT1fCna?Cdl{wx~c_rQZ&jlu2~q8|E3%l@oS5Sy*d#qll8)
z&Fv9&6obm!EzD@w)^2Jogust)Fqo5ghh<Q~tSe?bC&An*pZ)H8NY?e~el&n5Ug@nX
zyjC#wNk)Mc$70mm|5Y<qB@hS92}hG~32F&11R9_$AD$_jMNqDdsb^Qp4Y_nrs-j%`
zUl3j|IOw)LkxshG3{)hf0FcPIYckwSjne*zU{RqXy0@Uu$n*#qa^ys~Hz^i5yXI;-
z*}ho;%R^wLc<S(OneB-db6wZ-Syd4htjBRX_uUQs(Px(wL$aMjlJGiWl_Wt1y-S_5
zmzWiz3q`_2@$Z<X_-Xa&!`iN&VHRK?F=6Id-R@fwtDx_=_g#gz>Z4aDBB{g9Q>y7L
zD)7$bE+3a6=Nk5oFwOpQEw9+zOxufFgRZ)+sP5fTsE6PnwzCmE0QV?$MEbf)>W`@K
z6Aowfa;F(&%3SphIN9CBx3eA%PzTv=7iKv$)l`z_6WfJji#bC`RwGTmlt0P|wR8BK
z2%j8!RD>x+Kl|bbWn_(oj4QQP&3odB&>y9%TVrF>$R2l!)4ZcHhZYq@NhT)WUuOmT
zLKyt?Q7C7I8StH8TDY@Wc`(q{i8nlVxar2MeuSq}LvM_?YspGNNuNXcD*7Q#MZ=$8
z?Q+w>wIs!qZsi^>ftVm^qNov(wY6_70grA=hkS{Y?afRBs>fX{S!(=euxIou^b4k~
zT{!L~wu@J)h^s)6i@lTijTd!dPRDVlf6SKTjl@J0#eS7y>?XY2Rf~E30<eOx!zFf`
zKl#EXNA2G!1_|3exYpm=fhw_G(0f*{s@b2kQ1GLZ_E{dUQ`pCE;qr`Wj;P1ql?Dnl
zO|z(8>b%W_EEQJY<te^VGo!K@yZq)g&x4`(nk(8Lr`dxq=+0r-NOaIR9!s^{tXM}7
zn2sYU0?hsP&_<KcEc%SJX3?ZT>Dr@_$K@3Th1NL|KRF}iZsjz`mOiA#=TN9#wjyzn
z6N1@n##iWOku0dPwz0mjKt?<Yb8c|_4)Nsc*()Zg5|S0(%<->mU6}R`veqtStL4*G
zDhi<b8#hi^q3Y($I)jjN)u60j<EaWY9vOOoeWJqYG{>!2?aqwKGaJFEPM5A4Enk+N
zSQ&GzcH%C`_Y-i;@bx#*Ql;!ognOH%h!N{qgiDj8EANRs9|%RWC#z;ZIN~2Q<2ab7
zqkX7*^v!C9wm-*qjF`&`R@O)F>1+vvC}tf^=liH^C*{BHpv@g>{zez&;6K7k8&@8Y
zDaG(*sl(y~b9^RWNX69o>|5{HnpDz|HihT2d9uatzLRiE)wzhTd?_E=9)zURDpVFO
zk7*ewr&MPT^%9%%YUwa)H#8_v326P+CKQ`Tqp~*idM8#XBvbW8`W(>42@gI`&;-vM
zjI+AnQ^e`0?Tu5NWh6<WYE|)S_L<?GG^rKcSyA!FEgj0^qA{_S+P($Kr!H`IDK!Wm
z#_R96!*l3mL9BR_Fg|Cr&XkA<7gymvWD3-(BkI;R7WBz3MKy_5Kz?e3H)%M}#x+Bi
z$IbPES-~>(tbK7Z+YM=6Pg*&JdxG=7wEfGWn)|}m>|Jc%CAHfN&pC;?m}@;RLo%%N
zqaG@ksGE1~yP3YzQiw41$h>H5oT_gtGLR}ng;vZlXz9m1GY=Hk5zxHt&d8Cnj33r4
z9W-9mpw{M2@Zl`zJ}qogSn4J6E<bZgF&I>Wd}`p`v<U`mq*2)+#Evi_6>T$2iUQl~
z-@S&$i7kezD43@!I*0|y0HNNKbZ()k;KpAFHZEAIY$0s{xT-VZz^o*!NYW18k)_`0
zoXgnWEeU=K@RLQMsb5g>-)i;#iU0pUr66;~Z1)fZI{u(ak6?%nRRS#~#_R?=aKWbt
zdvV)2k(QPaT?cD3)d(L^BE|{rSTrG5Vt=A7$*Pql&jTo|KiJ)$6;4gNl^C&!1;nmc
z5X*bPv?OVAstN)Fx5J#|x4}{*)8sj1E3MFNLUs``y#v8iUfx8DF72U=swpPP`k#tE
z?bGNka%<-v=k~;Y-hvSCggDa?(hKw_8}OE1TJE*dLHv}heWTL5as<jwx>&z4jUSQ(
zvsv!G@SV7TJ5U+%=$LZ3{|<tAvKt*t3!1yD3~<+W#kdw;+kA3z5s`}^c+!1BM+52{
zb|x8JEeX;HAyX7YMB8*^#$pj6+?DrJGu@!zh|0ARwSquPf?h%KhqN3wn!y-RYO9#;
z1aZyysnZU3>Ddy-S1%qTYQR+11i#tzHUz}C)SmE+=OlqQs>y_CWkgOSG2^WfA2|eH
zUfGM5RMBw2^PU--T&_L1!&j7kyRN=2`?UBFtHpWW4z-|B5UK7F(xaMA9G(vD1k*P=
zq8M^)pQ0yc1cN?BZByLCKgI@4zU@qM;&-3fmabUFl|9+rt7Uzw>*`G%*RRW&=S3S7
zrbh8A^MHbsiT3c9B2W;Fq+ZSa8)Z@BUuCrYfYa1m_8&6dm+1d}j^K^{*dV?xJ}>}K
z7Y?Rdv*f02K-P3R+J+fQcEFd#j9bf~;>WMcKX>N1()>E`i=&gjw_}iSb=u0@6P@S_
ziAN^n;A7~{jVQz#P7e@oalYHB=-;+0S1l^kXqZ8tsSaN!ToLljO2-Epbvj;}`z#-X
zOtUQYe5A}D%i42Pmkzv&5ZCg4mVzXP1v=<p6;P$OiwuqDzKEC@XNgpo;e)~+n<xtD
z<%U_xIvXVyDHunm%LuUx2<$Hr16mPmu%!H_zG%N`3W<9Skl8Fbc+C>xoW-!AF7F~4
zN1ZcF3bGO+#bi0HwKiHX0r?YR8xBsTlj<L3zCM!HXy_yHqj`e)NI}rCxgRbwJ?X7T
z3KW$)8|NA&&=K7ULhA&XwF1rIrm?K%7$2X0SJ_Xu#QFLch6+q<1#qK7XAxJI-^{f@
zzA@PnpwY6tl~lL)LtVkjWux}{?ABb&RI+OTRHo4oH(!AKjavsXG%_N>-E`^^vZ-kI
zVPpP?aLN;GgP-g-4;U9a^-zI4wqnL>h6U_%4w_vI9i~2O!v%(odQxSE`Sy<c3p0S<
z{2(D@N)93i<OlcF0ZM!|B66Bl7Q+lV-X0(Y&6KmuSWJwLjgMQevMBWy9cwM>vOE4T
zvF612tGgmcrMH6l$r;U}ci^6BmLHNbYuxU0@*U{N46`=gv&)uO$oWcc>AOMRnk*Rt
z*?Gki7NZjqalftf=hvydKGbQs#`Zkakt~&4kN@;tvQ+yRlJ}+ETVl7Tg(-_{QAo^3
zyl2hDp|&RF*H82MY~1P8>C0oxUm6uq>g9-(S&g|!2($-0m=(TDkP%g}VP%iZQkcyr
z`g#~7wRTjNMHj!;k}%9H!4IDrD(P0794zW4xVK|NR3!$3C7h64O#*~Y5G=FXHWOHK
zX3(60@_=yXx+BPOr_8U&1!f&q(O??8sTB9_Mp2)WO6~IrtKLn{*kaLT-jn4j_I`8H
zrO4p!DO!jTKkh2>lX6IR<LO3|V^FbH|EcmHogumJtLp|VFD?3|8%k@QyD5iP-`YuK
zUbd~IQDx*-xzEcOS)f-a$sC_{+OT`!#+t(DP<d1aMunz|t)=38%^mF`F0YVs!#gF0
z<k4;`rSiy6ozzxz*Yok=d7<H=WA^qIwgh76#ZUd=5(>gySr^q~o%K%(AIc;rrx>Sa
z2mV)_8ORy}sLOeYy6sAkGF)?pm$5UM)0?)~N=fo_x^PhQZ;r!H-;&%D11hZxpxi`?
za!r*6-E<$jvjmzuxAHy!_&lj8nDo!oj{n-{PYE<|w&*Vz)Mrl-^CfU6a%YK%*k5Ko
z5DG(V+RXFr9AM$x5mo6n-u(;G2?skjfKov*Z{)Ut8u}6M=WA-Ry+il8uO}fWbc0bO
z74mPO&jd2|HVSJ6xe}^o9yW6jaB(EvneTuG(MHF%V%ak>#f44kUkbQ_Xt6BFr4BQ`
z*S;t(ebU$<W7C^v>2%ucrSDop#2TVPgTfnK4mPecqMQcbJ{h+dwhd=`46Ozz7?s4g
zv~1PMlooJ~j`x3ZKxO)Jsh>|FR2SQp-Y&4LkYv3<eZq*LOe>aOWGf`4i0g{ugQ+gs
z#s|TX2FOcrAF`UIfaq-mMa?9zqhy<?iIRBJcP@EQqgvfivUz_I>u$HVeaVB2oK#nt
zT{F$5(mUg7GI#kNri<k0o87164FCEzMe4g!HjXyxS(Zp<a^aXE=mj`f!e(4xm~~4p
z%hH68DrNQh?M(ZN)%8S_jyffy82}R2%lbW+B4h;14e87DJORT$>y~kWf^pHWPJ*X9
z43c#(75*DJLFV9I&g%T{6rR{^Mr#d~U*gmVL+s2%-@Z5kkXF~PA`X%sn2>#NoAMy>
zztFNY$J*)NA*V58996$xr#&mqKJ+y9>~Y=X=hsA^c-?~QMIrBY?7mppzQd?1h;}n1
zHxcD3pxPtpemXgP4!aF<Pt5y?d$#{a0rUS0yw$Z@^rG1?qBp>|oPga>Kpy}e|1TM1
zAhjuq#sRgCn+aQyf5P+Ma{qUvfaWC3wiN&v5`*Y0u?LS|le9q+5No<!0OtB{%uCbE
z&3sM>DpfAj;H*S#m&`OJ<Y$C)R2M(oeOHt8z}5Ji6Nh%mh%#!JsZmp;G~^cM8V2Aj
z&_%O01oij|z#yAxP|3(=SNod<NZ0!%N$%*&);ejb6ohhebNL%h8PiMF)|Z;zhffWh
z(DtKKG`1@?kC(Xkbs=T`#aCU>xb1ssbLWvKn8-}N>+uLFjQ+7_<(4KKA$buP8@A}5
zx@xxz3=N%yYnrd6zv+|vgm=^`cc$`gvs>wpEig4#5m20c=UK!iBxL9#Y#b)Q>~E67
z&sm$ehODIfA&Y^(jY-CyBld+pY@(d&qjVlQjmDRWzAAf>5V12UH*G|KO9dLm)^X3j
zTzbx5$#)Y^{Uo>p^SNAduIN)1^)8-3840sO<GLeF!o-M=H}L5(;$jbdT>$HV5fyAw
zPw?GdG8$aBxAlD}FkrtqI;qX)Fjmi7QImdp5j|#MoM*(XE!FPuV%^OsrUSspiS2!q
zrqCUv9Pi#4)I>uCHM@ynHJ{}NOV3z;MgIP_m*^qS?Ijv+of9;zC&Ip7yt{h6S|EIv
zFyDqx!W>TNsLG`Yrb<4Tm}E1|e6?ig0QmS?f%{82|0To)v;GDCC4M{PcM7jBh;<3%
zf&JOwg_+L74wqkq{9TgN-?;zXog4pj77h8|&HZOG+p{Hw{u{d<^Lb;Gw`IE8tqPBf
zPUod=7KM&|%I0*B%VC)1Kt{rxlTeYd6~0_cLc+{AqMbwMy6|ZurjuWqa*Mn_(+1Zo
z2ymS#FsCayZnjwT2|)*sAdbjvaHFg=AzHjU`$_|#hqFPgdLs)^O~C`bk>GPf^9{Ap
z){5I!dvSR&wt;<(V%JR0Hw#4;t6RXD(QSskQ)lA!+Z6n9Uz`><%k-HeI|!3HS2za0
z%!z@XR|R41y;7ya>{?;d*i%-km0APw2Cr_vHWD*0eX*Wfu4IwF`5g=xrYiH1EQLic
zV*wHG^6TO)JLgMB11b|G*Rg_Fg3E3g6hpW%+r;4*sd|y5H82m=^R{PQ){ac?w0tO!
z7%eZHSgw4@|Lx=x{<Nvyjxl?RaKYy<d2IQ}7cp_oj3hSbRcYYEA?$<c_{)%sKJhLg
ziXa(6qQfh{D%aK{q({tuI1HK?3$OBZcOn;+O?};b)8|f6d-E>8*U{XdWSbl+TTcbC
zw0Kr}t#USlw#Bm_2inv#J#TlxA0pE%ff7VjdlP4Y$YvIiF{Pg9l_p^_54}&Ik7&Q|
zPOn~|R3l=0NuSg3S#qoPrFf1^DHrOmsc#9H#(W%3w_{@S!nCQ1fBSRugj7fUOEHD{
zuP_olUq%W-><C)G8M`^KEZG&e2}m|4=N(~|y3XlqGvlg3y&r60-d=2ZDa=81fwfi4
z$WI({4x)768p0Z^nSt142jS4*CSVR@WmK2{*<!X|`~Cl^piOo7Z?dq6f0Kp%#Z|NX
z>j}Le0i+!1B;Y$uGoW!5S)i7yVr>QRz||_z`>fw~fLmHr%G*|tIoxsALLN9DK?*HT
zpCp--1rNcpQ1{%cD}wG+PT8mWNsNSsz8?n8()r$(k*HTjO_fRKGXnSI8)ssL?<bAg
zPoUa^Y}H(&KiU#gHxG99=Qk&ei7y^I6=<19%@EB6l-_x|<rZkOS{mo|Fmp_#T6z^x
z+lP0UwhVR{<#jbThMQYfIW;8a?L@P^6fxBGQc7xVVHUW*pl%|(TsXR!^TU=P_&dey
zUQ7Bw(-DWf?}66|xTrwIIsM}{-SbWcY84MSLbT_5&`iodW*XmS?2cW(T%XAP%=KvV
zi*Mt`L7ZByZfxZ{&x$Ov3{MHNo>eOj%0|?5(##wYb3wddxJu#s>0^(xfl);FTj-7(
zf?Fe-S(<HTL@*<1_e>!bbBPg#${BPy)RN^>uinon(Webh5VMw>H|@Y-LR;>xZgyo;
zp<aG_r9V~>?c-pWQaC?Yz}~-`g72yqGOey47bF6x%Kx+fQaf^=-WOYt4yRk0?STZy
z@kJo~K}$|VuRjR{0NIdp2R3Z_FQilyg8rUXRFC?9JFV#dM(F;{R1^(f_<!oND7kT^
zq`nb%NS=ufGM}zizU#)r)_+xjonr0SIiBCRXyPHBRPue?rXyHVX3<}6l5-L37X65A
zB67q5O$}i{ky^Gi{Bd)dd?O!ig#rl5g$`c0g*bhXd<hU;x4eH||38`N`tKJH<kBD7
z4+H<vuDJ}>ulY|%uCIVD*I~D^a^Klkjb$zd0=3y^C;FI<banff^Q8_+-B!Qads%wO
zte<!Mu{`-ibuW7500)$sh!AcSOJazCPg@;|!{4VXe{{C@#Y&;}NmjgVIm{>vt5zE^
zC5bF@dfwQB<k4r2<Q(r;aeG_)N&7hp-{s<(;FsvuCy2Z5`+Lv_pha^E6mVo?D_o%V
z*3lD>_f>^x6S4wNJgyei%3^!3SW>T@cxZyEa1k76-nHWGArLwhdz}WpE}OMnkH4I%
zR6VDZWO&=d;7)h4`aq-Eqn~{X<A?6*phU3tC1CXKVo8q@eth7BL&h9o$}u3qG6v_O
z@gI5L%99RB0N!j3MepltkQ9#Xf(11P9WxDiB!I35YX~Uej^ytYS6`;Xr$9h0u>+?8
zM(x=ERdD1ttT1glXnc>BnEsbX^KX(?t`DZSS%9RK>aPcd+W{4qXDG-|u}jmldoUM2
zka>8F_{Vc=RIq}B>U4vZ0|eyhk3sdR5qxep2oT%NB}tr;@Kx9vTf6OkK<$(XtmzJz
z8r*Zlyg%F}wm1j!pj@W~EvJ8{V8s&fys%OKpPYa8iU*c_^ga?g&r52=9a!cNDXTUP
zs6L@*M$wy;o9;w}@t}kX<C|9Z;_rQ;F;^0A__>S4#f*DB%Dwf~<A@N&%+bfkkdq02
zlJJGBV6G#F$IyM)_PO6Fd{1Kw6OrIzsbMY@5|GAQF~><_1l>0ys-OTHpT*x|!&&|Q
zebUd_X^nod*>~@v14_{To+6*9{FAU>H`2~IGVg0d^PF?watiA%aTt2%Q;PRwC^4}U
zRzEwxYjUpMvP5ov;mVsN?MJ~;AGtVRdej6yB-UKo5uOzWsLrdD`iT{}43B67t?a!<
zw~>J*Kd@fUX#l~_e^y5*2~99ULV|rU2=^c({F!pReoTW|OE)%b7Z!RtKNP53?2_ax
z)OOh^9^)zyUQ#4mQpZAr!G7I3-sC#weK4x_Skcra{)^|3v{-H3+Ya0PNsbDpRWGII
zsfwyviJ9DZkV$Kl_r0Hd$3Gky+H)z@Lt)HjoEn*EQq-s5eNj*Le0@pRWz!V9$$X59
za$(=X4VZ5ZS_0u~@^5JT=~;c)`B%+03I<?6|NJs9bRCrpq!6OvvSmya_(NGIa$5-v
zX+8r5Kt(*afGR}9ga9BlfPo#RfRQ{1S|D3k?ce^hLxB2nL_B)iUx^Yvn+=xv#awLZ
z!LO73;g~?s$RnLhg@c)9o3JZ;={pYY{|i?M#4Iq}=x<>0!VA)M!<qi)z!`#CklUh2
z$SC^PRZk&@Q?aRZP(;F94w*;qDagACMIrW!0aW#=3CZ>O96YLYgffKgsftF(6*`g$
zGuf<PgQRAo1jH}~0eeM<H=V4oc@50u!%*z9DV9*g46r%<CTy8#Fg-Yi1lG<1Yl|Ke
zos)0r(U?7spqB;nGAx^sC{gmp;(_6^(nIl5RE%Nt$UyJZ-0dN^0;zHHj;$V<#6^E=
z11gbs<Ov`+#17Or4~aSOXGu_BcT0BlFuKV`vct}srkgsx1L!_vnrbKEFh*%-7fZ|}
z^Au&NJY&yStf$x|Y4Q)^--y$&P|}CC-WdG`S^*b`++Iwnx7BAimC`fvO36xUbggXx
z`VgtAp?}$(zHlVz>ah})BTzq54*otig~5Y1?YptWlTV8$S2(8P2==pr1qIG&j8{JV
z%t1Th<J4WOx;5wc@b5#6EzT#Ny5~y+DsEa0P?>5pD);h4c1jEkooZN3JwuYUO_Ni{
zEWdYsFu5M*+@mNWQ)=RAsVL$Bba8TBMfsPFN}N7EcVB<Xb=K{-&#Do7DQG9B=`EmF
z4c6w{Xd)=ylb1a89cpbq;KoKu!WRRt#c$otCjLB%N-%mZ6XknZ2K1M3bbMaoC#EUU
zhSLSGh+`Cbxis)5c$pxq2B=rrURUsu`zB5;`QBYBHGJ&tWHr$a=gWG1JlVawPFG|#
ztB5O5`;#I?qckJ43<l5iL#ru?zK5J!8(V8I$d`>-`jJ2y6Fp<hOY%ziA+8Lg*Rcc<
zI#?081h|3!a6W+dhf23$w@p;fqw&|!fP!s;CQ6PLk?FvEz|#a`R}nqrse3@I#i1L!
zY-9$ype}$hb~_k50;F5|$OT2{K5{yI0t~{=i}1i8b_oVU>?jz)bE&X5FCiS24o4TY
z>60!3`}3=1+zl0N;M*I`3c*akP=J1KB<)=%Am-AAfNt<g9wcm`{a&E|x1Vf~lEe(7
zgvemQpGl`Z-mjB4*Z!M0Q`GrqKq6+k)js6*TT1S#3^+`bGz*B7yi8D?5gCKNuTQhq
zEJ|dJlu4k42e5R#$u8xqxQ19cKUN-9!cO^*;c7GQuW4IIwW6`0{1&ELcN6#*nBn+T
zLiV}&Wc*hI?Y$i-`gTx5`PFW%<5hmq59!mUHgj2eX3R#Zhnu>EgH@{7XWr&ZCRJa1
znXFIEzEfZq(F^YHOg=%SKfMt4b7ua-(14RH&^8y{GWwfO`9EG%kNEqddJJZNdl6&{
zv2%fh@jy1HPGz(6xD-j6*q>x!Z69}p&{16P=T(|_fhOtXt^W|P{EUX=aj=fk7ToQa
z03>rJ@sG)$F-ynbQ+|iH;a>D9gxwnEG|3Q}By{5UqF&8z#MP*M3$&i&X1y`2m*r=R
zZ>#yhVz%ee>srBcI*q;d;q&%$hL7YBc3Aue@Z&(q{V5Xi3Ir3Po9f`_<?bq~CZp`3
zq9+|gMJgRluN7=vdKqGz2V(PdJ2bbQs{1nWZyr%)UrT8zl%_u2c|uK#vCS|xcmGj}
zgUsNjlk}RU=sX5;*Z+=z<6q5~!CW~s=*^adf7sIOcqj^DsMl@?FcXQqq&ItrYA;aj
za0cX^@Va!c&cR6Zz9`H&xGM_6;6exxej<cgc}Z{c{&ic?-);NDvHs$rFu@R-D2m@}
zcc*Kg#fE3g5O4`9(lVRlY88-?dF9A<-TMe0pQD>CPQPvCP87^ZaNiI<capuUmXb3f
zqPp!a(U{Md=kfQ))vv#9JlN8sH+%HRtg%X_Af^SEF2JS7Q@uVdE;6*L)$P}PTHe9e
z&&Bx>$jEqQ9r<S+mH%Kr%GPG8eUN(Ui*xWlOF9MFa^?GIq+1h@b(Ak#(~oL+<l`nx
z2zyV^K#>?c?ok%#3b43zEM^n3+guujLK4>R(dS5BZ^W>QvdgGZ!S{hK@WVNU;Iyw&
z^rYt?>9Dm_`B#%!%$t2bS^@pT`%oX8i5ddFdU19U(#WhEQAbC5+4lonz&*)XM>zL!
zmY)N9^Kv(KUHLR90%z*gK_AA;!=I!OpaC<%f{+dhdW7JHRo(+@Pn%>L*FS7PF85NC
z`@o)ehY;ICbhBc}!x4TMYYcLB-@zphc>gQYgy-5J7(!oul8ykuAVBv5x*Po3nC%42
zmIpZ%)LHHY17kDVO#+_8aAE=OKm$j_fo`;NUFdaAP=i_`g(zwg1K9vKTR;GKi#>?{
zWu@EchJG;||7TYe2nSPf^S2+KfrkdvH#jk$64+CH?Ba_K6+28)Le8A#;t6l=!o*@{
za*><%$QlGWF@YVuQoq*!1>BY^xVP_}C%_HJ#EW&qYTtjSI6T^pUWtw%XwD!aY#4AV
zLix@aX;~Nc>YN@qrn<Y^ow36<K9%A!%6Ey(RogdqWLLb9{0@Vn!<&UwyK1-D@N2s=
zUhn@Q&^+wxs%`P1`t9I4p0lu_u|DmGeaNX!N&dGcTuCQ<c&QQd*9ky;%o_`$n(`fg
zzaJr<hnE69m9_2hKN}tuAFn;fMd6wB_()>XRUvlFMrKz5O;z~2{pw37t>H~l0`rRw
z*%Gyu@;ib@Q7<~mqK!ODl$><iw`ArA8|z7)VY|@RHQFa>qV#%cqB41*USaA5ddx(R
z+zH0Ol9tO&My3sBrW&*bBqMT6P1&KVv=)RV0EgsuQ=)B6bsZGtywxYlC8C})_RL#q
zSt(=ZgLf<MNfl{N{Gt9?>y-xCzUUnVA0+LyVwXMB0hflFmWXG10|VOa@^#64lU!#h
zAK6^(5Z!t<E7|5M-OgJ{Ah<@XqltE$KQ!=gOaZ#ZwSpw`O{_^aKe=!;KU<O``&p@r
z61k1#$eE&tu0DCb-j$bcI=tf5ri&VZSqQ8k9WtZq-HJU+Ld*drl^Y+^o5hA@=1TPa
z?A>##<ulR}1=_k*lgr8{^Ewy5UZ}4b`=-mDXpV~s?ImoM$F$BmIhZtKkP*Uz?Y5Qc
z>g*=|KRqgQ?rxG?)uL>=@_w6AVs^Iujp(^pp!lss3sV!*<ziG2j#00$*B@7z)C&uA
z<H3q~ef?mcXJJQp>tp|ep*W4D{Gw^bYoXJ{ePc!-x6CZ`6tPS%3DfxU!diL%{v+na
z4l){1?H+@Dz#EOF3}#2P#<5-_xIM*loAR=vs&t2am9qqcyr}EaCLT2S?!3g^Ye=H7
zzdXHidsL0{r%QUGNdR(mX|}TsLl5=M_O<ZF(xdL9xk<uJ-vm_86T-?DvtC0vSBYxR
zrdJ-vTAGmCCwpIcpN@9XF0=Rb2r;)vGP@)F7!n&7?3gXamn_tji0KJQf5Lm3$DR-|
zzoRF8u{=62uGOHVN4Oy|o|@Svr9e-n>|36xajBL^iJqz{zhuv)@6Iox?3agpJRmXa
zz$;wVnzIjVJ&CQ_v^M#Q_9xi2nIj4m)bmRRGUt8>cf5+7ey2~!`B-wiw?XH1k}$Xa
zZhWpsSM0S<^-GBZ*rFUr-iUu!6RP7e&qwp_q{L}n04Qz?<4jf97lq?VdZ4a%9=G)h
zEJZvx-klbAGRrWT9sQ{Ry9wHMV%KMUG34titjJ8%-1Rsf`h<4{dg*fVbUo{MHgB)t
z0+-E#0P8XG^rN7@(BabIS;YX$<dW{NayNf1(+`R^I5%A7henCcZE6q}6Zgqte|72p
ztKSQ}GQ+!%a0lMVu39r4dZo2Jb&odi8ezger!L}$NME;A;CYCXN$4<2rCBD)WN%`*
zJo$6Skxu4aasWwvBj%&z!e<3vM7C~7`hvig%EJWmIB<XM1D6P2iQ3yBrO<S2^e;=2
z>^tt_0V=CVE&>_%Fp>^{W%Z3PC1nda;}NjV(rymmt+NuP0dM_^USlP8pWR`4mpc@E
zE80;ILGA<#{M{G7dH_IT5OBuNOC<kJVeLytmUxfR#K=NAR|$eCniQvfk7~{w*<Fjq
zy_!X|b_d;B#_^~S?s0s4e$(eA$skv)BudYd{cX~|TWAF3DZ2^J0c`&rSxe)$D1?h?
zmg>VRCwQF{T3;SsbmF;RK}sDZX-5_r7K$>juk;4iI4gRo8!p`0u<$B!Slm`t5={fj
z>p<84gWRAw(JWG?hfW(iR88jTg5d?b+WKKxixHgOyKsYgn}F^J^WUxp`|(}-Qt{i_
zkCzmWA3e2vjs19#ny-4kfN+a+Xpv^Sap=9wlKOT%5l`LsifSclnu4=~W|o-)p>u3{
zw^MA5UL_gE807G41tvKUEVv3=G@A}QdT4nu_s#R%dR@j&)w4vN`dN3{WgEUhU)_LD
zb#k>f!UN?(3^otU{N7TRSp}(!9Xp;po+#t2IT6Y+>xO>78&aDs`wai}cM3+83o72;
z{yaH!;rzq4ImQ9|-j!clDPNR$d0Y$8urLl`zcyPhknpkHc0tUIL6p_(yDir{8N+t7
zB_DD;cYT2+M#X{7Q9mPar88|MFIPe3&ZK!Ob!1Ah(e7{_2lKt<{&}O_8p9Z$l}%Yy
z|1p&~pwAZn9~kxh_rbshYzAN{(CZ5@H+uE+z#5r<ZJ)IRyU*!>*rkmFgW^Tbz!VGm
zbKNCSfAt+n)&Nusv)PE%YryfMOd<LG1*t7ey{a<sHt3*k;O7*9&@PvwJy0GXfB=$A
zF%JHO-F0iG1Sk&UH_DaazvsXPg2?#<0~pF2<PtEHV^QD$x_!v|xDF)2KyqISGX`LX
z&RFdB#Ln_OxDh#UH|zEwQXb0CU;+D{Uq_Rl+F`fbZ$Nh-3-Fm0pqpzThXiN2mqn8c
zN%9pCb{7o+-5;8jbR}G*EcS%+E{9&~efkQ_#{I}suD>y;JaBj(S5`md7;<&75gC@V
z$%jJ2_0vF<V&ve%QS7&9h|0h4hLu>kQp(DGMKzrhmMQf?<w~fpoUOe+`*RlqZmH*`
zD9&Bn66j&6$RXa7GWxjqtz$6O(TMUzC-W&iQgSvgLurTfg7clq6ozzBy`Isc5&|+j
zSU%A1E#&_jieHnQ+${NJR+|pwv3R2A+PqNu8iZD)rPFhB72H_TTAlYb+eLmw_ez-r
z9;VVH&yNpss?6HxB7#qn69f5qvIB3FpoHe$G^-NH9WfCDO;=7f`jxG5L_O>Au`K|%
z{ruh2JVR@{Gw<;AWNAzctP10Hd5^(@kmN`HPkTH9gFDiBHi1`(+|9Y-d{Dq|&@-Dl
z!ftZe)HtVR`|QND-gLe6SZe&0PrW;oAA2g-8&9i9*taQ;NYpsZZN>^xeb8caHYiYH
zAF<B9K}TgS#${L(-98UF&An5IN8t%5ry-L|)p*OY-A$64GTYi+uZI{qJ&81F)~MH1
zHH)it>Gqe0O--cCUs@VJSTv8sKN#@zeWwsPxPVbKC~QawhpC8qiHG`u!NjzJ+$l?x
zeLD{q<mdF83qvSNMpkd~$^491(}g?r0j3#z=X?yd;_`>V<E13tPdGG&`)%Zgww1dP
zR8UoG4vn~qa=8{i%j5Fi7~4{{QniPs>d$L3liMmcZnAq;C-wv(nWAJodpIfMjWzjw
z&z12PJ2>!%u-`fte@d|kS9jxlD_D4|j9tvoqyB~N8U2g%<b{WGhsg24vImlrhVW=x
zfP{?&#z7<7sCu6i{j%H{Yx6+~C~pTN(~axo>$N4)tVE2$z*zQ|Pczuf#Kr7_770O9
z<`USsu{roXfP`|#uz8Y@ycw)3IlD*B{MBBfSR-U@zo!A@NL>;R@bYksGYlNyN#wo^
zHfDO)LkS1J=Ap)PlYnhax}HN02T1u~wf?uJ^WbwldY9S?%&M>O(+5ELN(oW;%ZESu
z-}sL%639t@*$W=b+0|@522ddzc}(3JcuwKGAIOaeXETG%F_IW`<XH0LPsUDf!Rgy9
zf;0hKj}$TQMW<%PrCvxLo+V7rB|7{Frj_pa{5!>@CvsQ5_jd{!96Rft4DK_41_}E2
z{whEy`e#<Kxe$|vcH`Zb&KX0)pOI~S<eNK6Kf!7;?~soE{)iD!2Zb|V{OY#jP7bj{
zZ-4FE3W4_DpysZTW19R*NUE+{+`-eghMLC9*&0?yl%!X^1nYx}RBmc>PgbPiF1KIM
z_!?W{`{}j@H$n1K&Rf>DjR;ldZzSa+k|5EhrsRRCCR(scA_JHQw;i)8^`XKKL+%1G
z?cF8R7{$W0u>}?(;H#Tn32A;k5X=WD<0D$QmD^RX4D<B|Ud~8%!Zj2O8Fo9|KgceV
zeZzqCH|Vmh+PWjQcP9Me?TwVs;|XV>*94(mdSYkTjyo5Wa|<$YDC#b{vnn~~PP=*#
z9OX^*HQc*+%x2llLu}6tN)n{km)U%)c4qVJ^f-#dl^2c4FifCbB!u5cH|TUw{9$sv
zf-0Pa?alRUR_1ljYC^U5DZ2}0Z`7$rdkZ;IH7q>~@%sDPlrc6f3LG(4J}Taay1w7(
z6ewJMqwCz?YOpIyfCp*CbO`ZDsl4{lCc>SJrc|k`!rD*C6@m1*fsEvI5+y!}%KfYh
z2VGdOG)cK>5dR^2?#@V0GU~IlW}(#=zJRikC%0V1`0WNN#S9p%GPe6zf`<Xt9n85h
z<`@T`gZotZ33Z>wV<ttq4m<o*MZZ;$)LvVzdTF@3maN%OzKfqd=o-G~BTwa?{eV6k
zBK1w;I-G*$0ziz2#9wHZYaFgQ#`=SKCIESQm1FgeU^#jw2F)|xiEk{!*4QnOx~~TJ
zU6@uLKlMsLD|rv`V>uG|RdgM+tDNrPZ(jOQc(fV~zHPm7YW;Ei@KPUlZ0iC^zs=%4
zKYd#3vE+G+3SN=2@K!x(W5|y-CY)Ld6)o#2d_6jzCEsKCZH=w0V#8oyDF7vmTZcO4
zEd=eg;%$vMa~#=i_%DS$yy?h}>siJ-+Yz$6%9Zro-ejSg)I_$&s<}nw(FRZ*aZUqz
zkWD{OT~23AoQ`kio6vQgK^$Ker|~=37<EnC+=T<W)2yteH3om=$GecVmiIlx5_9vD
zJ}1^u;*n!F+8<4yCCL(SbB-+lDG1HL#pgBhoE(}Zs;AFMo>|))xm9)N0j6}t!?HZQ
zYMxQ$kx|>t#60HoM@*yyV&9F%Drovp463q%fNn&n4ef9<)9MsWEfj_C9JGZyh-$-y
zF+r{ct7Z)yS;bBVzRe%pkEwCDm9f4QFKu7qJBN5X8AwC839CYyWvIQ~I$~dfulpI)
zxDJu^NucB`t=}hB;nFoiV85D+Oa21_vX8mv{eub)R{A%`(|zy}Z?_(9Hv7hfb9bfE
zP%;{HIfi18f(6gu-dzZEN3)c5AXkaCvWAx%ai({rtQeGud>1*}Y7%k9viYIcM0UKz
zvs7P>RL^=jA+C2!EB8~STsWDdjZ41y*ppvzN$`B&FqTThaHQrjN0zP`E*7xa1@aD&
zZsd~O$=Dhs)q_MRq0N*>fRNRjn%h%qwft^+QVoA<l+RUfRnhea1qJq{m>X7Nf}y1z
z0|^!MwNYatY*^}tnomGMDu`Kem-d6*8e#~V9Togb5I?^9U?i_0AnU~TT!p5{otq6_
zI=H2W2Gn6~4;elj`;|FOjvzOo*BuF1__gmO$m<{w^TWa1K%@m1gWeGzap}hETqv7+
zA6z#=WSR^AjEeVcZDCV1$RXX=G>***6mTY(;3`cVu7BW2Ywb5)(hV>%YE*6Oh)2Hq
zWLvCfvUP^NKx8mYMAj~cYS_bbS0TeVf4NIKRbjZmG;4Lpm(OsBobw+3Gf$E<fp9?Y
zUzvV__@!Wa2D@|5$rn!Cf!zN+m}Et$g4OCH$iXc@{!956fr<%X_$%D`A8cPeCkb6l
zC%wS^0GQl8`wrxR+v7vI=F6Z~Ckp)~E_et;a_TC;hb&yeXn@5xvDOU4V_d9o{(wT2
z<UXrC@HDBJ!JkEVTb5e-_Rlr}5bw?gK%>`!Y@XUU5;_eAX9#l7m~bN=U5V4~@m9tB
ziQc6)ZztmM=_1$IkDUuqIN#X!!&MXxd5nfnj{hw`q;|;*bXIW(xs}G-K(=cruy6K`
z&OM?+c%Ep$;}5(fQhK7B|7>lDOVqsi>k#=gi#t!=b#kQn+1)x~aP9tu844GoYHR11
zMte76tg6lCBBV>~><}g9Zh^KhPgFE!FyWR7Q7Ts)@pP0o1YhEnCnYVz@#nXUtg2z0
zlk3P$s~5SMf&0gHraK#ga^h6jXF;oHR0G|aJE$1-<^e|Q%CxT44IS2RvW=7D<2{~C
zhCN(vQ<jaT;e1u?29PaY{}f3C3c~Y7JU&@scc33CqfE;%;3VOCK5g0J<GXJE%NM?u
z$xgIrzcjIUl4N=$k{u!4#x5v|=x7OjXH0nMK!f2{>oQ)kYp#ju!5bYzrp1)&8H?Pl
zT*ehqZOmE}+!|~wlIQgk{JQMGW;tv^^&&PX(2cf5DoaPX6<>Mqwxh+Zv_MuivZM11
zImr)cxR{kPQ!tNxSJZZx6OFFg3z5Bo{2>LEwF04yP^e?%fL*+XTAvYReSWa!6sEjD
z&??|1Gv9ku!9K3{Nb^J7(D2MOO*1RxjrWUb7DdIV5d)=HkW5WL%h4@R>fGQ=<+qv@
zSJP>ehPU<cEBUFujyK=GkH2M4-<4-;YcohLFF=3O2LtTmxI4(OT}#zxtDK<mme;+1
zsb!!bF2(t}L0MD*vsO9keV4&PTNfSusmU(-($_LY>}vjsNv-<4G1*6@&6R2j%;+dj
zw+|G7dp=pfG~Qa@aK?1uRpHV}{KdNaYw(o{#9HX|fq2;g7{AYa(eneaBzhF|p*sjg
zNnl@%XIW{>;V7If{RK~7J-Ru5Vc)g$pbLHue8lT?>*++)v*+js+z&#OKvMXWM#w(q
zi;9mEAyjOzRxWIP5U@0=19<Zw^^94?Nm;nt$_Gbk%<(=r0g_12I{fl4!xDebTK`Xe
zS3L(AfRu92Aqdz;706lW?SxtGAT081Aea`(i{v+8=4A%=Vh=QkXPT!mXkc}x-=cTV
z;d982m93qhfrBW8*j4Dm?)WBvYim08{V!kPe;aVZ0r}-QaR3&GmAH`g!1fPE?Izdj
z${*xSDr<+tB<$t4vXaNFJ7pLpJRW`Fq`1oeTr{PQoQ%HTt$Wu6Bu-C|PVf{89bSEP
z$RhMaH}faUnVzC2r;ZDaJ`!HeZJZ7TiUli(9ULGp46H?O1F!Lhlf!7RMEJ>C(PPir
zMhe1P8%#XeESXD<oHIFmmO!C~*v_4Di`5!L4CuY_-uZijdEk9fIzWQY9?0TSv%Foh
zA-mC%Imf=7w3pn^l@JwpVrH7nNluA1s!6e6jYIBb@k&E&R^g=8Q%A?*<t>4ZbU$|*
zb=hbu_WlcXV@LBY?%!f7vk@ai0J$psq(+O4ME^q&n9!~g#A}Q4oaw96%-&vz5@}a)
z{%pZ=tN$a7qJgyl^R$W_Prge>0hKI&bb^GSbirv2dLDhpZfh-Z(}b!$aRW`S0PL9v
zCQ)4Bau;)Hi@E=yj>>C8PamPLMSW_8S;AW#mlq>M7o$^uCMyQHd;++P5D4(`Ohmfy
zZENpVyD3hC+|{w5=GR4USe6+kS*BJ5OfHocN2|3Z&M<lSV^j&q^(BHNezCY=Is|5h
z<gbqKdtXq>sz{`%s~uoCoiwnx@q=L$dyLNc+go;uC%04w)f|nEg_K2wU#_5|He%PC
z>SH#d(?VrD8ul_v;Wi&#t-lu&F3GGH`*w(^5RLM$UAoR(svbMuUvC(ja$!tUxqaR9
zK;wxD`)_MJqQsMN_#Sz^ktS~yR#l0i@?3{{+)_>LA$L>5v-I<3(YU=+#uAM2O~cqz
zka7!r^xH#{<jf>PXKOR>;23_rI!j5KxuD8=Lfg-)pI`d<{rU`xoJB=H`>6&?Q7zQN
zE3B;QVH|pAvlKx0KuRiK=R@a|**WHJs#I){LYcZR+W*2*2_wcPH^tes%<yum;upRA
z`2r>5NfE=OWT&ny!Ii<}#xOZNOYXWq<vIq;$wmSt+>6BH;5Ts+*a1~+PwOtQipi4@
z2<sdr32Q(NQ#~EI!i)UOBMX$8$Ay7L=0kTlzYTu*zrn({Bb+2@9!yD-bAshi$Tm5w
zTfc8YShHg7khouUu#YId=en=;@(l@rXt}QED2xk61_F!S1;5!&a<V=E!z4@1)Xx}#
z4ks7u*fr>{8zi|Zd@+g9l`VBDE3DD!Je%@%XYN9#gZi(}nHMsgerELN?Soi4!~p$C
z1nrPBF0^a&9%>|pk~De5V`ep?n?$gLXjC)q<^Tv3wgYq-0#rtIUpq^<`AtvwR|Wk>
z$qAa*C82fDv%Hw^jJP>Jm@9w|zPE}Yae_xH$n6iPBvqXtA?!x@5Ik?T%w^FYsQGO6
z&Kh-CZxt2WW442#<^H0`OsBz6%W6L6-00;G0abHQKm<?@)=0%+QfEp>%AOS{r8=r#
z&RiU+Om_a!{DyQ5(2jz!clD8R(~##nVm~<#<~txiy%H>s#&^C2>?b{}dod;(csCFR
zj-~;Rh^hz<M&>*UK&*Dsp}*xE0r3GyZSfN+|F|P)2(`9*d<OzfX#@|*fWA~BH|Hko
z^hE;{;I|$Gp%lHeu)k*uzwy{3pC<4^`gEI!vwyhzwQZMOZE^y9Cb%6-CyNM2o^Wka
zql@rOV6aAE3}3Z#bggBSkG%79urK-|I$4kvs?A`rGV;8g>CtB<bxpCax<Q(1p7!q(
zRKF57s9)UwzJ8y*OWMZ;m$_2kYOdMdD9>8z?pU#;$0*R{!JNiVnQnV&IK|<FX)0Tb
z^W{1j<D>??F7n7z0D4A020#C~u6+N&ekbl#Rz)ucd@mQzYihWaSKoN+y!NZeAFYuL
z{=>(sE}Yk(7mM#^^E<`r*^_axa6F)PTie=4>&xQ{N!CekEyPpn&~iVgdsaz(i-Yi)
z8(`!$L(kyTQHRPTIl{&#Ya{)b-2`ZGuABo++pc^~c^Q3%L|5))xfCz!@=9LvT#QY)
zGV<hjSN}s}oMZ*R=au|xSSP@x%^7?3F{c)@4wQg)Ga+yP6S#0#3?-_A8#{GB+OdD9
z{f5Krm7^P7N!OTvMpTR=SL!Y``bX~mPH~BynKXg^)P*G6%6W?9R%L|_!B*l%n|mxP
zmMM_66qNl^H}T<5HC-zbYQ>&GAv>q5=~{WidH7;<`jOfJk!}^$eVi^FRd<5kR%d*y
z$#B0+?GqXdM*k;S#7riF8a?v@dt6rk3Hn6yxj+MAdTW+OyIHXGz&0i-V=}%LW*Mq+
zv3gJw;7+IMSoA$8L2szAHhOpVTUm#sPE`l}VoJlYZNt9`ZZ%tw-=pVy(cqaJ@}U_K
zJB&kXtImJwBvh<pV08;Wa)d2n=lshr37`V91K<dPAac<83R9~x{1UFu2O*g?cum?B
zq602H&HuQK%>D^ok4LO?M8FwONop~_Q#@`1f|of5R=|a6&;zuci@*Bo9sB&JQ5EdQ
zTZtuQZ+zrER76K04=QB<BzWrpRsy}c)kpQo&+O4Vf<oJ%jpe5JSH#*bD@XwSq6I*&
z4*;Qo>Bs^C#EkP1BoOpe8W92JWk74!%n3%7B)Urkad-Mq9tqa@<*2ov`k>Hd^9Br@
z%~4?}3ILGd?~t_M9kF5;m_e&q@+-_!5D4AeK62pu_aoot$e}4}Tp#j2lp&#rwI#0J
zenn6KUO#R_qP$!#sL*`=o22XY_QlJEGka8>-bbo=b~bj1=_Dp?SjT0z2wavF==~vo
z#m^yS&g^pcir!CVsqb3en$;7Ikwxql<wjMHT#UW!mqbupuD4b$+Db9TIIm^MI12dU
zOx}@-!3C;fXUc;wk)u?P)P0)d-}OL8v&h@X5GiBriOMJdDYoKYSw!eo2Qa;>BgEHm
zmY1zKlF!*JswbwjWg~m`cu6<Sh>UP7u!Wa{%RxJ$RqSaP<VL~(nt8B<1_TnTRaFrP
z9>gPCbJzpbu!9x8E{K!MW(_}jUpk20ww_kI_iAiv3JfT3uF88S{DZGeVc%`>(!A%V
z*I;d9Z)VZ#rZbZH>M2PduXQ$#_l=!MgO4ZmEu+|lZmp~_O?BGk-zi?a%|y2wi`?$I
zyh`*8dobhS6BJKhJ87aGr}{WWJ|!bTW~m7yEKo`Y?1so__&mBl6)fWV!`S^v;8)W#
zlWqd?kSOS!mQ#k_YQQYOYr8GP-Uz&T^En8!YYFA$k`NrOwRPxOmn$Iux@G<#-9Sxg
zHOWco(d*rP*SU~;8F!1zX|84KzP94kscZB1y&{QUid3P;$KB{?<|cw$<zHiA9ShE(
zi>Rj68&fVOT|=LIEoL-1uE7;uY4QC{`m4?3jwhQ)lHsbH_T1**KF(TeF|>5lP`&N*
z_@3dyZffi?>=4%0o5p9mIuvG3b{thvxsvK+T*mgTbZ*&BVK1Igno?%xv6|)XRx%R0
z89gK*Cay4y(O+Y-H;gW)@(TSBQ(W3TcI`St%1q7%;ve2x0u+gv6(X}E;d^A@z+?>v
zUrxW4*%0g7eIx;+5-jsNHQYFy=eH1qluZulTKoyX$$E$-V0ah6oX8o*^?{KRS<sH%
z=Hw*_wfLil<af~8!CyhQwyIgS<2m1}C}@&`g;qaMUdVy!Y_eQ-{ovEMLoch~sp%8I
z_(jc}ZswYX+&k)~P7ha8j-(@NUAlEewT4H2b{ig^d?{ADY32NlUu^f%tz-=me7Xzo
z9K(MV<H-{BTc_CZ!CdB~q7xztC3eMwCN})chTptH9eK3AN56H}3fSCI@9GO6Pe)8R
z<#?X-A-s$ik@Z-kGYyQ6=V48`yTI;kl&7az#GUfy=g;i4f5``UBHaFR^z@&^bbmYs
zf(twI0jwdhbg}4NQvj^wkRMJzkv*iRJcvZErJxUv;^wd<RxE7rl$Zc_O|N_ZPEr1f
za1v{G4!i<7vKLrjuUb;1{pFS1lKlP%;vMdXD#eAIr)WkBihIc**3B;Zc&t1~+oGX2
zt^gIIIaJN<r(Zo>ZGFFEO=sAr8#0{jzCE~M8xYW_Tzuh#VBPx=jcD@;W*@u!yEt`*
z%2vC#-=oyJWTco}6bh4*3Jj$^b(dcmRJv=wm~~Qu9&Z~N_!bZ6=kFN!@?8`h7|x#@
zg<W&b1bX9jSz9b0WtFU*9kaP1u%><N)1&ne?fQJ3YST@=iP$%nIw`iEwfi9mRZY|&
zxTD3w$B^J@37|bTK-RR6{B-`2ACOQz%ks$49^A&y%U?C^pL$Sy0RRxCtuAo3?!zCk
z+s?>S2tpQul#Co?Y6|WyF)Y$jl#uPnQZUjzJUJF>Y~*S;@FA^M6Pb%RRWh@9xm<H7
z%_&^ZD`m`P<ja@ZfoASTm*FbtF`$HJ%^(lhTtWRk$1fIg;>Ca5BzhS359ie&Da-GI
z$&+UY0Vgwb1z4AiL0FJl63izF>7TEEr|1bqx3RSYX<!^3Xj_Qbe*yDg@s21~IGhtr
zI*A24Xd(Xa8OEdWqu95SWRCO}&>dJGc<dmbb*6{>6umsud=`yYLhlPEKrh6waOnHK
zhKS!O<S0qkVv%H855O=s0#0ZGehp^v17EkT1IUaE$Ohlc`{M@8Pga=w)!|W)l?F0-
zfT!XEp1&?HVSV~LD;%edr-T`SJxY4;J4IATH!zCYv4_Rzt-F7`8z%I<{n<ux4cf7+
z%#UGw0v<rn7KPkq6HC-3vT7#D9|?I&6JH*F<O%;d9#OJ@%YHa8x6gxnk>JFRn6W|l
z^y?xL17wA#3x5>MNB&Kw`-2xR)k;)RoZ&1EI2g3}+BD*xN-`V&ee=|ud$yUSA?y0v
zTgX%>Ics3{BH^d!^*Eg?V)q#@r8t>*`tq+6dy^OEz2YmMISFfh$sc!IzC)Kf^u@+E
z&7_QK_|p4KCI`ceq|kbTXKDG@l<L?4KW*dnH-H2aY!mcL<@<l$M?eD3&rR%n2i6&V
z$j<{jj6rYQXw9EE0gn|Cq_|4#A=?agM=-JdCHiwCFCea%W5`msVOG^YSmn))@nb@}
zccEFPC({Is?_~bwF@OH!v9VA5`DIrTe<0@>0y#g}JsL|z@+T}#8z<b%>9FA%lDSyp
zRL*fAuB!dLMf_`;)0L`;3-|7|)tyaSzF-L-f|k0m=u`LKa|y^>F~{PY9x>9YONR=W
z(MNWR^yleGms~V@mZjMt;+@IIvLUQ4Ttjksg3+dKQxMbjFZH>!-^=mAIZjK9sd(<S
zVg7_&f$+r^30?Ke$u&uCpCJtcNdNFIw72U&K$2%|*AJ6MIhqsBL~)Is%$fOMe`BP^
znqq~eQu8;>S&N-4J!*FMH|gtU9I;Bacdh5+>?n^3N=85SYvVlvw{lE}`n=jW?i5Ar
z)*+a_#+&<6N7)bN6H=7YBQV?kZbR+l5(nmySLfm_LZdvMI(YP&=eP|9QwEQ`L%HY^
z)TV!k&IHiWSgf#Q9J_;ZnOSq15{%KVuKB;(yY_G>`);j+oEsgGL#7iWLWOsbOe#c0
z4mC-c${Qi3LZ*y)N(V_O$5Kd=&_s^OVRC+v^O=}aavIE3^f1p%``5ewdB3;z{@(Aq
zzP+#gUAsSA7r$#f&&7J4wbs4Xz3vs!WFD6l#Fnw{R<13&y|Fmn_TCb;>f`Lye#yBJ
zSDuXG@_^GQRd{Y1o`Rrwg6xQ6{#q9e_V23h$Q-PH7WSavRiP}wGoUAWsD;$l9Jb;1
z;>F7%6OtSf4%IqVALkksqGcRbROZ%L8rLz;k-)&}t0JwpK~LrLxAcz3<d96V`6o%w
zx9{8MdNa!<zALAlI+;jFiF3bvLsfsvs)vOIhfP={k71HAe8xbN#0gt{Ekk<0(TUA>
zvqG!cxIEXLnJGqg*ENOR`6o0q;D?a3wr~rjo1TvjDRpAem8%$3Ol9)Y4apTlXr4tj
zb5ie6acN=ag+|Cfb;xO4Yc(y#$(!m!WhUU4zmrfh<n>V+yYaN8<ycA^dLhSu5_00A
zgF`MOmW&eIGS4sl!y_Tbd2aCmI%%l`<Hj*xupLfP1K8qgw%c+q4p^z}rL}^=F>~B?
zp&sC|znFiIzk^t`o2WfCe+<}-Ix%EDx%Wi<_@^IszgB=eMeV#9n>lOpAG`{wpo#gI
zXz@x|JvDs>y~H)}{U@;QzK~se#cmv11KV`Z4t|J(-~ASU^7CMb<KENK%D==#U$DG@
zzk{zhnM?LNo7AW%D6Y|KdMTNI>d;e%NNTm=!}3IrK&QYX%c~t5wCt+t?lUtD)NG<7
z*1bFYmKBGQxqo~(boL&BL%yZxk8<lWbTd2{<xtZc6O`aMk*WVl$=|CSe?;<tzovy_
z$Lv5hJp1aa$cq)wXkqqfqfn*`6p;3Qn~ii|M1{teF<uLOef}QVf&C{cLk2fYXl(@W
zmNrp<)%(xN^r$f5&soR6;^%)^=7O{WSAP{?5=WdnSY#F#=9Lal(LpR+NiiTr?&u_&
zG0=U}O!Y9`TNDgjV$<=s)MF;T<B_Rt2(r;#Ew*kpJ9Nf9qPbEkFf>XlJ*?gb1Tu*%
z{#gJnxOT+eFU*ihuQX&E4L<13Q$u!#GJ^J2%aW2lSrSnbJ^d<SItAWoqpgDvZsh7Q
z{YFiwLOXii+vBKG(0yBiI}O2`6ehKpgxfSZ$_imUj%y9|r_R1?r-+fV?72`ibZeQ5
zN3s+m<NM-tv}o5Eq3S!&c?((qo8;=4WazI&I-{LLdUMN&ed^=_ev3JU2vz8TVVS2m
zF&g73Bu=*Fi7`iRkO|zDI<$-l;Uf178(l%WdBEA3)Wb{<KjT17aCusotB*evps^uP
ziYNCx`X(lV)zD6ISvJ4xZjZ^O)&+mWmVe><fA9U2aHCLC4O9UQ_#lVrTg8hPK1eEW
z`X`v~#h?_sTNmV8J=*{u-I$Jlgil7kk@=;nz*0^CZGb#bQP-%PFzh((9XEMaW*49b
z?GUc7fl67p2?0@Hw61_N>FpkH_KMTwWcvMsp9&H*`re$8J!{QiTy0s5AE~RP-=-Bt
z*3d<q$@{q3Hz4<wt;DIBQI3AN4RrIu>vuV$2?ewF+8T{L&yTgfJF2XH`n}F^+bvF=
zB94^H<Ft=bowSL{>8~PI@3R{s!Sr4LPobL7*I`i_GJ6efVN7ZrE9aHvkqDLZPp^AD
z>a85w*iB7~Xj5-f?_7qd+KZhh-Kk&CiQ4R_``fwIN4qZ$5rP|&P8qhWZ+>|`CEoQc
zNuKAp<-WFx!I7>lcVpbeH|;;Ik)~gk8k144g|>`xXk^E+w;ZNFt32@b<6Gmb@XC`f
zJq}QJZOL$1ImSGtxY4RD9iz_b55Da#Kf|ZZo`(UibG2~Qn9#Vml~8r$@%SZL$V?M@
za^ciak8`WefNf{Zb%w~XUV}~_Z}-|`hQ#~(1Ix(U_$xi<%kQQmhhg-yaNPrWunfD9
z>lVriLfaGpCFLdYzFxDVr#h+HNqZt_QVPa3Z=x(;72m7ZQw)&j-ztw@U%Pj6eQxO~
z>j<hREUo`i?wFiei=2{OdRY9++v!U9>Pl*gMDxQ#F(ZcO>*o}kl~$PyY_75oPP0hT
z1PlDwc3pOrU6cXwSSvLnf{cB1DO>5*Y{sf4D~*2v-K@dH|8MDL$o~%A>>G6rRi_K<
zLP+W&Hd+u%>mB(j;*4d;P^!&@kJ0nPXyN68Z!5P%!B@<2GC)wdMSLJmHsOXW$nL#~
z>jV${(Nq9R<S&~#3Nk42h<-fNc2b7tMhp>3KVaR4@`yE%WED@IC_UN^_oHOY8QIH+
z>K>V~Lh`&lWmXe02@`x{t<HSC-#YZoo4r1M`F(^w#)$HkHY*D!gJ~O7wcJ!P*MT<B
zb4ZYwk6ML@w$faa2gGJWy2>|!{HUxjk@Z#NGFr?(`=+X7s{wk&7=T{apX<|Py^eV9
ziMteo*Iqr-lvJCr-So8k!4vCxJ`fP)mwX$<2HwTT3R`eoTY4BhXJj&+n_WAvg_xIi
zQM5?F0|$hb>T)3)Z2wdqD@;ST)Gn*PKPYN%mQ#B(%5>#&y4)4LVk;5h^Q;;n4heuA
z_}YjPe4R&UD}?%Gi;}Uhe+q#VUp@Z;NUz_?wAfkYy_A*g+2tQN=lyxoxO@8-Y=8J*
zS*i8Mh6Qs949sL6FPSwmNe6>@^+}rqcPZ<T%`l#euIg9MV-WitT%zqvrQsU?jFzXn
z&ZWbYZIP(nv1hH+nQHC>nLvE(<D6o?<1f!kEI+O1lPDl&=3^#7aZyyJ6T$FLQr^&u
zzTBcQlAzq|i0ou$WCc2yzJPp+MMP2BT}-*NDQbXotH2_JdQ<fq_tM3W)cd?nx{`AE
z=y+VfW-Ar@2G^vqCnAFFplwbz6(?I?Wzr_8d<e1N*3?x!$c8ltb&>Mqvv2@I@sXEu
znHV#%#PcPL$Cu@h+9q|#JAFnvz2odgtWL)D?2`_#)1!EU-jC^05Z3pAQX^WV91HNJ
z0*>)CIX|QVb&ViHI}Jscio(9S6}V-6V>EdL&+<Yq3(DMx1h9kb>kgDfR;aGL6}W$W
zl~BH|Iq`$@hH16Rg=w@@RBx70QVKQcf00eN(eKW2;vW;f{Y6!jy9t}%lEU9XR_YWg
zk?;o#wA5q0Ks{3W=wPOq?=vUMl|(`ELdtlDmx$aB+)}6_A1y)AAgc&&QLwNttFDT!
zNXGWepav{wul8{CiOQsagWT@)z?AVL!(krtWz;yTmzT-B_jv83*Tv-qrQ^>}-o4x;
zLUD&G3{0@V5*8qjc?K(kJ>)#zh)`0FXUT}KLiOU==m^S!@nm_(z2t%IQehjUia?J&
z18B!-V3w>v%G+WaQHJEOKe)+$yK!?nzuJDt)h@}cFzS$C2e_gXr5)Hh{`NG^hu%w@
zeGI2%ApX1&&T77S3%6tkT+iCDi`Ak?k~*4KTn5%at6|S+T4s38V)mDPA`Vho+uqf9
zYc*&`Spe63m14S_YzH6A01MWgS%fO^YocUe7t!T#F^eXS4MU7F;UKatGi*#EkGCYa
zKarZ#<rnzKtf=$M?e5&*U#$BwK~iOrd<|Mzt_p0slg4;us>!2+E5W!b9H32jMv6`l
zP%~demXmxBw&g=gWAgKuo84>&Zh0!?CHl1Zgw)X;xq}|<?{+EgSIP@!$f8Dv{%ZB=
zn@9V%-c|Sm_#X6tU?S`;7b}9J`CG_j?)EOuYk%5Ju(V^S*}2!%=~U6-&&rL|G37hY
zt%{_AEv0Qery53-oRW^&`?RJ<RNDA7^>40rh)Ikc$h9@k8a?mI)V!zSZJ5<S+P81r
zi`ZdJy<7N8T16$TLQ%?}iUFS`=hJ5wfQRe=*;Y_cz9>0tXqDjcbD@&QFG*W_5VXP#
zA7ne#vpMQ?n@3NyOIlN0(TRnYe)B2omMwYz>jigjB&Zk=O+S)BMJctR9mfsh>3$V4
zkQ1cgYiv*rnI=Wb_lyshO5RXjW;+gNoIjYhs?sZA)kX91#D~9jKNyE}9JmvqzT(d>
zD2S(r)D+`Wab6*j2F6hz!iHl{*L!Fu*jlC&>-OF}^g&|sU5*tqN<nkS@!Zt)avNlo
zrO)nJqgq>zX~mrwTvYk`cq>_tzk||YjJnbOVg7^eQV$m#w<+naNM|;tysRjARjD}U
zhQySPx|_s#wS^Yz4uB;Qpu@~VVd_xxCe-zyd`JV0s84p5$gX-`l`v9k52$wOR@0D9
z{M`tiZk0(hRSD+UXCJw)nZMD|&sXz&xXSKPn=<t=`w#=;=oD**Yjdd>Yh%@j6_-g9
zi)_+1X`@fs<d1b`pggWT1X5D&O1=fw(Zh*`-I=Kx{Z`gTCeoRamGo;v60i|7l?g2x
z%kAs44Kz*97#bREdN!dnl<lW5d8xc0^O)Ydai#D!bG@Vc-myJ3*GO(x2XYlGRoe*k
z=Zi-yh07=)A^z|fbeW^sAp+GycR`Vn5DLvbdogdSdv=Lx_@?dBWQ_x<HY=aCrCqJ5
z1S`YXt11%>EhSS|?bM3fa?48NNnD*8K^m?tv+AZ^k3CZ0HeyfRXQS$mG0Gn{YCasY
z(W)4$KH&Yd4;}&O%zrzXwh0Ym!3v#)RQ|7{?`6%zDqPgo1aCG&bE7|sGkQ4F$O`va
zrE)<qP}`1Y`ed;Awl`l#h9W@_JCN#DXOX-+zX*Hjm+;g&R~c&4@aO4V?X_-E?high
zxh>dXm)wLn@)q4NOuo@Awy};+c0A|kscCaVedz&*Yw0?kF>4n;dNh6USmE=?SKKdG
z^*C3zkxJ_%!K#IjEY#`D*4Q)+13>)9iyerKjm-}@EkFVC)?BIJ&0l5B|0AD6&VyI$
z3()`BY`O(N6=y*NG?PaW>EY}~kFN#L-wxDEr3ewgqq=~Ktc6%Dj;{s1Y!Ma`-zW)g
zUKQ-d%{1Ty3r;Tt6IYj4cm^L9kzhQOzB_8`u-AO0h`s2+<}Rt|=*vs2#%;DGA3372
zU}5Hn=ap|Kwi$H}(|hC`qcR^QS&1yP%~<0LcRc%trWyXd9qso^VF{ncnl2r?m|hz>
zx1J`C<0Ozk)@v7p2Pz=p|Fv8e#SE$_r+qxplRWEe0s)rmv{cWOIR!L>e{dXx0;y@e
z6G&@&8<)+c)-s_vOfJM<08;2DVBnmf6Ik&V(D^@#=KuJ4F?RglZ59c=a15p`tYOI^
zGU$C6HkOmfii}j11u5m}>+lsm2EIeq?p%^VWNp;fM$IO|+U!Z^gIAnw`5R^rxyZK8
zc%OGldF{tnyZ*+$0|R<?=-lkdbiS^zKS{oae2lMy5V-VOoHWVSp|I7&>;>%Puh8Kq
zaC6o#-ETCNea3rz+^F-`{Tt5eKi%NB^^DG=%R!)^{!i0M{*M$hT<-G8!aiE=GzZk+
zYA#_XSMc)Q(8UXMLuoQXwK2*<uub>bYdhYdRBvWjmIuc#<gRo8)~+SvaBZtrsNOk6
zN&E&f2ttch1ckIyx_}szQ^&c&{{=CGM;N+_WIs57Z%FbXs*ty~7Mv>QSzb?Rb5Yi|
z-|tqduU^0D#f+5J#RRtl>aW6=>^*rINBX0m`hK?@ZDg(|txJ2aO>VdTgaBh-6ften
zwdqCv1mDTxWN~g;@LsR1iy~`c5+q!(B3BC{4bl`d{XHB#3T&RJw+}w6w@UlKG2c_%
zk+GOFB4LirJs*aLE+V5)#ZsCFy|0I<z;X6x+TOw`m#+f*cD2YZyuzoiQDKOC%4y?_
zju#vg!?>cyb#|q}jQ)2fD>mt!>029G`BL*7UlFu&2Kfq5TL)PTun-cNIH6NdBWM7c
zLoi0ItNP1UviSX9UnN)#p5TF*fK_Ypv3<Sf(k^OyQf|wyT^}yD>}+Pr4mlKJv`Z*a
z^;KlI<^<@8VX6s2kBO-~vb(6;ToP6<E0lG_U`uZ(3uB!mrX91f0qB=(jWe>g@^a%0
zmbOGU=Df3uW1M{@>9^H5r?Tz(s?H?c)rvNo5821KUh}|qskhDG5o`eG7!cvw&jsC<
zr2*G~2Y0vp(IG(-{2?*lNIRc*zMyegWOER!lOo~sA;^jl;mw0RLKWMaG^C>^k@HGf
zc$Eaah4@yQ?tBDL__;qYX_zg%vK5VNssp)+;{U*H+rGc+y+9kSZ`yPrK*bFNE~GZ-
z;1A5w_Jgw?pr$>Hf-xw8Y*sd6n7j;{OHUQ5q=4rdAhE}CAaw{?!2&s={^vlQCEy-I
zpo3Z|o!ux*Eu{~BQ0)NE{pbo18!O)eXS*f&AXF7VsiA-Nqb~qaa)f02RV2>u2WS2t
zvwvXmQE)d6sKO8$y?zX0R>NVS&$?Z;@d|AeYzpLF)%pksJJZ-N7en${wE4xqLEJoG
z%D3)a1nwaHWq@Ec_6>v>4XyYpGMtG1{)n6f><+TXV37Q_NxlX~4&^BT;WZ%k*MR2Q
zpy$`(3Gg{nQ0)0+YnjU+&dabOku?nnZG0P0iaPPrus;p^x8CEgI*Olf?9Vs$ZzJSS
zSNziz|BSIeW9<La<OwMSaJ&yELUW__cf28p(Y%uT^4JO$bQQ!4_N98zq$q1h?v+W~
zxMe~bD_I1tJ6M><oyzg17VO&I0OP9Tm(}>LtJ;{??qAZ_m!P#xE7VmO3^Ag-_@dy-
zMzj;U!Gt`Yc)ks#g<WIpa-&=mPS2|0VJfXm%XrutFZp!VgT{EJ{2OMjdb?8k+$;}&
zp44nNFkGA*2*K;zkrkKBwJ>i2L+s4}7LXOL&5aUYb7x3>{hG^`Dzf759cavYklubv
z8L_{ojL;`rD?3JD3Hw!YTwAWNrBNr5Zb0whbS30k>9eNjTVGBZX40qb%S=rR2O@!N
z8B#G#@hUGmWDUC3{{uNQ?$jIlXXP3+Y!*8Qbp|}(msa(kBM27$ii4dZ0h-Yp&xU=b
zdrQ8lI^u*X(-MFq*c|!&Q8~a7-)H3L?;%6qS*&0QSokM<ib9xK{XsardG;axn=Us>
zDDMi8*6sN$p>oNr&hP4vC~(njl@O8#)E~b)%Z`P9kdFTdEX*DS%zd*`J5W1SB*Y6W
zFVafhZcmp3?N9YxpyLQcFNCh9nLw>$N)SdH9s8!>!EELxe-+sTaK`tC6T?6`^G^^D
z#OWU*hVAi-pjP5ah`9tm42n`i#Hmjxj~eK+#2s)CHyoh4w`9_oCA5>%K#}t@SQq45
ziUE?G#<(Ax%>h}>AH@Rb2N?9HS$~@K=iB=275@3Q{`)v#oXI!-93UE%OCcVD9310*
zIul)kTYy+8q3XglyeunRFzsY>K@5=`z~%OVst>0zD>7G*&iOQ_dV!8@h;{=K$4@<x
zPOccTTx(Voc4ZT!keytp!Xk%<zv^Fxt9DVPz2;?y&lo=U57+##t;zIr@3^s`DQ4Qs
zf2A*=I`0YIeMobU+&um;Z;5N{$(;M@i#C$xS{rhDU)yKhvrn<0TCrZ9uO5@$?%6sV
zvMu~w^aY#R`=(->t5|wR-A?A*rQYkQ>U8A#Yj&lJGr6r*6PleF)*s*7bYJLkxdLV^
z->UvjGlzq|iij<ODvX}f7^!HkEjlzww$(4c1n%Kp=pO;hm<p{Yp)OOz6(pao;qKDS
z5x?G?Gu7d?cIIE($zSqh{>+r*8#B=20KNYM;2-5H{0;ZAIU8t%*>SsS!+Gr4CuM;*
zIR##rg09ky+{mgP3wo83{>!)I39m%RG18tV^I2QvjP{fE^>-vr#wyIeIR(^H4>Y{V
z(h_NCs1iM_5-h$!)@-qg(ygKXp=NQaw>Gn&@cPbOMB>V`X3Lj`uC&vvQ@Z_7Q)AI~
OjfI|n7>vPv?fVaDE8Nro

literal 0
HcmV?d00001


From 463e4877a00c416a60b703b52d13136507f490a0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 16:03:32 +0100
Subject: [PATCH 169/264] [move_arm_action] Included whole body moveit group

This is an optional MoveIt! group that may be required for moving a
robot to arbitrary 3D poses.

The group is only initialised and used if a name for the group is passed;
otherwise, only the arm group will be used during execution.
---
 .../mdr_move_arm_action/README.md             |  4 +-
 .../ros/launch/move_arm.launch                |  2 +
 .../ros/scripts/move_arm_action               |  3 +-
 .../src/mdr_move_arm_action/action_states.py  | 46 +++++++++++++++++--
 4 files changed, 48 insertions(+), 7 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/README.md
index 516009c75..d1cb64102 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/README.md
@@ -1,8 +1,8 @@
 # mdr_move_arm_action
 
-An action for moving the arm of a robot.
+An action for moving the arm (or the whole body) of a robot.
 
-The action uses MoveIt! for moving the arm to named targets and specified joint values; motion to arbitrary 3D poses can be performed either using a dynamic motion primitives or using MoveIt!.
+The action uses MoveIt! for moving the arm to named targets and specified joint values; motion to arbitrary 3D poses can be performed either using a dynamic motion primitives or using MoveIt! (motion using MoveIt! may require whole body motion planning depending on the robot).
 
 ## Action definition
 
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
index ba5cc4d98..26ec4a6b9 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="arm_name" default="arm" />
+    <arg name="whole_body_name" default="whole_body" />
     <arg name="palm_link_name" default="/palm_link" />
     <arg name="cartesian_velocity_topic" default="/arm_1/arm_controller/cartesian_velocity_command" />
     <arg name="base_vel_topic" default="/cmd_vel" />
@@ -11,6 +12,7 @@
 
     <node pkg="mdr_move_arm_action" type="move_arm_action" name="move_arm_server" output="screen">
         <param name="arm_name" value="$(arg arm_name)" />
+        <param name="whole_body_name" value="$(arg whole_body_name)" />
         <param name="palm_link_name" value="$(arg palm_link_name)" />
         <param name="cartesian_velocity_topic" value="$(arg cartesian_velocity_topic)" />
         <param name="base_vel_topic" value="$(arg base_vel_topic)" />
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
index d5783216b..a215a2a67 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
@@ -17,9 +17,10 @@ class MoveArmServer(object):
     '''
     def __init__(self):
         arm_name = rospy.get_param('~arm_name', 'arm')
+        whole_body_name = rospy.get_param('~whole_body_name', 'whole_body')
 
         rospy.loginfo('[move_arm] Initialising state machine')
-        self.action_sm = MoveArmSM(arm_name=arm_name)
+        self.action_sm = MoveArmSM(arm_name=arm_name, whole_body_name=whole_body_name)
         rospy.loginfo('[move_arm] State machine initialised')
 
         self.action_server = actionlib.SimpleActionServer('move_arm_server',
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
index 5f8151687..6f5c2bc2c 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
@@ -11,22 +11,52 @@
 from mdr_move_arm_action.dmp import DMPExecutor
 
 class MoveArmSM(ActionSMBase):
-    def __init__(self, timeout=120.0, arm_name='arm', max_recovery_attempts=1):
+    def __init__(self, timeout=120.0, arm_name='arm',
+                 whole_body_name='', max_recovery_attempts=1):
         super(MoveArmSM, self).__init__('MoveArm', [], max_recovery_attempts)
         self.timeout = timeout
         self.arm_name = arm_name
+        self.whole_body_name = whole_body_name
         self.arm = None
+        self.whole_body = None
+        self.end_effector = None
+        self.planning_scene = None
 
     def init(self):
         try:
+            rospy.loginfo('[move_arm] Initialising group %s', self.arm_name)
             self.arm = moveit_commander.MoveGroupCommander(self.arm_name)
+            rospy.loginfo('[move_arm] Group %s initialised', self.arm_name)
         except:
             rospy.logerr('[move_arm] %s could not be initialised', self.arm_name)
             return FTSMTransitions.INIT_FAILED
+
+        if self.whole_body_name:
+            try:
+                rospy.loginfo('[move_arm] Initialising group %s', self.whole_body_name)
+                self.whole_body = moveit_commander.MoveGroupCommander(self.whole_body_name)
+                self.whole_body.allow_replanning(True)
+                self.whole_body.set_planning_time(5)
+                self.end_effector = self.whole_body.get_end_effector_link()
+                rospy.loginfo('[move_arm] Group %s initialised', self.whole_body_name)
+            except:
+                rospy.logerr('[move_arm] %s could not be initialised', self.whole_body_name)
+                return FTSMTransitions.INIT_FAILED
+        else:
+            rospy.loginfo('[move_arm] whole_body_name not specified; not initialising whole body group')
+
+        self.planning_scene = moveit_commander.PlanningSceneInterface()
+
         return FTSMTransitions.INITIALISED
 
     def running(self):
         self.arm.clear_pose_targets()
+
+        if self.whole_body:
+            self.whole_body.clear_pose_targets()
+            self.planning_scene.remove_attached_object(self.end_effector)
+        rospy.sleep(1)
+
         success = False
         if self.goal.goal_type == MoveArmGoal.NAMED_TARGET:
             self.arm.set_named_target(self.goal.named_target)
@@ -41,6 +71,7 @@ def running(self):
             # we use a dynamic motion primitive for moving the arm if one is specified;
             # otherwise, we just use moveit for planning a trajectory and moving the arm
             if dmp_name:
+                rospy.loginfo('[move_arm] Using a DMP for arm motion')
                 dmp_traj_executor = DMPExecutor(dmp_name, tau)
                 dmp_execution_thread = Thread(target=dmp_traj_executor.move_to, args=(goal,))
                 dmp_execution_thread.start()
@@ -56,9 +87,16 @@ def running(self):
                     self.result = self.set_result(False)
                     return FTSMTransitions.DONE
             else:
-                self.arm.set_pose_reference_frame(goal.header.frame_id)
-                self.arm.set_pose_target(goal.pose)
-                success = self.arm.go(wait=True)
+                if self.whole_body:
+                    rospy.loginfo('[move_arm] Planning whole body motion...')
+                    self.whole_body.set_pose_reference_frame(goal.header.frame_id)
+                    self.whole_body.set_pose_target(goal.pose)
+                    success = self.whole_body.go(wait=True)
+                else:
+                    rospy.loginfo('[move_arm] Planning arm motion...')
+                    self.arm.set_pose_reference_frame(goal.header.frame_id)
+                    self.arm.set_pose_target(goal.pose)
+                    success = self.arm.go(wait=True)
         elif self.goal.goal_type == MoveArmGoal.JOINT_VALUES:
             joint_values = self.goal.joint_values
             self.arm.set_joint_value_target(joint_values)

From 8c5140b090707a475deda8b3bdc71525c28ff0a9 Mon Sep 17 00:00:00 2001
From: Ahmed Abdelrahman <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 4 Dec 2020 16:50:14 +0100
Subject: [PATCH 170/264] [mdr_manipulation_msgs] Add UpdatePlanningScene
 service

---
 mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt             | 4 ++++
 mdr_msgs/mdr_manipulation_msgs/package.xml                | 2 ++
 .../mdr_manipulation_msgs/srv/UpdatePlanningScene.srv     | 8 ++++++++
 3 files changed, 14 insertions(+)
 create mode 100644 mdr_msgs/mdr_manipulation_msgs/srv/UpdatePlanningScene.srv

diff --git a/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt b/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
index f9d129e0e..6f0529b65 100644
--- a/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
+++ b/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
@@ -4,19 +4,23 @@ project(mdr_manipulation_msgs)
 find_package(catkin REQUIRED COMPONENTS
   message_generation
   geometry_msgs
+  mas_perception_msgs
 )
 
 add_service_files(FILES
   GetBottleState.srv
   Grasp.srv
+  UpdatePlanningScene.srv
 )
 
 generate_messages(DEPENDENCIES
   geometry_msgs
+  mas_perception_msgs
 )
 
 catkin_package(
   CATKIN_DEPENDS
     geometry_msgs
     message_runtime
+    mas_perception_msgs
 )
diff --git a/mdr_msgs/mdr_manipulation_msgs/package.xml b/mdr_msgs/mdr_manipulation_msgs/package.xml
index 8954777eb..85643e635 100644
--- a/mdr_msgs/mdr_manipulation_msgs/package.xml
+++ b/mdr_msgs/mdr_manipulation_msgs/package.xml
@@ -12,8 +12,10 @@
 
   <build_depend>message_generation</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>mas_perception_msgs</build_depend>
 
   <run_depend>message_runtime</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>mas_perception_msgs</run_depend>
 
 </package>
diff --git a/mdr_msgs/mdr_manipulation_msgs/srv/UpdatePlanningScene.srv b/mdr_msgs/mdr_manipulation_msgs/srv/UpdatePlanningScene.srv
new file mode 100644
index 000000000..3c0dbcfbc
--- /dev/null
+++ b/mdr_msgs/mdr_manipulation_msgs/srv/UpdatePlanningScene.srv
@@ -0,0 +1,8 @@
+string ADD=add
+string REMOVE=remove
+
+mas_perception_msgs/ObjectList objects
+string operation
+
+---
+bool success

From fd5121dd9f0f63f0c4f9efc8b377429779e2c5a5 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 17:27:56 +0100
Subject: [PATCH 171/264] [move_arm] Added a server for updating the planning
 scene

The service is of the newly added type mdr_manipulation_msgs/UpdatePlanningScene
---
 .../ros/launch/move_arm.launch                |  2 +
 .../ros/scripts/move_arm_action               |  6 ++-
 .../src/mdr_move_arm_action/action_states.py  | 46 +++++++++++++++++--
 3 files changed, 50 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
index 26ec4a6b9..c67ef8ef9 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/launch/move_arm.launch
@@ -2,6 +2,7 @@
 <launch>
     <arg name="arm_name" default="arm" />
     <arg name="whole_body_name" default="whole_body" />
+    <arg name="update_planning_scene_srv_name" default="/move_arm_action/update_planning_scene" />
     <arg name="palm_link_name" default="/palm_link" />
     <arg name="cartesian_velocity_topic" default="/arm_1/arm_controller/cartesian_velocity_command" />
     <arg name="base_vel_topic" default="/cmd_vel" />
@@ -13,6 +14,7 @@
     <node pkg="mdr_move_arm_action" type="move_arm_action" name="move_arm_server" output="screen">
         <param name="arm_name" value="$(arg arm_name)" />
         <param name="whole_body_name" value="$(arg whole_body_name)" />
+        <param name="update_planning_scene_srv_name" value="$(arg update_planning_scene_srv_name)" />
         <param name="palm_link_name" value="$(arg palm_link_name)" />
         <param name="cartesian_velocity_topic" value="$(arg cartesian_velocity_topic)" />
         <param name="base_vel_topic" value="$(arg base_vel_topic)" />
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
index a215a2a67..865e1bc0c 100755
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action
@@ -18,9 +18,13 @@ class MoveArmServer(object):
     def __init__(self):
         arm_name = rospy.get_param('~arm_name', 'arm')
         whole_body_name = rospy.get_param('~whole_body_name', 'whole_body')
+        update_planning_scene_srv_name = rospy.get_param('~update_planning_scene_srv_name',
+                                                         '/move_arm_action/update_planning_scene')
 
         rospy.loginfo('[move_arm] Initialising state machine')
-        self.action_sm = MoveArmSM(arm_name=arm_name, whole_body_name=whole_body_name)
+        self.action_sm = MoveArmSM(arm_name=arm_name,
+                                   whole_body_name=whole_body_name,
+                                   update_planning_scene_srv_name=update_planning_scene_srv_name)
         rospy.loginfo('[move_arm] State machine initialised')
 
         self.action_server = actionlib.SimpleActionServer('move_arm_server',
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
index 6f5c2bc2c..50acb86f5 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
@@ -1,26 +1,32 @@
 #!/usr/bin/python
 from threading import Thread
-import numpy as np
 
 import rospy
 import moveit_commander
 
+from mdr_manipulation_msgs.srv import UpdatePlanningScene, UpdatePlanningSceneRequest, UpdatePlanningSceneResponse
+
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_arm_action.msg import MoveArmGoal, MoveArmResult
 from mdr_move_arm_action.dmp import DMPExecutor
 
 class MoveArmSM(ActionSMBase):
-    def __init__(self, timeout=120.0, arm_name='arm',
-                 whole_body_name='', max_recovery_attempts=1):
+    def __init__(self, timeout=120.0,
+                 arm_name='arm',
+                 whole_body_name='',
+                 update_planning_scene_srv_name='/move_arm_action/update_planning_scene',
+                 max_recovery_attempts=1):
         super(MoveArmSM, self).__init__('MoveArm', [], max_recovery_attempts)
         self.timeout = timeout
         self.arm_name = arm_name
         self.whole_body_name = whole_body_name
+        self.update_planning_scene_srv_name = update_planning_scene_srv_name
         self.arm = None
         self.whole_body = None
         self.end_effector = None
         self.planning_scene = None
+        self.update_planning_scene_server = None
 
     def init(self):
         try:
@@ -45,8 +51,15 @@ def init(self):
         else:
             rospy.loginfo('[move_arm] whole_body_name not specified; not initialising whole body group')
 
+        rospy.loginfo('[move_arm] Creating MoveIt! planning scene')
         self.planning_scene = moveit_commander.PlanningSceneInterface()
 
+        rospy.loginfo('[move_arm] Exposing server for planning scene updates as %s', self.update_planning_scene_srv_name)
+        self.update_planning_scene_server = rospy.Service(self.update_planning_scene_srv_name,
+                                                          UpdatePlanningScene,
+                                                          self.update_planning_scene_handler)
+        rospy.loginfo('[move_arm] Move arm action initialised')
+
         return FTSMTransitions.INITIALISED
 
     def running(self):
@@ -124,3 +137,30 @@ def set_result(self, success):
         result = MoveArmResult()
         result.success = success
         return result
+
+    def update_planning_scene_handler(self, request):
+        rospy.loginfo('[move_arm] Received a planning scene update request to %s %d objects', request.operation,
+                                                                                              len(request.objects.objects))
+        response = UpdatePlanningSceneResponse(success=False)
+        if request.operation == UpdatePlanningSceneRequest.ADD:
+            try:
+                for obj in request.objects.objects:
+                    self.planning_scene.add_box(obj.name, obj.pose,
+                                                (obj.dimensions.vector.x,
+                                                 obj.dimensions.vector.y,
+                                                 obj.dimensions.vector.z))
+                response.success = True
+            except Exception as exc:
+                rospy.logerr('[move_arm/update_planning_scene_handler] Error while adding objects')
+                rospy.logerr(str(exc))
+        elif request.operation == UpdatePlanningSceneRequest.REMOVE:
+            try:
+                for obj in request.objects.objects:
+                    self.planning_scene.remove_world_object(obj.name)
+                response.success = True
+            except Exception as exc:
+                rospy.logerr('[move_arm/update_planning_scene_handler] Error while removing objects')
+                rospy.logerr(str(exc))
+        else:
+            rospy.logerr('[move_arm/update_planning_scene_handler] Received unknown operation %s', request.operation)
+        return response

From 693dd42679d5f16251861a49b4f03bfcd4d56bbd Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 17:29:14 +0100
Subject: [PATCH 172/264] [wrs_tidy_up] Added mdr_manipulation_msgs to the list
 of dependencies

---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt            | 2 ++
 .../mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
index 1c6b7a67f..2fb6c8cc5 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   roslint
   std_msgs
   geometry_msgs
+  mdr_manipulation_msgs
   mas_execution_manager
   mas_knowledge_base
 )
@@ -17,6 +18,7 @@ catkin_package(
     rospy
     std_msgs
     geometry_msgs
+    mdr_manipulation_msgs
     mas_execution_manager
     mas_knowledge_base
 )
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
index b9c9dd5b7..f56cae4c2 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/package.xml
@@ -17,12 +17,14 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>mdr_manipulation_msgs</build_depend>
   <build_depend>mas_execution_manager</build_depend>
   <build_depend>mas_knowledge_base</build_depend>
 
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>mdr_manipulation_msgs</exec_depend>
   <exec_depend>mas_execution_manager</exec_depend>
   <exec_depend>mas_knowledge_base</exec_depend>
 

From d469864e0f78ff88541676d815591a9365b45ce8 Mon Sep 17 00:00:00 2001
From: Ahmed Abdelrahman <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 4 Dec 2020 18:16:58 +0100
Subject: [PATCH 173/264] Split cleanup_object state, and add missing module
 names

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 135 ++++++++++++++----
 1 file changed, 109 insertions(+), 26 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index f3c80a51e..a046b2f6e 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -2,21 +2,18 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_wrs_tidy_up
-states: [CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_FLOOR_OBJECTS, SELECT_OBJECT_FOR_PICKING, CLEAN_UP_OBJECT, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, CLEAN_UP_OBSTACLE]
+states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_FLOOR_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICK_UP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, CLEAN_UP_OBSTACLE]
 outcomes: [DONE, TIMEOUT, FAILED]
 state_descriptions:
     # Task 1: Cleaning up objects
     - state:
-        name: CHECK_TIDYING_DONE
-        state_module_name: mdr_wrs_tidy_up.scenario_states
-        state_class_name: CheckTidyingDone
+        name: INITIALISE_SCENARIO 
+        state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
+        state_class_name: Initialise_scenario
         transitions:
             - transition:
-                name: room_not_clear
+                name: succeeded
                 state: SELECT_SCANNING_POSE
-            - transition:
-                name: room_clear
-                state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
             - argument:
                 name: floor_objects_cleared
@@ -29,9 +26,20 @@ state_descriptions:
                 value:
                     long_table_b: false
                     tall_table: false
+    - state:
+        name: CHECK_TIDYING_DONE
+        state_module_name: mdr_wrs_tidy_up.scenario_states.check_tidying_done
+        state_class_name: CheckTidyingDone
+        transitions:
+            - transition:
+                name: room_not_clear
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: room_clear
+                state: GO_TO_SECOND_ROOM_CORRIDOR
     - state:
         name: SELECT_SCANNING_POSE
-        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
         state_class_name: SelectScanningPose
         transitions:
             - transition:
@@ -52,12 +60,15 @@ state_descriptions:
                 value: 3
     - state:
         name: GO_TO_SCANNING_POSE
-        state_module_name: mdr_navigation_behaviours.move_base
-        state_class_name: MoveBase
+        state_module_name: mdr_wrs_tidy_up.scenario_states.go_to_scanning_pose
+        state_class_name: MoveBaseForCleanup
         transitions:
             - transition:
-                name: succeeded
+                name: floor_cleanup
                 state: FIND_FLOOR_OBJECTS
+            - transition:
+                name: table_cleanup
+                state: FIND_TABLE_OBJECTS
             - transition:
                 name: failed
                 state: GO_TO_SCANNING_POSE
@@ -69,8 +80,8 @@ state_descriptions:
                 name: number_of_retries
                 value: 3
     - state:
-        name: FIND_FLOOR_OBJECTS
-        state_module_name: mdr_wrs_tidy_up.scenario_states
+        name: FIND_OBJECTS
+        state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
         state_class_name: FindObjects
         transitions:
             - transition:
@@ -91,7 +102,7 @@ state_descriptions:
                 value: 3
     - state:
         name: SELECT_OBJECT_FOR_PICKING
-        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
         state_class_name: SelectObjectForPicking
         transitions:
             - transition:
@@ -108,16 +119,52 @@ state_descriptions:
                 name: number_of_retries
                 value: 3
     - state:
-        name: CLEAN_UP_OBJECT
-        state_module_name: mdr_wrs_tidy_up.scenario_states
-        state_class_name: CleanUpObject
+        name: PICK_UP_OBJECT
+        state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
+        state_class_name: PickupObject
         transitions:
             - transition:
                 name: succeeded
-                state: SELECT_SCANNING_POSE
+                state: MOVE_TO_STORAGE_LOCATION
             - transition:
                 name: failed
-                state: CLEAN_UP_OBJECT
+                state: PICKUP_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: GO_TO_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: MOVE_TO_STORAGE_LOCATION
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_to_storage_location
+        state_class_name: MoveToStorageLocation
+        transitions:
+            - transition:
+                name: succeeded
+                state: THROW_OBJECT
+            - transition:
+                name: failed
+                state: MOVE_TO_STORAGE_LOCATION
+            - transition:
+                name: failed_after_retrying
+                state: MOVE_TO_STORAGE_LOCATION
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: THROW_OBJECT
+        state_module_name: mdr_wrs_tidy_up.scenario_states.throw_object
+        state_class_name: ThrowObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: CHECK_TIDYING_DONE
+            - transition:
+                name: failed
+                state: THROW_OBJECT
             - transition:
                 name: failed_after_retrying
                 state: CHECK_TIDYING_DONE
@@ -147,7 +194,7 @@ state_descriptions:
                 value: 3
     - state:
         name: FIND_FLOOR_OBSTACLES
-        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_module_name: mdr_wrs_tidy_up.scenario_states.find_floor_obstacles
         state_class_name: FindObjects
         transitions:
             - transition:
@@ -168,7 +215,7 @@ state_descriptions:
                 value: 3
     - state:
         name: SELECT_OBSTACLE_FOR_PICKING
-        state_module_name: mdr_wrs_tidy_up.scenario_states
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_obstacle_for_picking
         state_class_name: SelectObjectForPicking
         transitions:
             - transition:
@@ -185,16 +232,52 @@ state_descriptions:
                 name: number_of_retries
                 value: 3
     - state:
-        name: CLEAN_UP_OBSTACLE
-        state_module_name: mdr_wrs_tidy_up.scenario_states
-        state_class_name: CleanUpObject
+        name: PICK_UP_OBSTACLE
+        state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
+        state_class_name: PickupObject
         transitions:
             - transition:
                 name: succeeded
+                state: MOVE_TO_BOX
+            - transition:
+                name: failed
+                state: PICKUP_OBSTACLE
+            - transition:
+                name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: MOVE_TO_BOX
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_to_storage_location
+        state_class_name: MoveToStorageLocation
+        transitions:
+            - transition:
+                name: succeeded
+                state: THROW_OBSTACLE
             - transition:
                 name: failed
-                state: CLEAN_UP_OBSTACLE
+                state: MOVE_TO_BOX
+            - transition:
+                name: failed_after_retrying
+                state: MOVE_TO_BOX
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: THROW_OBSTACLE
+        state_module_name: mdr_wrs_tidy_up.scenario_states.throw_object
+        state_class_name: ThrowObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+            - transition:
+                name: failed
+                state: THROW_OBSTACLE
             - transition:
                 name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR

From 77215cac9db9020f5d44ec239fec27aec31040c0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 18:31:36 +0100
Subject: [PATCH 174/264] [tidy_up/config] Small updates to the SM

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 41 +++++++++++--------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index a046b2f6e..48a7881c3 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -2,12 +2,12 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_wrs_tidy_up
-states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_FLOOR_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICK_UP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, CLEAN_UP_OBSTACLE]
+states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE]
 outcomes: [DONE, TIMEOUT, FAILED]
 state_descriptions:
     # Task 1: Cleaning up objects
     - state:
-        name: INITIALISE_SCENARIO 
+        name: INITIALISE_SCENARIO
         state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
         state_class_name: Initialise_scenario
         transitions:
@@ -36,7 +36,7 @@ state_descriptions:
                 state: SELECT_SCANNING_POSE
             - transition:
                 name: room_clear
-                state: GO_TO_SECOND_ROOM_CORRIDOR
+                state: INITIALISE_OBSTACLE_REMOVAL
     - state:
         name: SELECT_SCANNING_POSE
         state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
@@ -60,15 +60,12 @@ state_descriptions:
                 value: 3
     - state:
         name: GO_TO_SCANNING_POSE
-        state_module_name: mdr_wrs_tidy_up.scenario_states.go_to_scanning_pose
-        state_class_name: MoveBaseForCleanup
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
         transitions:
             - transition:
-                name: floor_cleanup
-                state: FIND_FLOOR_OBJECTS
-            - transition:
-                name: table_cleanup
-                state: FIND_TABLE_OBJECTS
+                name: succeeded
+                state: FIND_OBJECTS
             - transition:
                 name: failed
                 state: GO_TO_SCANNING_POSE
@@ -92,7 +89,7 @@ state_descriptions:
                 state: SELECT_SCANNING_POSE
             - transition:
                 name: failed
-                state: FIND_FLOOR_OBJECTS
+                state: FIND_OBJECTS
             - transition:
                 name: failed_after_retrying
                 state: SELECT_SCANNING_POSE
@@ -107,7 +104,7 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: CLEAN_UP_OBJECT
+                state: PICKUP_OBJECT
             - transition:
                 name: failed
                 state: SELECT_OBJECT_FOR_PICKING
@@ -119,7 +116,7 @@ state_descriptions:
                 name: number_of_retries
                 value: 3
     - state:
-        name: PICK_UP_OBJECT
+        name: PICKUP_OBJECT
         state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
         state_class_name: PickupObject
         transitions:
@@ -174,6 +171,18 @@ state_descriptions:
                 value: 3
 
     # Task 2a: Going to the second room
+    - state:
+        name: INITIALISE_OBSTACLE_REMOVAL
+        state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
+        state_class_name: Initialise_scenario
+        transitions:
+            - transition:
+                name: succeeded
+                state: GO_TO_SECOND_ROOM_CORRIDOR
+        arguments:
+            - argument:
+                name: object_location
+                value: floor
     - state:
         name: GO_TO_SECOND_ROOM_CORRIDOR
         state_module_name: mdr_navigation_behaviours.move_base
@@ -194,7 +203,7 @@ state_descriptions:
                 value: 3
     - state:
         name: FIND_FLOOR_OBSTACLES
-        state_module_name: mdr_wrs_tidy_up.scenario_states.find_floor_obstacles
+        state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
         state_class_name: FindObjects
         transitions:
             - transition:
@@ -220,7 +229,7 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: CLEAN_UP_OBSTACLE
+                state: PICKUP_OBSTACLE
             - transition:
                 name: failed
                 state: SELECT_OBSTACLE_FOR_PICKING
@@ -232,7 +241,7 @@ state_descriptions:
                 name: number_of_retries
                 value: 3
     - state:
-        name: PICK_UP_OBSTACLE
+        name: PICKUP_OBSTACLE
         state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
         state_class_name: PickupObject
         transitions:

From a6ddda3c32af50181bbcaf57d93521af2f2681d8 Mon Sep 17 00:00:00 2001
From: Ahmed Abdelrahman <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 4 Dec 2020 19:40:46 +0100
Subject: [PATCH 175/264] [wrs_tidy_up] Add SelectObjectForPicking state

---
 .../select_object_for_picking.py              | 38 +++++++++++++++++++
 1 file changed, 38 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
new file mode 100644
index 000000000..c60aec84a
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
@@ -0,0 +1,38 @@
+import tf
+import rospy
+import numpy as np
+
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class SelectObjectForPicking(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'select_object_for_picking',
+                                   save_sm_state=save_sm_state,
+                                   output_keys=['selected_object'],
+                                   outcomes=['succeeded'])
+        self.tf_listener = tf.TransformListener()
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'select_object_for_picking')
+
+    def execute(self, userdata):
+        closest_obj_index = 0
+        closest_obj_distance = float('inf')
+        for index, obj in enumerate(userdata.detected_objects):
+            while not rospy.is_shutdown():
+                try:
+                    (obj_position, obj_orientation) = self.tf_listener.lookupTransform('/base_link', 
+                                                                                       obj.pose.header.frame_id, 
+                                                                                       rospy.Time(0))
+                    break
+                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+                    continue
+
+            distance_to_obj = np.linalg.norm(np.array(obj_position))
+
+            if distance_to_obj < closest_obj_distance:
+                closest_obj_index = index
+                closest_obj_distance = distance_to_obj
+
+        userdata.selected_object = userdata.objects[closest_obj_index]
+
+        return 'succeeded'

From a986ac6ffb6005f9d9e892e1e62ba479caec0121 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 4 Dec 2020 20:13:18 +0100
Subject: [PATCH 176/264] [wrs_tidy_up/config] Various fixes to the SM

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 43 +++++++++----------
 1 file changed, 20 insertions(+), 23 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 48a7881c3..40675c54e 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -9,7 +9,7 @@ state_descriptions:
     - state:
         name: INITIALISE_SCENARIO
         state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
-        state_class_name: Initialise_scenario
+        state_class_name: InitialiseScenario
         transitions:
             - transition:
                 name: succeeded
@@ -105,12 +105,6 @@ state_descriptions:
             - transition:
                 name: succeeded
                 state: PICKUP_OBJECT
-            - transition:
-                name: failed
-                state: SELECT_OBJECT_FOR_PICKING
-            - transition:
-                name: failed_after_retrying
-                state: SELECT_SCANNING_POSE
         arguments:
             - argument:
                 name: number_of_retries
@@ -135,8 +129,8 @@ state_descriptions:
                 value: 3
     - state:
         name: MOVE_TO_STORAGE_LOCATION
-        state_module_name: mdr_wrs_tidy_up.scenario_states.move_to_storage_location
-        state_class_name: MoveToStorageLocation
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
         transitions:
             - transition:
                 name: succeeded
@@ -153,8 +147,8 @@ state_descriptions:
                 value: 3
     - state:
         name: THROW_OBJECT
-        state_module_name: mdr_wrs_tidy_up.scenario_states.throw_object
-        state_class_name: ThrowObject
+        state_module_name: mdr_manipulation_behaviours.throw_object_in
+        state_class_name: ThrowObjectIn
         transitions:
             - transition:
                 name: succeeded
@@ -174,7 +168,7 @@ state_descriptions:
     - state:
         name: INITIALISE_OBSTACLE_REMOVAL
         state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
-        state_class_name: Initialise_scenario
+        state_class_name: InitialiseScenario
         transitions:
             - transition:
                 name: succeeded
@@ -198,6 +192,9 @@ state_descriptions:
                 name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
+            - argument:
+                name: destination_locations
+                value: [second_room_corridor]
             - argument:
                 name: number_of_retries
                 value: 3
@@ -219,23 +216,23 @@ state_descriptions:
                 name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
+            - argument:
+                name: object_detection_server_name
+                value: /mas_perception/detect_objects
+            - argument:
+                name: cloud_obstacle_detection_topic
+                value: /mas_perception/cloud_obstacle_detection/obstacle_objects
             - argument:
                 name: number_of_retries
                 value: 3
     - state:
         name: SELECT_OBSTACLE_FOR_PICKING
-        state_module_name: mdr_wrs_tidy_up.scenario_states.select_obstacle_for_picking
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
         state_class_name: SelectObjectForPicking
         transitions:
             - transition:
                 name: succeeded
                 state: PICKUP_OBSTACLE
-            - transition:
-                name: failed
-                state: SELECT_OBSTACLE_FOR_PICKING
-            - transition:
-                name: failed_after_retrying
-                state: FIND_FLOOR_OBSTACLES
         arguments:
             - argument:
                 name: number_of_retries
@@ -260,8 +257,8 @@ state_descriptions:
                 value: 3
     - state:
         name: MOVE_TO_BOX
-        state_module_name: mdr_wrs_tidy_up.scenario_states.move_to_storage_location
-        state_class_name: MoveToStorageLocation
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
         transitions:
             - transition:
                 name: succeeded
@@ -278,8 +275,8 @@ state_descriptions:
                 value: 3
     - state:
         name: THROW_OBSTACLE
-        state_module_name: mdr_wrs_tidy_up.scenario_states.throw_object
-        state_class_name: ThrowObject
+        state_module_name: mdr_manipulation_behaviours.throw_object_in
+        state_class_name: ThrowObjectIn
         transitions:
             - transition:
                 name: succeeded

From e2b6440ecf91c0ce01800cffe4aa16f5df0dc026 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 13:23:33 +0100
Subject: [PATCH 177/264] [wrs_tidy_up] Added state for scenario initialisation

The initialisation of the MoveIt! planning scene is missing here;
this will be added with a bit later on.
---
 .../scenario_states/initialise_scenario.py    | 21 +++++++++++++++++++
 1 file changed, 21 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
new file mode 100644
index 000000000..1ec3d7beb
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -0,0 +1,21 @@
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class InitialiseScenario(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'initialise_scenario',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded'],
+                                   output_keys=['floor_objects_cleared',
+                                                'table_objects_cleared',
+                                                'object_location'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'initialise_scenario')
+        self.floor_objects_cleared = kwargs.get('floor_objects_cleared', None)
+        self.table_objects_cleared = kwargs.get('table_objects_cleared', None)
+        self.object_location = kwargs.get('object_location', 'floor')
+
+    def execute(self, userdata):
+        userdata.floor_objects_cleared = self.floor_objects_cleared
+        userdata.table_objects_cleared = self.table_objects_cleared
+        userdata.object_location = self.object_location
+        return 'succeeded'

From 998a326e45cb3e5eeb3335cdd3ec0017c33f6482 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 13:24:13 +0100
Subject: [PATCH 178/264] [wrs_tidy_up] Added a state that takes care of
 grasping an object

---
 .../scenario_states/pickup_object.py          | 121 ++++++++++++++++++
 1 file changed, 121 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
new file mode 100644
index 000000000..b82c265f0
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -0,0 +1,121 @@
+import numpy as np
+
+import rospy
+import actionlib
+import tf
+
+from geometry_msgs.msg import Pose, PoseStamped
+from mdr_pickup_action.msg import PickupAction, PickupGoal
+
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class PickupObject(ScenarioStateBase):
+    pickup_server_name = 'pickup_server'
+    pickup_client = None
+    tf_listener = None
+    grasping_timeout_s = 30.
+
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'pickup_object',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed',
+                                             'failed_after_retrying'],
+                                   input_keys=['selected_object'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'pickup_object')
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.pickup_server_name = kwargs.get('pickup_server_name', 'pickup_server')
+        self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
+        self.retry_count = 0
+        self.__init_ros_components()
+
+    def execute(self, userdata):
+        object_to_pick_up = userdata.selected_object
+
+        goal = PickupGoal()
+        goal.pose.header.frame_id = object_to_pick_up.pose.header.frame_id
+        goal.pose.header.stamp = rospy.Time.now()
+        goal.pose.pose = self.get_grasping_pose(object_to_pick_up)
+
+        rospy.loginfo('[%s] Picking up object at %s position (%f %f %f)', self.state_name,
+                      goal.pose.header.frame_id, goal.pose.pose.position.x,
+                      goal.pose.pose.position.y, goal.pose.pose.position.z)
+
+        self.pickup_client.send_goal(goal)
+        if self.pickup_client.wait_for_result(rospy.Duration.from_sec(self.grasping_timeout_s)):
+            pickup_result = self.pickup_client.get_result()
+            if pickup_result.success:
+                rospy.loginfo('[%s] Successfully grasped object', self.state_name)
+                return 'succeeded'
+            else:
+                rospy.logerr('[%s] Failed to grasp object', self.state_name)
+        else:
+            rospy.logerr('[%s] Object not picked up within %f seconds; giving up',
+                         self.state_name, self.grasping_timeout_s)
+            self.pickup_client.cancel_all_goals()
+
+        if self.retry_count == self.number_of_retries:
+            rospy.logerr('[%s] Could not pick up object after retrying; giving up', self.state_name)
+            return 'failed_after_retrying'
+
+        rospy.loginfo('[%s] Retrying to place object', self.state_name)
+        self.retry_count += 1
+        return 'failed'
+
+    def get_grasping_pose(self, object_to_pick_up):
+        '''Returns a geometry_msgs.msg.Pose object representing a grasping pose
+        for the given object. The grasping position is given by the center of the
+        object's bounding box; the orientation ensures a top-down grasp aligned
+        with the longest axis of the object.
+
+        Keyword arguments:
+        object_to_pick_up: mas_perception_msgs.msg.Object
+
+        '''
+        pose = Pose()
+        pose.position.x = object_to_pick_up.bounding_box.center.x
+        pose.position.y = object_to_pick_up.bounding_box.center.y
+        pose.position.z = object_to_pick_up.bounding_box.center.z
+
+        gripper_orientation_z = np.arctan2(object_to_pick_up.bounding_box.dimensions.y,
+                                           object_to_pick_up.bounding_box.dimensions.x)
+
+        # this orientation guarantees a top-down grasp and
+        # alignment along the longest axis of the object
+        desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
+
+        pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
+                                                        object_to_pick_up.pose.header.frame_id)
+        return pose
+
+    def get_gripper_orientation(self, orientation, target_frame):
+        '''Returns a geometry_msgs.msg.Quaternion object representing the given
+        orientation, which is represented in "base_link", in the given target frame.
+
+        Keyword arguments:
+        orientation: Tuple[float, float, float] -- desired (roll, pitch, yaw) orientation of
+                                                   the gripper in the base_link frame
+        target_frame: str -- frame in which the orientation should be represented
+
+        '''
+        gripper_pose = PoseStamped()
+        gripper_pose.header.frame_id = 'base_link'
+        gripper_pose.header.stamp = rospy.Time(0)
+
+        gripper_quaternion = tf.transformations.quaternion_from_euler(*orientation)
+        gripper_pose.pose.orientation.x = gripper_quaternion[0]
+        gripper_pose.pose.orientation.y = gripper_quaternion[1]
+        gripper_pose.pose.orientation.z = gripper_quaternion[2]
+        gripper_pose.pose.orientation.w = gripper_quaternion[3]
+
+        rospy.loginfo('[%s] Transforming pose from base_link to %s', self.state_name, target_frame)
+        gripper_pose_in_target_frame = self.tf_listener.transformPose(target_frame, gripper_pose)
+        return gripper_pose_in_target_frame.pose.orientation
+
+    def __init_ros_components(self):
+        rospy.loginfo('Initialising client for action %s', self.pickup_server_name)
+        self.pickup_client = actionlib.SimpleActionClient(self.pickup_server_name, PickupAction)
+        self.pickup_client.wait_for_server()
+        rospy.loginfo('Client for action %s initialised', self.pickup_server_name)
+
+        self.tf_listener = tf.TransformListener()

From e37fc1207c868ef7beed50bb4a6b78573e9be7ea Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 13:25:07 +0100
Subject: [PATCH 179/264] [wrs_tidy_up/select_object_for_picking] Declared
 missing input keys

---
 .../scenario_states/select_object_for_picking.py           | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
index c60aec84a..0f6a73db6 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
@@ -8,6 +8,7 @@ class SelectObjectForPicking(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'select_object_for_picking',
                                    save_sm_state=save_sm_state,
+                                   input_keys=['detected_objects'],
                                    output_keys=['selected_object'],
                                    outcomes=['succeeded'])
         self.tf_listener = tf.TransformListener()
@@ -20,8 +21,8 @@ def execute(self, userdata):
         for index, obj in enumerate(userdata.detected_objects):
             while not rospy.is_shutdown():
                 try:
-                    (obj_position, obj_orientation) = self.tf_listener.lookupTransform('/base_link', 
-                                                                                       obj.pose.header.frame_id, 
+                    (obj_position, obj_orientation) = self.tf_listener.lookupTransform('/base_link',
+                                                                                       obj.pose.header.frame_id,
                                                                                        rospy.Time(0))
                     break
                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
@@ -33,6 +34,6 @@ def execute(self, userdata):
                 closest_obj_index = index
                 closest_obj_distance = distance_to_obj
 
-        userdata.selected_object = userdata.objects[closest_obj_index]
+        userdata.selected_object = userdata.detected_objects[closest_obj_index]
 
         return 'succeeded'

From 99741c5b67ad7d7c04de6d7b0864a1ecf6898c0c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 14:42:28 +0100
Subject: [PATCH 180/264] [wrs_tidy_up/pickup_object] Set a 'grasped_object'
 output and added the grasped object to the KB

These are both necessary so that the mdr_manipulation_behaviours.throw_object_in state can be used
---
 .../src/mdr_wrs_tidy_up/scenario_states/pickup_object.py   | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index b82c265f0..e234f8311 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -20,7 +20,8 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'failed',
                                              'failed_after_retrying'],
-                                   input_keys=['selected_object'])
+                                   input_keys=['selected_object'],
+                                   output_keys=['grasped_object'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'pickup_object')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
@@ -45,7 +46,9 @@ def execute(self, userdata):
         if self.pickup_client.wait_for_result(rospy.Duration.from_sec(self.grasping_timeout_s)):
             pickup_result = self.pickup_client.get_result()
             if pickup_result.success:
-                rospy.loginfo('[%s] Successfully grasped object', self.state_name)
+                rospy.loginfo('[%s] Successfully grasped object %s', self.state_name, object_to_pick_up.name)
+                userdata.grasped_object = object_to_pick_up.name
+                self.kb_interface.insert_obj_instance(object_to_pick_up.name, object_to_pick_up)
                 return 'succeeded'
             else:
                 rospy.logerr('[%s] Failed to grasp object', self.state_name)

From e9895e3c8d49567f6e1e5dd23eb7ba1a34d00fca Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 21:45:01 +0100
Subject: [PATCH 181/264] [wrs_tidy_up/config] Added a config file for
 initialising the moveit planning scene with static environment objects

All tables, the two bins, and all walls have been added to the scene;
the bookshelf and the drawers have not been added in this version
---
 .../config/planning_scene_map.yaml            | 109 ++++++++++++++++++
 1 file changed, 109 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
new file mode 100644
index 000000000..9b30ef77e
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
@@ -0,0 +1,109 @@
+# the positions of the objects in the map frame have been calculated based on:
+# * the object poses given in the wrs2020_fast world file and
+# * the object sizes as specified in their respective model files
+# * the initial position of the robot in the world: pos=(-2.1, -1.2, 0.), rot=(0, -1.57, 0.)
+frame_id: map
+objects:
+    # object ID: wall_back
+    - name: wall_back
+      position: [-0.8, 2.7, 0.3]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.1, 4.8, 0.6]
+    # object ID: wall_front
+    - name: wall_front
+      position: [3.2, 2.1, 0.3]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.1, 6.0, 0.6]
+    # object ID: wall_left
+    - name: wall_left
+      position: [1.2, -0.9, 0.3]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [4.1, 0.1, 0.6]
+    # object ID: wall_right
+    - name: wall_right
+      position: [1.2, 5.1, 0.3]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [4.1, 0.1, 0.6]
+    # object ID: wall_center
+    - name: wall_center
+      position: [0.6, 2.1, 0.3]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [2.8, 0.1, 0.6]
+
+    # object ID: wrc_bin_green
+    - name: bin_a
+      position:   [2.4, -0.6, 0.165]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.33, 0.33, 0.33]
+
+    # object ID: wrs_bin_black
+    - name: bin_b
+      position:   [2.9, -0.6, 0.165]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.33, 0.33, 0.33]
+
+    # object ID: wrc_long_table_0
+    - name: long_table_a
+      position:   [1.5, -0.6, 0.39]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [1.2, 0.4, 0.02]
+    - name: long_table_a_leg1
+      position:   [2.1, -0.4, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_a_leg2
+      position:   [2.1, -0.8, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_a_leg3
+      position:   [0.9, -0.4, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_a_leg4
+      position:   [0.9, -0.8, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+
+    # object ID: wrc_tall_table
+    - name: tall_table
+      position:   [0.0, 1.8, 0.59]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.40, 0.40, 0.02]
+    - name: tall_table_leg1
+      position:   [-0.2, 1.6, 0.295]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.60]
+    - name: tall_table_leg2
+      position:   [-0.2, 2.0, 0.295]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.60]
+    - name: tall_table_leg3
+      position:   [0.2, 1.6, 0.295]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.60]
+    - name: tall_table_leg4
+      position:   [0.2, 2.0, 0.295]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.60]
+
+    # object ID: wrc_long_table
+    - name: long_table_b
+      position:   [1.0, 1.8, 0.39]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [1.2, 0.4, 0.02]
+    - name: long_table_b_leg1
+      position:   [0.4, 2.0, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_b_leg2
+      position:   [0.4, 1.6, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_b_leg3
+      position:   [1.6, 2.0, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]
+    - name: long_table_b_leg4
+      position:   [1.6, 1.6, 0.195]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.02, 0.02, 0.40]

From 067ae2e939cc541a47ed86e6d47bc04747f8783c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 7 Dec 2020 21:47:33 +0100
Subject: [PATCH 182/264] [wrs_tidy_up/initialise_scenario] The moveit planning
 scene is now initialised

---
 .../scenario_states/initialise_scenario.py    | 73 +++++++++++++++++++
 1 file changed, 73 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 1ec3d7beb..10d869502 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -1,6 +1,24 @@
+import yaml
+
+import rospy
+import tf
+from geometry_msgs.msg import Pose, Point, Quaternion, Vector3
+
+from mas_perception_msgs.msg import Object, ObjectList
+from mdr_manipulation_msgs.srv import UpdatePlanningScene, UpdatePlanningSceneRequest
+
+from mas_tools.ros_utils import get_package_path
+
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
 class InitialiseScenario(ScenarioStateBase):
+    floor_objects_cleared = None
+    table_objects_cleared = None
+    object_location = ''
+    planning_scene_update_service_name = ''
+    planning_scene_update_proxy = None
+    planning_scene_map_file = None
+
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'initialise_scenario',
                                    save_sm_state=save_sm_state,
@@ -13,9 +31,64 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.floor_objects_cleared = kwargs.get('floor_objects_cleared', None)
         self.table_objects_cleared = kwargs.get('table_objects_cleared', None)
         self.object_location = kwargs.get('object_location', 'floor')
+        self.planning_scene_map_file = kwargs.get('planning_scene_map_file', '')
+        self.planning_scene_update_service_name = kwargs.get('planning_scene_update_service_name',
+                                                             '/move_arm_action/update_planning_scene')
+        self.__init_ros_components()
 
     def execute(self, userdata):
         userdata.floor_objects_cleared = self.floor_objects_cleared
         userdata.table_objects_cleared = self.table_objects_cleared
         userdata.object_location = self.object_location
+
+        update_planning_scene_req = UpdatePlanningSceneRequest()
+        update_planning_scene_req.operation = UpdatePlanningSceneRequest.ADD
+        update_planning_scene_req.objects = self.__get_environment_objects()
+
+        if self.planning_scene_map_file:
+            rospy.loginfo('[%s] Updating planning scene', self.state_name)
+            response = self.planning_scene_update_proxy(update_planning_scene_req)
+            if response is not None:
+                if response.success:
+                    rospy.loginfo('[%s] Successfully updated the planning scene', self.state_name)
+                else:
+                    rospy.logerr('[%s] Failed to update the planning scene', self.state_name)
+            else:
+                rospy.logerr('[%s] Response not received', self.state_name)
+        else:
+            rospy.loginfo('[%s] Planning scene map file not specified; not updating scene', self.state_name)
+
         return 'succeeded'
+
+    def __get_environment_objects(self):
+        object_list = ObjectList()
+
+        scene_file_path = get_package_path('mdr_wrs_tidy_up', 'config',
+                                           self.planning_scene_map_file)
+        scene_data = None
+        with open(scene_file_path, 'r') as planning_scene_map:
+            scene_data = yaml.safe_load(planning_scene_map)
+
+        frame_id = scene_data['frame_id']
+        for obj_data in scene_data['objects']:
+            obj = Object()
+            obj.name = obj_data['name']
+            obj.pose.header.frame_id = frame_id
+
+            euler_orientation = obj_data['orientation']
+            quaternion = tf.transformations.quaternion_from_euler(*euler_orientation)
+
+            obj.pose.pose = Pose(Point(*obj_data['position']), Quaternion(*quaternion))
+            obj.dimensions.header.frame_id = frame_id
+            obj.dimensions.vector = Vector3(*obj_data['dimensions'])
+            object_list.objects.append(obj)
+        return object_list
+
+    def __init_ros_components(self):
+        rospy.loginfo('[%s] Creating a service proxy for %s',
+                      self.state_name, self.planning_scene_update_service_name)
+        rospy.wait_for_service(self.planning_scene_update_service_name)
+        self.planning_scene_update_proxy = rospy.ServiceProxy(self.planning_scene_update_service_name,
+                                                              UpdatePlanningScene)
+        rospy.loginfo('[%s] Service proxy for %s created',
+                      self.state_name, self.planning_scene_update_service_name)

From 869dfca86de6aea5c55e0a2b5fe1f9a86e1d609f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 8 Dec 2020 09:53:25 +0100
Subject: [PATCH 183/264] [wrs_tidy_up/config/storage_locations] box -> bin;
 added bin as default location for unknown objects

---
 .../config/storage_locations.yaml             | 73 ++++++++++---------
 1 file changed, 37 insertions(+), 36 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml
index 9126e8bc6..44023125b 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/storage_locations.yaml
@@ -1,3 +1,4 @@
+unknown: bin
 001_chips_can: long_table_a
 002_master_chef_can: long_table_a
 003_cracker_box: long_table_a
@@ -23,44 +24,44 @@
 024_bowl: long_table_a
 025_mug: long_table_a
 026_sponge: long_table_a
-027-skillet: box
-028_skillet_lid: box
+027-skillet: bin
+028_skillet_lid: bin
 029_plate: long_table_a
 030_fork: long_table_a
 031_spoon: long_table_a
 032_knife: long_table_a
 033_spatula: long_table_a
-035_power_drill: box
-036_wood_block: box
-037_scissors: box
-038_padlock: box
-039_key: box
-040_large_marker: box
-041_small_marker: box
-042_adjustable_wrench: box
-043_phillips_screwdriver: box
-044_flat_screwdriver: box
-046_plastic_bolt: box
-047_plastic_nut: box
-048_hammer: box
-049_small_clamp: box
-050_medium_clamp: box
-051_large_clamp: box
-052_extra_large_clamp: box
-053_mini_soccer_ball: box
-054_softball: box
-055_baseball: box
-056_tennis_ball: box
-057_racquetball: box
-058_golf_ball: box
-059_chain: box
-061_foam_brick: box
-062_dice: box
-063-a_marbles: box
-065-a_cups: box
-070-a_colored_wood_blocks: box
-071_nine_hole_peg_test: box
-072-a_toy_airplane: box
-073-a_lego_duplo: box
-076_timer: box
-077_rubiks_cube: box
+035_power_drill: bin
+036_wood_block: bin
+037_scissors: bin
+038_padlock: bin
+039_key: bin
+040_large_marker: bin
+041_small_marker: bin
+042_adjustable_wrench: bin
+043_phillips_screwdriver: bin
+044_flat_screwdriver: bin
+046_plastic_bolt: bin
+047_plastic_nut: bin
+048_hammer: bin
+049_small_clamp: bin
+050_medium_clamp: bin
+051_large_clamp: bin
+052_extra_large_clamp: bin
+053_mini_soccer_ball: bin
+054_softball: bin
+055_baseball: bin
+056_tennis_ball: bin
+057_racquetball: bin
+058_golf_ball: bin
+059_chain: bin
+061_foam_brick: bin
+062_dice: bin
+063-a_marbles: bin
+065-a_cups: bin
+070-a_colored_wood_blocks: bin
+071_nine_hole_peg_test: bin
+072-a_toy_airplane: bin
+073-a_lego_duplo: bin
+076_timer: bin
+077_rubiks_cube: bin

From 9a8ff811ccbf3b54885cb4af2b3b2ad6a6c3b535 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 8 Dec 2020 10:03:50 +0100
Subject: [PATCH 184/264] [wrs_tidy_up/initialise_scenario] The KB is now
 initialised as well

---
 .../scenario_states/initialise_scenario.py    | 92 ++++++++++++-------
 1 file changed, 57 insertions(+), 35 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 10d869502..8d4dc228c 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -11,6 +11,32 @@
 
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
+def get_environment_objects(planning_scene_map_file):
+    object_list = ObjectList()
+
+    scene_file_path = get_package_path('mdr_wrs_tidy_up', 'config', planning_scene_map_file)
+    scene_data = None
+    with open(scene_file_path, 'r') as planning_scene_map:
+        scene_data = yaml.safe_load(planning_scene_map)
+
+    frame_id = scene_data['frame_id']
+    for obj_data in scene_data['objects']:
+        obj = Object()
+        obj.name = obj_data['name']
+        obj.pose.header.frame_id = frame_id
+
+        euler_orientation = obj_data['orientation']
+        quaternion = tf.transformations.quaternion_from_euler(*euler_orientation)
+
+        obj.pose.pose = Pose(Point(*obj_data['position']), Quaternion(*quaternion))
+        obj.dimensions.header.frame_id = frame_id
+        obj.dimensions.vector = Vector3(*obj_data['dimensions'])
+        obj.bounding_box.center = Point(*obj_data['position'])
+        obj.bounding_box.dimensions = Vector3(*obj_data['dimensions'])
+        object_list.objects.append(obj)
+    return object_list
+
+
 class InitialiseScenario(ScenarioStateBase):
     floor_objects_cleared = None
     table_objects_cleared = None
@@ -41,48 +67,44 @@ def execute(self, userdata):
         userdata.table_objects_cleared = self.table_objects_cleared
         userdata.object_location = self.object_location
 
+        if not self.planning_scene_map_file:
+            rospy.loginfo('[%s] Planning scene map file not specified; not initialising KB and scene', self.state_name)
+            return 'succeeded'
+
+        environment_objects = get_environment_objects(self.planning_scene_map_file)
+
+        # initialising the knowledge base
+        rospy.loginfo('[%s] Initialing knowledge base', self.state_name)
+        self.__init_kb(environment_objects)
+        rospy.loginfo('[%s] Knowledge base initialised', self.state_name)
+
+        # initialising the MoveIt! planning scene
         update_planning_scene_req = UpdatePlanningSceneRequest()
         update_planning_scene_req.operation = UpdatePlanningSceneRequest.ADD
-        update_planning_scene_req.objects = self.__get_environment_objects()
-
-        if self.planning_scene_map_file:
-            rospy.loginfo('[%s] Updating planning scene', self.state_name)
-            response = self.planning_scene_update_proxy(update_planning_scene_req)
-            if response is not None:
-                if response.success:
-                    rospy.loginfo('[%s] Successfully updated the planning scene', self.state_name)
-                else:
-                    rospy.logerr('[%s] Failed to update the planning scene', self.state_name)
+        update_planning_scene_req.objects = environment_objects
+
+        rospy.loginfo('[%s] Initialising planning scene', self.state_name)
+        response = self.planning_scene_update_proxy(update_planning_scene_req)
+        if response is not None:
+            if response.success:
+                rospy.loginfo('[%s] Successfully updated the planning scene', self.state_name)
             else:
-                rospy.logerr('[%s] Response not received', self.state_name)
+                rospy.logerr('[%s] Failed to update the planning scene', self.state_name)
         else:
-            rospy.loginfo('[%s] Planning scene map file not specified; not updating scene', self.state_name)
+            rospy.logerr('[%s] Response not received', self.state_name)
 
         return 'succeeded'
 
-    def __get_environment_objects(self):
-        object_list = ObjectList()
-
-        scene_file_path = get_package_path('mdr_wrs_tidy_up', 'config',
-                                           self.planning_scene_map_file)
-        scene_data = None
-        with open(scene_file_path, 'r') as planning_scene_map:
-            scene_data = yaml.safe_load(planning_scene_map)
-
-        frame_id = scene_data['frame_id']
-        for obj_data in scene_data['objects']:
-            obj = Object()
-            obj.name = obj_data['name']
-            obj.pose.header.frame_id = frame_id
-
-            euler_orientation = obj_data['orientation']
-            quaternion = tf.transformations.quaternion_from_euler(*euler_orientation)
-
-            obj.pose.pose = Pose(Point(*obj_data['position']), Quaternion(*quaternion))
-            obj.dimensions.header.frame_id = frame_id
-            obj.dimensions.vector = Vector3(*obj_data['dimensions'])
-            object_list.objects.append(obj)
-        return object_list
+    def __init_kb(self, object_list):
+        try:
+            for obj in object_list.objects:
+                # we don't add walls and table legs to the knowledge base
+                if obj.name.find('wall') != -1 or obj.name.find('leg') != -1:
+                    continue
+                self.kb_interface.insert_obj_instance(obj.name, obj)
+        except Exception as exc:
+            rospy.logerr('[%s] Error while initialising the knowledge base', self.state_name)
+            rospy.logerr('[%s] %s', self.state_name, str(exc))
 
     def __init_ros_components(self):
         rospy.loginfo('[%s] Creating a service proxy for %s',

From 844cfdb381a3319e42ba1ed65ac8d01bea61c83f Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 8 Dec 2020 20:41:41 +0100
Subject: [PATCH 185/264] environments/wrs20-arena] Added poses for the second
 room

---
 mdr_environments/wrs20-arena/navigation_goals.yaml | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mdr_environments/wrs20-arena/navigation_goals.yaml b/mdr_environments/wrs20-arena/navigation_goals.yaml
index f4d81f9ef..3f65a5991 100644
--- a/mdr_environments/wrs20-arena/navigation_goals.yaml
+++ b/mdr_environments/wrs20-arena/navigation_goals.yaml
@@ -10,3 +10,6 @@ long_table_a_boxes: [1.009, 0.063, -1.544]
 long_table_a_trays: [1.721, 0.187, -1.544]
 bin_a: [2.400, 0.047, -1.544]
 bin_b: [2.748, 0.047, -1.544]
+
+second_room_corridor: [2.535, 1.585, 1.571]
+second_room_cupboard: [2.199, 3.839, 1.501]

From 12d7f0f695c3fa1f6565c1ef566b7d0d7f24bc3b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 9 Dec 2020 17:10:30 +0100
Subject: [PATCH 186/264] [wrs_tidy_up/pickup_object] Added a grasping height
 offset

This avoids collisions of the gripper with the surface on which the
object is
---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py  | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index e234f8311..e52775fcc 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -14,6 +14,7 @@ class PickupObject(ScenarioStateBase):
     pickup_client = None
     tf_listener = None
     grasping_timeout_s = 30.
+    grasping_height_offset = 0.
 
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'pickup_object',
@@ -27,6 +28,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.pickup_server_name = kwargs.get('pickup_server_name', 'pickup_server')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
         self.retry_count = 0
         self.__init_ros_components()
 
@@ -78,7 +80,7 @@ def get_grasping_pose(self, object_to_pick_up):
         pose = Pose()
         pose.position.x = object_to_pick_up.bounding_box.center.x
         pose.position.y = object_to_pick_up.bounding_box.center.y
-        pose.position.z = object_to_pick_up.bounding_box.center.z
+        pose.position.z = object_to_pick_up.bounding_box.center.z + self.grasping_height_offset
 
         gripper_orientation_z = np.arctan2(object_to_pick_up.bounding_box.dimensions.y,
                                            object_to_pick_up.bounding_box.dimensions.x)

From d6c27b4f62e5a9f1d42adb42b8023aa47e7a375b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 01:00:36 +0100
Subject: [PATCH 187/264] [wrs_tidy_up] Added new states; modify existing ones
 to prevent interaction with MongoDB

---
 .../scenario_states/find_objects.py           | 100 +++++++++++++++++
 .../scenario_states/initialise_scenario.py    |  11 +-
 .../scenario_states/move_base.py              |  56 ++++++++++
 .../scenario_states/pickup_object.py          |   4 +-
 .../scenario_states/release_object.py         | 103 ++++++++++++++++++
 5 files changed, 268 insertions(+), 6 deletions(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
new file mode 100644
index 000000000..8fbcba380
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -0,0 +1,100 @@
+import rospy
+import actionlib
+from std_msgs.msg import Bool
+
+from mas_perception_msgs.msg import ObjectList, DetectObjectsAction, DetectObjectsActionGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class FindObjects(ScenarioStateBase):
+    object_detection_server_name = None
+    cloud_obstacle_detection_topic = None
+    cloud_obstacle_cache_reset_topic = None
+    object_detection_client = None
+    obstacle_sub = None
+    detected_cloud_objects = None
+    obstacle_cache_reset_pub = None
+    last_cloud_object_detection_time = None
+    retry_count = 0
+
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'find_objects',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'no_objects',
+                                             'failed', 'failed_after_retrying'],
+                                   input_keys=['floor_objects_cleared',
+                                               'table_objects_cleared',
+                                               'object_location'],
+                                   output_keys=['detected_objects'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'find_objects')
+        self.object_detection_server_name = kwargs.get('object_detection_server_name',
+                                                       '/mas_perception/detect_objects')
+        self.cloud_obstacle_detection_topic = kwargs.get('cloud_obstacle_detection_topic',
+                                                         '/mas_perception/cloud_obstacle_detection/obstacle_objects')
+        self.cloud_obstacle_cache_reset_topic = kwargs.get('cloud_obstacle_cache_reset_topic',
+                                                           '/mas_perception/cloud_obstacle_detection/reset_cache')
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.__init_ros_components()
+
+    def execute(self, userdata):
+        if userdata.object_location == 'floor':
+            last_msg_time = self.last_cloud_object_detection_time
+
+            rospy.loginfo('[%s] Resetting cloud obstacle cache and waiting a bit', self.state_name)
+            self.obstacle_cache_reset_pub.publish(Bool(data=True))
+
+            rospy.loginfo('[%s] Waiting for cloud obstacle detection', self.state_name)
+            while last_msg_time == self.last_cloud_object_detection_time:
+                rospy.sleep(0.05)
+
+            rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
+            userdata.detected_objects = self.detected_cloud_objects
+
+            if not self.detected_cloud_objects:
+                return 'no_objects'
+        else:
+            pass
+
+        return 'succeeded'
+
+    def store_detected_objects(self, object_msg):
+        '''Saves the objects received in the input message and stores the
+        current time as the latest object detection time.
+
+        Keyword arguments:
+        object_msg: mas_perception_msgs.msg.ObjectList -- list of detected objects
+        '''
+        self.detected_cloud_objects = object_msg.objects
+        self.last_cloud_object_detection_time = rospy.Time.now().to_sec()
+
+    def __init_ros_components(self):
+        '''Initialises:
+        * an object detection action client
+        * a subscriber for objects detected by a cloud-based processing component
+        * a publisher for resetting the object cache of the cloud-based processing component
+        '''
+        # we create an action client for an image-based object detector
+        rospy.loginfo('[%s] Initialising client for action %s',
+                      self.state_name, self.object_detection_server_name)
+        self.object_detection_client = actionlib.SimpleActionClient(self.object_detection_server_name,
+                                                                    DetectObjectsAction)
+        rospy.loginfo('[%s] Client for action %s initialised',
+                      self.state_name, self.object_detection_server_name)
+
+        # we create a subscriber for objects detected by a cloud-based processing component
+        rospy.loginfo('[%s] Creating subscriber for topic %s',
+                      self.state_name, self.cloud_obstacle_detection_topic)
+        self.obstacle_sub = rospy.Subscriber(self.cloud_obstacle_detection_topic,
+                                             ObjectList, self.store_detected_objects)
+        rospy.sleep(0.5)
+        rospy.loginfo('[%s] Subscriber for topic %s created',
+                      self.state_name, self.cloud_obstacle_detection_topic)
+
+        # we create a publisher for clearing the object cache of the cloud-based processing component
+        rospy.loginfo('[%s] Creating publisher for topic %s',
+                      self.state_name, self.cloud_obstacle_cache_reset_topic)
+        self.obstacle_cache_reset_pub = rospy.Publisher(self.cloud_obstacle_cache_reset_topic,
+                                                        Bool, queue_size=1)
+        rospy.sleep(0.5)
+        rospy.loginfo('[%s] Publisher for topic %s created',
+                      self.state_name, self.cloud_obstacle_cache_reset_topic)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 8d4dc228c..203b92403 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -51,7 +51,8 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    outcomes=['succeeded'],
                                    output_keys=['floor_objects_cleared',
                                                 'table_objects_cleared',
-                                                'object_location'])
+                                                'object_location',
+                                                'environment_objects'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'initialise_scenario')
         self.floor_objects_cleared = kwargs.get('floor_objects_cleared', None)
@@ -75,7 +76,7 @@ def execute(self, userdata):
 
         # initialising the knowledge base
         rospy.loginfo('[%s] Initialing knowledge base', self.state_name)
-        self.__init_kb(environment_objects)
+        userdata.environment_objects = self.__init_kb(environment_objects, userdata)
         rospy.loginfo('[%s] Knowledge base initialised', self.state_name)
 
         # initialising the MoveIt! planning scene
@@ -95,16 +96,18 @@ def execute(self, userdata):
 
         return 'succeeded'
 
-    def __init_kb(self, object_list):
+    def __init_kb(self, object_list, userdata):
+        kb_objects = dict()
         try:
             for obj in object_list.objects:
                 # we don't add walls and table legs to the knowledge base
                 if obj.name.find('wall') != -1 or obj.name.find('leg') != -1:
                     continue
-                self.kb_interface.insert_obj_instance(obj.name, obj)
+                kb_objects[obj.name] = obj
         except Exception as exc:
             rospy.logerr('[%s] Error while initialising the knowledge base', self.state_name)
             rospy.logerr('[%s] %s', self.state_name, str(exc))
+        return kb_objects
 
     def __init_ros_components(self):
         rospy.loginfo('[%s] Creating a service proxy for %s',
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
new file mode 100644
index 000000000..a31ff909c
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
@@ -0,0 +1,56 @@
+import rospy
+import actionlib
+
+from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class MoveBase(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'move_base',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed',
+                                             'failed_after_retrying'],
+                                   input_keys=['destination_locations'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'move_base')
+        self.action_server_name = kwargs.get('action_server_name', 'move_base_server')
+        self.destination_locations = list(kwargs.get('destination_locations', None))
+        self.timeout = kwargs.get('timeout', 120.)
+        self.debug = kwargs.get('debug', False)
+
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.retry_count = 0
+
+    def execute(self, userdata):
+        if self.destination_locations is None:
+            self.destination_locations = userdata.destination_locations
+            rospy.loginfo("Using userdata's destination_locations %s", self.destination_locations)
+
+        client = actionlib.SimpleActionClient(self.action_server_name, MoveBaseAction)
+        client.wait_for_server()
+        for destination_location in self.destination_locations:
+            goal = MoveBaseGoal()
+            goal.goal_type = MoveBaseGoal.NAMED_TARGET
+            goal.destination_location = destination_location
+
+            client.send_goal(goal)
+            client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+            result = client.get_result()
+            if result:
+                if result.success:
+                    rospy.loginfo('[%s] Successfully moved to %s',
+                                  self.state_name, destination_location)
+                else:
+                    rospy.logerr('[%s] Could not move to %s', self.state_name, destination_location)
+                    if self.retry_count == self.number_of_retries:
+                        rospy.logerr('[%s] Could not move to %s even after retrying; giving up',
+                                     self.state_name, destination_location)
+                        self.retry_count = 0
+                        return 'failed_after_retrying'
+                    self.retry_count += 1
+                    return 'failed'
+            else:
+                rospy.logerr('[%s] Could not move to %s within %f seconds; giving up',
+                             self.state_name, destination_location, self.release_timeout)
+                client.cancel_all_goals()
+        return 'succeeded'
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index e52775fcc..33b7b4e0e 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -49,8 +49,7 @@ def execute(self, userdata):
             pickup_result = self.pickup_client.get_result()
             if pickup_result.success:
                 rospy.loginfo('[%s] Successfully grasped object %s', self.state_name, object_to_pick_up.name)
-                userdata.grasped_object = object_to_pick_up.name
-                self.kb_interface.insert_obj_instance(object_to_pick_up.name, object_to_pick_up)
+                userdata.grasped_object = object_to_pick_up
                 return 'succeeded'
             else:
                 rospy.logerr('[%s] Failed to grasp object', self.state_name)
@@ -61,6 +60,7 @@ def execute(self, userdata):
 
         if self.retry_count == self.number_of_retries:
             rospy.logerr('[%s] Could not pick up object after retrying; giving up', self.state_name)
+            self.retry_count = 0
             return 'failed_after_retrying'
 
         rospy.loginfo('[%s] Retrying to place object', self.state_name)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
new file mode 100644
index 000000000..aa8a41af9
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
@@ -0,0 +1,103 @@
+import numpy as np
+import tf
+import rospy
+import actionlib
+
+from std_msgs.msg import Header
+from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped
+
+from action_execution.action import Action
+from ae_ros_message_converter.converter_factory import ConverterFactory
+
+from mdr_place_action.msg import PlaceAction, PlaceGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class ReleaseObject(ScenarioStateBase):
+    tf_listener = None
+
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'release_object',
+                                   save_sm_state=save_sm_state,
+                                   input_keys=['grasped_object',
+                                               'environment_objects'],
+                                   outcomes=['succeeded', 'failed',
+                                             'failed_after_retrying'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'release_object')
+        self.action_server_name = kwargs.get('action_server_name', 'place_server')
+        self.release_timeout = kwargs.get('release_timeout', 120.)
+        self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.release_target_name = kwargs.get('release_target_name', 'bin_a')
+        self.retry_count = 0
+        self.tf_listener = tf.TransformListener()
+
+    def execute(self, userdata):
+        release_target = userdata.environment_objects[self.release_target_name]
+
+        goal = PlaceGoal()
+        goal.pose = self.get_release_pose(release_target, userdata.grasped_object)
+        goal.release_on_impact = False
+
+        client = actionlib.SimpleActionClient(self.action_server_name, PlaceAction)
+        client.wait_for_server()
+        client.send_goal(goal)
+        client.wait_for_result(rospy.Duration.from_sec(int(self.release_timeout)))
+        result = client.get_result()
+        if result:
+            if result.success:
+                rospy.loginfo('[%s] Successfully released object', self.state_name)
+                return 'succeeded'
+            else:
+                rospy.logerr('[%s] Could not release object', self.state_name)
+        else:
+            rospy.logerr('[%s] Object not released within %f seconds; giving up',
+                         self.state_name, self.release_timeout)
+            client.cancel_all_goals()
+
+        if self.retry_count == self.number_of_retries:
+            rospy.logerr('[%s] Could not release object after retrying; giving up', self.state_name)
+            self.retry_count = 0
+            return 'failed_after_retrying'
+
+        rospy.loginfo('[%s] Retrying to release object', self.state_name)
+        self.retry_count += 1
+        return 'succeeded'
+
+    def get_release_pose(self, release_target, grasped_object):
+        release_target_obj = ConverterFactory.convert_ros_msg(release_target)
+        manipulated_obj = ConverterFactory.convert_ros_msg(grasped_object)
+
+        action_model = Action(action_name='Throw')
+        number_of_samples = 10
+
+        base_link_origin_pose = PoseStamped(Header(1, rospy.Time(0), 'base_link'),
+                                            Pose(Point(0., 0., 0.), Quaternion(0., 0., 0., 1.)))
+        robot_pose = self.tf_listener.transformPose(release_target.pose.header.frame_id,
+                                                    base_link_origin_pose)
+        robot_pose = ConverterFactory.convert_ros_msg(robot_pose)
+        model_results = action_model.get_execution_guidelines(data_count=number_of_samples,
+                                                              frame_id=release_target.pose.header.frame_id,
+                                                              manipulated_object=manipulated_obj,
+                                                              target_object=release_target_obj,
+                                                              robot_pose=robot_pose)
+        candidate_pose_idx = np.argmax(model_results['success_probabilities'])
+        candidate_pose = model_results['candidate_poses'][candidate_pose_idx]
+        candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose)
+
+        candidate_pose.header.stamp = rospy.Time(0)
+        candidate_pose.pose.position.z += 0.1
+
+        # use top-down orientation for throwing
+        gripper_pose = PoseStamped()
+        gripper_pose.header.frame_id = 'base_link'
+        gripper_pose.header.stamp = rospy.Time(0)
+
+        gripper_quaternion = tf.transformations.quaternion_from_euler(np.pi, 0., 0.)
+        gripper_pose.pose.orientation.x = gripper_quaternion[0]
+        gripper_pose.pose.orientation.y = gripper_quaternion[1]
+        gripper_pose.pose.orientation.z = gripper_quaternion[2]
+        gripper_pose.pose.orientation.w = gripper_quaternion[3]
+
+        gripper_pose_in_target_frame = self.tf_listener.transformPose(release_target.pose.header.frame_id, gripper_pose)
+        candidate_pose.pose.orientation = gripper_pose_in_target_frame.pose.orientation
+        return candidate_pose

From e1dcf027ae1f5be6cabab2085391821041298b09 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 01:01:27 +0100
Subject: [PATCH 188/264] [wrs_tidy_up/config] Commented out states for T1;
 updated state modules with new states

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 383 ++++++++++--------
 1 file changed, 217 insertions(+), 166 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 40675c54e..49330cf19 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -2,167 +2,185 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_wrs_tidy_up
-states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE]
+states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE, MOVE_TO_CUPBOARD]
 outcomes: [DONE, TIMEOUT, FAILED]
 state_descriptions:
-    # Task 1: Cleaning up objects
-    - state:
-        name: INITIALISE_SCENARIO
-        state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
-        state_class_name: InitialiseScenario
-        transitions:
-            - transition:
-                name: succeeded
-                state: SELECT_SCANNING_POSE
-        arguments:
-            - argument:
-                name: floor_objects_cleared
-                value:
-                    table_inspection_area1: false
-                    table_inspection_area2: false
-                    table_inspection_area3: false
-            - argument:
-                name: table_objects_cleared
-                value:
-                    long_table_b: false
-                    tall_table: false
-    - state:
-        name: CHECK_TIDYING_DONE
-        state_module_name: mdr_wrs_tidy_up.scenario_states.check_tidying_done
-        state_class_name: CheckTidyingDone
-        transitions:
-            - transition:
-                name: room_not_clear
-                state: SELECT_SCANNING_POSE
-            - transition:
-                name: room_clear
-                state: INITIALISE_OBSTACLE_REMOVAL
-    - state:
-        name: SELECT_SCANNING_POSE
-        state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
-        state_class_name: SelectScanningPose
-        transitions:
-            - transition:
-                name: succeeded
-                state: GO_TO_SCANNING_POSE
-            - transition:
-                name: failed
-                state: SELECT_SCANNING_POSE
-        arguments:
-            - argument:
-                name: floor_scanning_poses
-                value: [table_inspection_area1, table_inspection_area2, table_inspection_area3]
-            - argument:
-                name: table_scanning_poses
-                value: [long_table_b, tall_table]
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: GO_TO_SCANNING_POSE
-        state_module_name: mdr_navigation_behaviours.move_base
-        state_class_name: MoveBase
-        transitions:
-            - transition:
-                name: succeeded
-                state: FIND_OBJECTS
-            - transition:
-                name: failed
-                state: GO_TO_SCANNING_POSE
-            - transition:
-                name: failed_after_retrying
-                state: SELECT_SCANNING_POSE
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: FIND_OBJECTS
-        state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
-        state_class_name: FindObjects
-        transitions:
-            - transition:
-                name: succeeded
-                state: SELECT_OBJECT_FOR_PICKING
-            - transition:
-                name: no_obstacles
-                state: SELECT_SCANNING_POSE
-            - transition:
-                name: failed
-                state: FIND_OBJECTS
-            - transition:
-                name: failed_after_retrying
-                state: SELECT_SCANNING_POSE
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: SELECT_OBJECT_FOR_PICKING
-        state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
-        state_class_name: SelectObjectForPicking
-        transitions:
-            - transition:
-                name: succeeded
-                state: PICKUP_OBJECT
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: PICKUP_OBJECT
-        state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
-        state_class_name: PickupObject
-        transitions:
-            - transition:
-                name: succeeded
-                state: MOVE_TO_STORAGE_LOCATION
-            - transition:
-                name: failed
-                state: PICKUP_OBJECT
-            - transition:
-                name: failed_after_retrying
-                state: GO_TO_SCANNING_POSE
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: MOVE_TO_STORAGE_LOCATION
-        state_module_name: mdr_navigation_behaviours.move_base
-        state_class_name: MoveBase
-        transitions:
-            - transition:
-                name: succeeded
-                state: THROW_OBJECT
-            - transition:
-                name: failed
-                state: MOVE_TO_STORAGE_LOCATION
-            - transition:
-                name: failed_after_retrying
-                state: MOVE_TO_STORAGE_LOCATION
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
-    - state:
-        name: THROW_OBJECT
-        state_module_name: mdr_manipulation_behaviours.throw_object_in
-        state_class_name: ThrowObjectIn
-        transitions:
-            - transition:
-                name: succeeded
-                state: CHECK_TIDYING_DONE
-            - transition:
-                name: failed
-                state: THROW_OBJECT
-            - transition:
-                name: failed_after_retrying
-                state: CHECK_TIDYING_DONE
-        arguments:
-            - argument:
-                name: number_of_retries
-                value: 3
+    # # Task 1: Cleaning up objects
+    # - state:
+    #     name: INITIALISE_SCENARIO
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
+    #     state_class_name: InitialiseScenario
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: SELECT_SCANNING_POSE
+    #     arguments:
+    #         - argument:
+    #             name: floor_objects_cleared
+    #             value:
+    #                 table_inspection_area1: false
+    #                 table_inspection_area2: false
+    #                 table_inspection_area3: false
+    #         - argument:
+    #             name: table_objects_cleared
+    #             value:
+    #                 long_table_b: false
+    #                 tall_table: false
+    #         - argument:
+    #             name: planning_scene_map_file
+    #             value: planning_scene_map.yaml
+    # - state:
+    #     name: CHECK_TIDYING_DONE
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.check_tidying_done
+    #     state_class_name: CheckTidyingDone
+    #     transitions:
+    #         - transition:
+    #             name: room_not_clear
+    #             state: SELECT_SCANNING_POSE
+    #         - transition:
+    #             name: room_clear
+    #             state: INITIALISE_OBSTACLE_REMOVAL
+    # - state:
+    #     name: SELECT_SCANNING_POSE
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
+    #     state_class_name: SelectScanningPose
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: GO_TO_SCANNING_POSE
+    #         - transition:
+    #             name: failed
+    #             state: SELECT_SCANNING_POSE
+    #     arguments:
+    #         - argument:
+    #             name: floor_scanning_poses
+    #             value: [table_inspection_area1, table_inspection_area2, table_inspection_area3]
+    #         - argument:
+    #             name: table_scanning_poses
+    #             value: [long_table_b, tall_table]
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: GO_TO_SCANNING_POSE
+    #     state_module_name: mdr_navigation_behaviours.move_base
+    #     state_class_name: MoveBase
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: FIND_OBJECTS
+    #         - transition:
+    #             name: failed
+    #             state: GO_TO_SCANNING_POSE
+    #         - transition:
+    #             name: failed_after_retrying
+    #             state: SELECT_SCANNING_POSE
+    #     arguments:
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: FIND_OBJECTS
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
+    #     state_class_name: FindObjects
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: SELECT_OBJECT_FOR_PICKING
+    #         - transition:
+    #             name: no_objects
+    #             state: SELECT_SCANNING_POSE
+    #         - transition:
+    #             name: failed
+    #             state: FIND_OBJECTS
+    #         - transition:
+    #             name: failed_after_retrying
+    #             state: SELECT_SCANNING_POSE
+    #     arguments:
+    #         - argument:
+    #             name: object_detection_server_name
+    #             value: /mas_perception/detect_objects
+    #         - argument:
+    #             name: cloud_obstacle_detection_topic
+    #             value: /mas_perception/cloud_obstacle_detection/obstacle_objects
+    #         - argument:
+    #             name: cloud_obstacle_cache_reset_topic
+    #             value: /mas_perception/cloud_obstacle_detection/reset_cache
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: SELECT_OBJECT_FOR_PICKING
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
+    #     state_class_name: SelectObjectForPicking
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: PICKUP_OBJECT
+    #     arguments:
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: PICKUP_OBJECT
+    #     state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
+    #     state_class_name: PickupObject
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: MOVE_TO_STORAGE_LOCATION
+    #         - transition:
+    #             name: failed
+    #             state: PICKUP_OBJECT
+    #         - transition:
+    #             name: failed_after_retrying
+    #             state: GO_TO_SCANNING_POSE
+    #     arguments:
+    #         - argument:
+    #             name: pickup_server_name
+    #             value: pickup_server
+    #         - argument:
+    #             name: grasping_timeout_s
+    #             value: 30.0
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: MOVE_TO_STORAGE_LOCATION
+    #     state_module_name: mdr_navigation_behaviours.move_base
+    #     state_class_name: MoveBase
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: THROW_OBJECT
+    #         - transition:
+    #             name: failed
+    #             state: MOVE_TO_STORAGE_LOCATION
+    #         - transition:
+    #             name: failed_after_retrying
+    #             state: MOVE_TO_STORAGE_LOCATION
+    #     arguments:
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
+    # - state:
+    #     name: THROW_OBJECT
+    #     state_module_name: mdr_manipulation_behaviours.throw_object_in
+    #     state_class_name: ThrowObjectIn
+    #     transitions:
+    #         - transition:
+    #             name: succeeded
+    #             state: CHECK_TIDYING_DONE
+    #         - transition:
+    #             name: failed
+    #             state: THROW_OBJECT
+    #         - transition:
+    #             name: failed_after_retrying
+    #             state: CHECK_TIDYING_DONE
+    #     arguments:
+    #         - argument:
+    #             name: number_of_retries
+    #             value: 3
 
     # Task 2a: Going to the second room
     - state:
@@ -174,12 +192,15 @@ state_descriptions:
                 name: succeeded
                 state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
+            - argument:
+                name: planning_scene_map_file
+                value: planning_scene_map.yaml
             - argument:
                 name: object_location
                 value: floor
     - state:
         name: GO_TO_SECOND_ROOM_CORRIDOR
-        state_module_name: mdr_navigation_behaviours.move_base
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
         state_class_name: MoveBase
         transitions:
             - transition:
@@ -207,8 +228,8 @@ state_descriptions:
                 name: succeeded
                 state: SELECT_OBSTACLE_FOR_PICKING
             - transition:
-                name: no_obstacles
-                state: DONE
+                name: no_objects
+                state: MOVE_TO_CUPBOARD
             - transition:
                 name: failed
                 state: GO_TO_SECOND_ROOM_CORRIDOR
@@ -222,6 +243,9 @@ state_descriptions:
             - argument:
                 name: cloud_obstacle_detection_topic
                 value: /mas_perception/cloud_obstacle_detection/obstacle_objects
+            - argument:
+                name: cloud_obstacle_cache_reset_topic
+                value: /mas_perception/cloud_obstacle_detection/reset_cache
             - argument:
                 name: number_of_retries
                 value: 3
@@ -257,7 +281,7 @@ state_descriptions:
                 value: 3
     - state:
         name: MOVE_TO_BOX
-        state_module_name: mdr_navigation_behaviours.move_base
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
         state_class_name: MoveBase
         transitions:
             - transition:
@@ -270,13 +294,16 @@ state_descriptions:
                 name: failed_after_retrying
                 state: MOVE_TO_BOX
         arguments:
+            - argument:
+                name: destination_locations
+                value: [bin_a]
             - argument:
                 name: number_of_retries
                 value: 3
     - state:
         name: THROW_OBSTACLE
-        state_module_name: mdr_manipulation_behaviours.throw_object_in
-        state_class_name: ThrowObjectIn
+        state_module_name: mdr_wrs_tidy_up.scenario_states.release_object
+        state_class_name: ReleaseObject
         transitions:
             - transition:
                 name: succeeded
@@ -288,8 +315,32 @@ state_descriptions:
                 name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
+            - argument:
+                name: release_target_name
+                value: bin_a
             - argument:
                 name: number_of_retries
                 value: 3
 
-    # TODO: Add states for Task 2b: Delivering an object to a person
+    # TODO: Task 2b: Delivering an object to a person
+    - state:
+        name: MOVE_TO_CUPBOARD
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: DONE
+            - transition:
+                name: failed
+                state: MOVE_TO_CUPBOARD
+            - transition:
+                name: failed_after_retrying
+                state: MOVE_TO_CUPBOARD
+        arguments:
+            - argument:
+                name: destination_locations
+                value: [second_room_cupboard]
+            - argument:
+                name: number_of_retries
+                value: 3

From 5c362e87dcee5d91155329a892144e775d003f1e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 13:26:10 +0100
Subject: [PATCH 189/264] [wrs_tidy_up/states/move_base] Fixed undefined var
 error

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
index a31ff909c..6c5b9271b 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
@@ -51,6 +51,6 @@ def execute(self, userdata):
                     return 'failed'
             else:
                 rospy.logerr('[%s] Could not move to %s within %f seconds; giving up',
-                             self.state_name, destination_location, self.release_timeout)
+                             self.state_name, destination_location, self.timeout)
                 client.cancel_all_goals()
         return 'succeeded'

From 86b760cc1a717d4f8c81c1816736360bd5464d9d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 22:33:16 +0100
Subject: [PATCH 190/264] [wrs_tidy_up] Added state for selecting a scanning
 pose (Task 1)

---
 .../scenario_states/select_scanning_pose.py   | 38 +++++++++++++++++++
 1 file changed, 38 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
new file mode 100644
index 000000000..65994e363
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
@@ -0,0 +1,38 @@
+import random
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+def get_dict_keys_with_false_values(d):
+    keys_with_false_values = []
+    for k, v in d.items():
+        if not v:
+            keys_with_false_values.append(k)
+    return keys_with_false_values
+
+class SelectScanningPose(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'select_scanning_pose',
+                                   save_sm_state=save_sm_state,
+                                   input_keys=['floor_objects_cleared',
+                                               'table_objects_cleared'],
+                                   output_keys=['destination_locations',
+                                                'object_location'],
+                                   outcomes=['floor_not_cleared',
+                                             'table_not_cleared',
+                                             'tidying_done'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'select_scanning_pose')
+
+    def execute(self, userdata):
+        dirty_locations = []
+        if False in userdata.floor_objects_cleared.values():
+            dirty_locations = get_dict_keys_with_false_values(userdata.floor_objects_cleared)
+            userdata.object_location = 'floor'
+            userdata.destination_locations = [random.choice(dirty_locations)]
+            return 'floor_not_cleared'
+        elif False in userdata.table_objects_cleared.values():
+            dirty_locations = get_dict_keys_with_false_values(userdata.table_objects_cleared)
+            userdata.object_location = 'table'
+            userdata.destination_locations = [random.choice(dirty_locations)]
+            return 'table_not_cleared'
+        else:
+            return 'tidying_done'

From af8c76c1e04a630334e58a3e732b44bfcd030475 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 22:55:33 +0100
Subject: [PATCH 191/264] [wrs_tidy_up/states/move_base] Don't convert
 potentially None argument to list

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
index 6c5b9271b..f34d9ec28 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
@@ -14,7 +14,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'move_base')
         self.action_server_name = kwargs.get('action_server_name', 'move_base_server')
-        self.destination_locations = list(kwargs.get('destination_locations', None))
+        self.destination_locations = kwargs.get('destination_locations', None)
         self.timeout = kwargs.get('timeout', 120.)
         self.debug = kwargs.get('debug', False)
 

From d6fee4493f4568afe079deac96c30eff61077c64 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 22:56:23 +0100
Subject: [PATCH 192/264] [wrs_tidy_up/states/init_scenario] Convert args to
 dict before adding them to the userdata

They otherwise had the type "OrderedDict", which is then stored as
smach.user_data.Const
---
 .../mdr_wrs_tidy_up/scenario_states/initialise_scenario.py    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 203b92403..233067d85 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -64,8 +64,8 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.__init_ros_components()
 
     def execute(self, userdata):
-        userdata.floor_objects_cleared = self.floor_objects_cleared
-        userdata.table_objects_cleared = self.table_objects_cleared
+        userdata.floor_objects_cleared = dict(self.floor_objects_cleared)
+        userdata.table_objects_cleared = dict(self.table_objects_cleared)
         userdata.object_location = self.object_location
 
         if not self.planning_scene_map_file:

From 223f7bfedd849aaef16b4f927f7a13b0bd7ba826 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 22:58:26 +0100
Subject: [PATCH 193/264] [wrs_tidy_up/config] The basic workflow for task 1 is
 now there

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 336 +++++++++---------
 1 file changed, 159 insertions(+), 177 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 49330cf19..bd934e2e2 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -2,185 +2,167 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_wrs_tidy_up
-states: [INITIALISE_SCENARIO, CHECK_TIDYING_DONE, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, THROW_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE, MOVE_TO_CUPBOARD]
+states: [INITIALISE_SCENARIO, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, RELEASE_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE, MOVE_TO_CUPBOARD]
 outcomes: [DONE, TIMEOUT, FAILED]
 state_descriptions:
-    # # Task 1: Cleaning up objects
-    # - state:
-    #     name: INITIALISE_SCENARIO
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
-    #     state_class_name: InitialiseScenario
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: SELECT_SCANNING_POSE
-    #     arguments:
-    #         - argument:
-    #             name: floor_objects_cleared
-    #             value:
-    #                 table_inspection_area1: false
-    #                 table_inspection_area2: false
-    #                 table_inspection_area3: false
-    #         - argument:
-    #             name: table_objects_cleared
-    #             value:
-    #                 long_table_b: false
-    #                 tall_table: false
-    #         - argument:
-    #             name: planning_scene_map_file
-    #             value: planning_scene_map.yaml
-    # - state:
-    #     name: CHECK_TIDYING_DONE
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.check_tidying_done
-    #     state_class_name: CheckTidyingDone
-    #     transitions:
-    #         - transition:
-    #             name: room_not_clear
-    #             state: SELECT_SCANNING_POSE
-    #         - transition:
-    #             name: room_clear
-    #             state: INITIALISE_OBSTACLE_REMOVAL
-    # - state:
-    #     name: SELECT_SCANNING_POSE
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
-    #     state_class_name: SelectScanningPose
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: GO_TO_SCANNING_POSE
-    #         - transition:
-    #             name: failed
-    #             state: SELECT_SCANNING_POSE
-    #     arguments:
-    #         - argument:
-    #             name: floor_scanning_poses
-    #             value: [table_inspection_area1, table_inspection_area2, table_inspection_area3]
-    #         - argument:
-    #             name: table_scanning_poses
-    #             value: [long_table_b, tall_table]
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: GO_TO_SCANNING_POSE
-    #     state_module_name: mdr_navigation_behaviours.move_base
-    #     state_class_name: MoveBase
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: FIND_OBJECTS
-    #         - transition:
-    #             name: failed
-    #             state: GO_TO_SCANNING_POSE
-    #         - transition:
-    #             name: failed_after_retrying
-    #             state: SELECT_SCANNING_POSE
-    #     arguments:
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: FIND_OBJECTS
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
-    #     state_class_name: FindObjects
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: SELECT_OBJECT_FOR_PICKING
-    #         - transition:
-    #             name: no_objects
-    #             state: SELECT_SCANNING_POSE
-    #         - transition:
-    #             name: failed
-    #             state: FIND_OBJECTS
-    #         - transition:
-    #             name: failed_after_retrying
-    #             state: SELECT_SCANNING_POSE
-    #     arguments:
-    #         - argument:
-    #             name: object_detection_server_name
-    #             value: /mas_perception/detect_objects
-    #         - argument:
-    #             name: cloud_obstacle_detection_topic
-    #             value: /mas_perception/cloud_obstacle_detection/obstacle_objects
-    #         - argument:
-    #             name: cloud_obstacle_cache_reset_topic
-    #             value: /mas_perception/cloud_obstacle_detection/reset_cache
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: SELECT_OBJECT_FOR_PICKING
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
-    #     state_class_name: SelectObjectForPicking
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: PICKUP_OBJECT
-    #     arguments:
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: PICKUP_OBJECT
-    #     state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
-    #     state_class_name: PickupObject
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: MOVE_TO_STORAGE_LOCATION
-    #         - transition:
-    #             name: failed
-    #             state: PICKUP_OBJECT
-    #         - transition:
-    #             name: failed_after_retrying
-    #             state: GO_TO_SCANNING_POSE
-    #     arguments:
-    #         - argument:
-    #             name: pickup_server_name
-    #             value: pickup_server
-    #         - argument:
-    #             name: grasping_timeout_s
-    #             value: 30.0
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: MOVE_TO_STORAGE_LOCATION
-    #     state_module_name: mdr_navigation_behaviours.move_base
-    #     state_class_name: MoveBase
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: THROW_OBJECT
-    #         - transition:
-    #             name: failed
-    #             state: MOVE_TO_STORAGE_LOCATION
-    #         - transition:
-    #             name: failed_after_retrying
-    #             state: MOVE_TO_STORAGE_LOCATION
-    #     arguments:
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
-    # - state:
-    #     name: THROW_OBJECT
-    #     state_module_name: mdr_manipulation_behaviours.throw_object_in
-    #     state_class_name: ThrowObjectIn
-    #     transitions:
-    #         - transition:
-    #             name: succeeded
-    #             state: CHECK_TIDYING_DONE
-    #         - transition:
-    #             name: failed
-    #             state: THROW_OBJECT
-    #         - transition:
-    #             name: failed_after_retrying
-    #             state: CHECK_TIDYING_DONE
-    #     arguments:
-    #         - argument:
-    #             name: number_of_retries
-    #             value: 3
+    # Task 1: Cleaning up objects
+    - state:
+        name: INITIALISE_SCENARIO
+        state_module_name: mdr_wrs_tidy_up.scenario_states.initialise_scenario
+        state_class_name: InitialiseScenario
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: floor_objects_cleared
+                value:
+                    table_inspection_area1: false
+                    table_inspection_area2: false
+                    table_inspection_area3: false
+            - argument:
+                name: table_objects_cleared
+                value:
+                    long_table_b: false
+                    tall_table: false
+            - argument:
+                name: planning_scene_map_file
+                value: planning_scene_map.yaml
+    - state:
+        name: SELECT_SCANNING_POSE
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_scanning_pose
+        state_class_name: SelectScanningPose
+        transitions:
+            - transition:
+                name: floor_not_cleared
+                state: GO_TO_SCANNING_POSE
+            - transition:
+                name: table_not_cleared
+                state: GO_TO_SCANNING_POSE
+            - transition:
+                name: tidying_done
+                state: INITIALISE_OBSTACLE_REMOVAL
+    - state:
+        name: GO_TO_SCANNING_POSE
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: FIND_OBJECTS
+            - transition:
+                name: failed
+                state: GO_TO_SCANNING_POSE
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: FIND_OBJECTS
+        state_module_name: mdr_wrs_tidy_up.scenario_states.find_objects
+        state_class_name: FindObjects
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_OBJECT_FOR_PICKING
+            - transition:
+                name: no_objects
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: failed
+                state: FIND_OBJECTS
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: object_detection_server_name
+                value: /mas_perception/detect_objects
+            - argument:
+                name: cloud_obstacle_detection_topic
+                value: /mas_perception/cloud_obstacle_detection/obstacle_objects
+            - argument:
+                name: cloud_obstacle_cache_reset_topic
+                value: /mas_perception/cloud_obstacle_detection/reset_cache
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: SELECT_OBJECT_FOR_PICKING
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_object_for_picking
+        state_class_name: SelectObjectForPicking
+        transitions:
+            - transition:
+                name: succeeded
+                state: PICKUP_OBJECT
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: PICKUP_OBJECT
+        state_module_name: mdr_wrs_tidy_up.scenario_states.pickup_object
+        state_class_name: PickupObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: MOVE_TO_STORAGE_LOCATION
+            - transition:
+                name: failed
+                state: PICKUP_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: GO_TO_SCANNING_POSE
+        arguments:
+            - argument:
+                name: pickup_server_name
+                value: pickup_server
+            - argument:
+                name: grasping_timeout_s
+                value: 30.0
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: MOVE_TO_STORAGE_LOCATION
+        state_module_name: mdr_navigation_behaviours.move_base
+        state_class_name: MoveBase
+        transitions:
+            - transition:
+                name: succeeded
+                state: RELEASE_OBJECT
+            - transition:
+                name: failed
+                state: MOVE_TO_STORAGE_LOCATION
+            - transition:
+                name: failed_after_retrying
+                state: MOVE_TO_STORAGE_LOCATION
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
+    - state:
+        name: RELEASE_OBJECT
+        state_module_name: mdr_wrs_tidy_up.scenario_states.release_object
+        state_class_name: ReleaseObject
+        transitions:
+            - transition:
+                name: succeeded
+                state: SELECT_SCANNING_POSE
+            - transition:
+                name: failed
+                state: RELEASE_OBJECT
+            - transition:
+                name: failed_after_retrying
+                state: SELECT_SCANNING_POSE
+        arguments:
+            - argument:
+                name: number_of_retries
+                value: 3
 
     # Task 2a: Going to the second room
     - state:

From 5feb2c5d8eeb370f9f21accd71f465c2b0bcd870 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 10 Dec 2020 23:03:30 +0100
Subject: [PATCH 194/264] [wrs_tidy_up/config] Removed one remaining reference
 to mdr_behaviours (use move_base from scenario instead)

---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index bd934e2e2..785772f12 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -129,7 +129,7 @@ state_descriptions:
                 value: 3
     - state:
         name: MOVE_TO_STORAGE_LOCATION
-        state_module_name: mdr_navigation_behaviours.move_base
+        state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
         state_class_name: MoveBase
         transitions:
             - transition:

From 97f6658b42637dd6877705d0592ba6354a59db73 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 11 Dec 2020 15:03:02 +0100
Subject: [PATCH 195/264] [wrs_tidy_up] Added state for selecting a storage
 location

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 35 ++++++++++++-
 .../select_storage_location.py                | 50 +++++++++++++++++++
 2 files changed, 83 insertions(+), 2 deletions(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 785772f12..2903fa043 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -2,7 +2,7 @@
 # Author: Alex Mitrevski
 # Email: aleksandar.mitrevski@h-brs.de
 sm_id: mdr_wrs_tidy_up
-states: [INITIALISE_SCENARIO, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, MOVE_TO_STORAGE_LOCATION, RELEASE_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE, MOVE_TO_CUPBOARD]
+states: [INITIALISE_SCENARIO, SELECT_SCANNING_POSE, GO_TO_SCANNING_POSE, FIND_OBJECTS, SELECT_OBJECT_FOR_PICKING, PICKUP_OBJECT, SELECT_STORAGE_LOCATION, MOVE_TO_STORAGE_LOCATION, RELEASE_OBJECT, INITIALISE_OBSTACLE_REMOVAL, GO_TO_SECOND_ROOM_CORRIDOR, FIND_FLOOR_OBSTACLES, SELECT_OBSTACLE_FOR_PICKING, PICKUP_OBSTACLE, MOVE_TO_BOX, THROW_OBSTACLE, MOVE_TO_CUPBOARD]
 outcomes: [DONE, TIMEOUT, FAILED]
 state_descriptions:
     # Task 1: Cleaning up objects
@@ -110,7 +110,7 @@ state_descriptions:
         transitions:
             - transition:
                 name: succeeded
-                state: MOVE_TO_STORAGE_LOCATION
+                state: SELECT_STORAGE_LOCATION
             - transition:
                 name: failed
                 state: PICKUP_OBJECT
@@ -127,6 +127,37 @@ state_descriptions:
             - argument:
                 name: number_of_retries
                 value: 3
+    - state:
+        name: SELECT_STORAGE_LOCATION
+        state_module_name: mdr_wrs_tidy_up.scenario_states.select_storage_location
+        state_class_name: SelectStorageLocation
+        transitions:
+            - transition:
+                name: succeeded
+                state: MOVE_TO_STORAGE_LOCATION
+        arguments:
+            - argument:
+                name: default_storage_location
+                value: bin
+            - argument:
+                name: storage_locations_file
+                value: storage_locations.yaml
+            - argument:
+                name: available_trays
+                value: ['tray_1', 'tray_2']
+            - argument:
+                name: available_bins
+                value: ['bin_a', 'bin_b']
+            - argument:
+                name: tray_locations
+                value:
+                    tray_1: long_table_a_trays
+                    tray_2: long_table_a_trays
+            - argument:
+                name: bin_locations
+                value:
+                    bin_a: bin_a
+                    bin_b: bin_b
     - state:
         name: MOVE_TO_STORAGE_LOCATION
         state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
new file mode 100644
index 000000000..713664901
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
@@ -0,0 +1,50 @@
+import random
+import rospy
+from mas_tools.ros_utils import get_package_path
+from mas_tools.file_utils import load_yaml_file
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class SelectStorageLocation(ScenarioStateBase):
+    storage_locations_map = None
+
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'select_storage_location',
+                                   save_sm_state=save_sm_state,
+                                   input_keys=['grasped_object'],
+                                   output_keys=['destination_locations',
+                                                'storage_location'],
+                                   outcomes=['succeeded'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'select_storage_location')
+        self.storage_locations_file = kwargs.get('storage_locations_file', 'storage_location.yaml')
+
+        self.available_trays = kwargs.get('available_trays', ['tray_1', 'tray_2'])
+        self.tray_locations = kwargs.get('tray_locations', {'tray_1': 'long_table_a_trays',
+                                                            'tray_2': 'long_table_a_trays'})
+        self.available_bins = kwargs.get('available_bins', ['bin_a', 'bin_b'])
+        self.bin_locations = kwargs.get('bin_locations', {'bin_a': 'bin_a',
+                                                          'bin_b': 'bin_b'})
+        self.default_storage_location = kwargs.get('default_storage_location', 'bin')
+
+        storage_locations_path = get_package_path('mdr_wrs_tidy_up', 'config', self.storage_locations_file)
+        if not self.storage_locations_file:
+            raise ValueError('[{0}] Argument "storage_locations_file" cannot be empty'.format(self.state_name))
+        self.storage_locations_map = load_yaml_file(storage_locations_path)
+
+    def execute(self, userdata):
+        obj_category = userdata.grasped_object.category
+        storage_location = self.default_storage_location
+        if obj_category and obj_category in self.storage_locations_map:
+            storage_location = self.storage_locations_map[obj_category]
+
+        if 'table' in storage_location:
+            storage_location = random.choice(self.available_trays)
+            userdata.storage_location = storage_location
+            userdata.destination_locations = [self.tray_locations[storage_location]]
+        else:
+            storage_location = random.choice(self.available_bins)
+            userdata.storage_location = storage_location
+            userdata.destination_locations = [self.bin_locations[storage_location]]
+            rospy.logwarn('STORAGE LOCATION %s', storage_location)
+            rospy.logwarn([self.bin_locations[storage_location]])
+        return 'succeeded'

From 6c7a286dc21212f03533a23b2cf370da7890fa2a Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 11 Dec 2020 15:10:51 +0100
Subject: [PATCH 196/264] [wrs_tidy_up/states/init_scenario] Use mas_tools fn
 for reading YAML file

---
 .../mdr_wrs_tidy_up/scenario_states/initialise_scenario.py   | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 233067d85..1f967c606 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -8,6 +8,7 @@
 from mdr_manipulation_msgs.srv import UpdatePlanningScene, UpdatePlanningSceneRequest
 
 from mas_tools.ros_utils import get_package_path
+from mas_tools.file_utils import load_yaml_file
 
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
@@ -15,9 +16,7 @@ def get_environment_objects(planning_scene_map_file):
     object_list = ObjectList()
 
     scene_file_path = get_package_path('mdr_wrs_tidy_up', 'config', planning_scene_map_file)
-    scene_data = None
-    with open(scene_file_path, 'r') as planning_scene_map:
-        scene_data = yaml.safe_load(planning_scene_map)
+    scene_data = load_yaml_file(scene_file_path)
 
     frame_id = scene_data['frame_id']
     for obj_data in scene_data['objects']:

From 28e224a6f49377644a5a1c572384a67bffb68ddc Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 11 Dec 2020 15:12:16 +0100
Subject: [PATCH 197/264] [wrs_tidy_up/states/pickup_object] Added sideways
 grasp for tall objects

---
 .../mdr_wrs_tidy_up/scenario_states/pickup_object.py  | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index 33b7b4e0e..b6873e96f 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -80,14 +80,21 @@ def get_grasping_pose(self, object_to_pick_up):
         pose = Pose()
         pose.position.x = object_to_pick_up.bounding_box.center.x
         pose.position.y = object_to_pick_up.bounding_box.center.y
-        pose.position.z = object_to_pick_up.bounding_box.center.z + self.grasping_height_offset
+        pose.position.z = object_to_pick_up.bounding_box.center.z
 
         gripper_orientation_z = np.arctan2(object_to_pick_up.bounding_box.dimensions.y,
                                            object_to_pick_up.bounding_box.dimensions.x)
 
+        # this orientation guarantees a sideways grasp and
+        # alignment along the longest axis of the object
+        if object_to_pick_up.dimensions.vector.z > max(object_to_pick_up.dimensions.vector.x,
+                                                       object_to_pick_up.dimensions.vector.y):
+            desired_gripper_orientation_base_link = (np.pi, -np.pi/2, 0.)
         # this orientation guarantees a top-down grasp and
         # alignment along the longest axis of the object
-        desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
+        else:
+            desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
+            pose.position.z += self.grasping_height_offset
 
         pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
                                                         object_to_pick_up.pose.header.frame_id)

From 99558d2b5a8c06beea2c2219745057a2481d5400 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 11 Dec 2020 15:40:04 +0100
Subject: [PATCH 198/264] [.travis.yml] Added pip upgrade command

---
 .travis.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.travis.yml b/.travis.yml
index 9783fb036..3fa835206 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -11,6 +11,7 @@ env:
     - CONTAINER_RELEASE_IMAGE: bitbots/bitbots-domestic:$TRAVIS_BRANCH
 
 before_install:
+  - sudo pip install --upgrade pip
   - sudo rm /usr/local/bin/docker-compose
   - sudo curl -L https://github.com/docker/compose/releases/download/1.22.0/docker-compose-$(uname -s)-$(uname -m) -o /usr/local/bin/docker-compose
   - sudo chmod +x /usr/local/bin/docker-compose

From 50477835e17262b9aea28784e8bdf732c4595986 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 11 Dec 2020 15:58:35 +0100
Subject: [PATCH 199/264] [wrs_tidy_up/states/select_storage_location] Removed
 unnecessary debugging msgs

---
 .../mdr_wrs_tidy_up/scenario_states/select_storage_location.py  | 2 --
 1 file changed, 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
index 713664901..007b9f221 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
@@ -45,6 +45,4 @@ def execute(self, userdata):
             storage_location = random.choice(self.available_bins)
             userdata.storage_location = storage_location
             userdata.destination_locations = [self.bin_locations[storage_location]]
-            rospy.logwarn('STORAGE LOCATION %s', storage_location)
-            rospy.logwarn([self.bin_locations[storage_location]])
         return 'succeeded'

From 2ed83af52f8e562a47bcf1e3b00cdbf977a777da Mon Sep 17 00:00:00 2001
From: Ahmed Abdelrahman <ahmedfaisal_95@hotmail.co.uk>
Date: Fri, 11 Dec 2020 18:31:50 +0100
Subject: [PATCH 200/264] [wrs_tidy_up/states/pickup] Added grasp strategy
 selection

---
 .../scenario_states/pickup_object.py               | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index b6873e96f..ab07db379 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -38,7 +38,10 @@ def execute(self, userdata):
         goal = PickupGoal()
         goal.pose.header.frame_id = object_to_pick_up.pose.header.frame_id
         goal.pose.header.stamp = rospy.Time.now()
-        goal.pose.pose = self.get_grasping_pose(object_to_pick_up)
+
+        grasping_pose, grasping_strategy = self.get_grasping_pose_and_strategy(object_to_pick_up)
+        goal.pose.pose = grasping_pose
+        goal.strategy = grasping_strategy
 
         rospy.loginfo('[%s] Picking up object at %s position (%f %f %f)', self.state_name,
                       goal.pose.header.frame_id, goal.pose.pose.position.x,
@@ -67,7 +70,7 @@ def execute(self, userdata):
         self.retry_count += 1
         return 'failed'
 
-    def get_grasping_pose(self, object_to_pick_up):
+    def get_grasping_pose_and_strategy(self, object_to_pick_up):
         '''Returns a geometry_msgs.msg.Pose object representing a grasping pose
         for the given object. The grasping position is given by the center of the
         object's bounding box; the orientation ensures a top-down grasp aligned
@@ -85,20 +88,23 @@ def get_grasping_pose(self, object_to_pick_up):
         gripper_orientation_z = np.arctan2(object_to_pick_up.bounding_box.dimensions.y,
                                            object_to_pick_up.bounding_box.dimensions.x)
 
+        grasping_strategy = None
         # this orientation guarantees a sideways grasp and
         # alignment along the longest axis of the object
         if object_to_pick_up.dimensions.vector.z > max(object_to_pick_up.dimensions.vector.x,
                                                        object_to_pick_up.dimensions.vector.y):
             desired_gripper_orientation_base_link = (np.pi, -np.pi/2, 0.)
+            grasping_strategy = PickupGoal.SIDEWAYS_GRASP
         # this orientation guarantees a top-down grasp and
         # alignment along the longest axis of the object
         else:
             desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
-            pose.position.z += self.grasping_height_offset
+            grasping_strategy = PickupGoal.TOP_GRASP
 
         pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
                                                         object_to_pick_up.pose.header.frame_id)
-        return pose
+
+        return pose, grasping_strategy
 
     def get_gripper_orientation(self, orientation, target_frame):
         '''Returns a geometry_msgs.msg.Quaternion object representing the given

From 271c043c112f92f7073497f3df6c79670d396329 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 11 Dec 2020 19:19:51 +0100
Subject: [PATCH 201/264] Add dynamic reconfigure clients to change the object
 detection setup

---
 .../scenario_states/initialise_scenario.py         |  3 +++
 .../scenario_states/select_scanning_pose.py        |  4 ++++
 .../ros/src/mdr_wrs_tidy_up/utils.py               | 14 ++++++++++++++
 3 files changed, 21 insertions(+)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 1f967c606..bb08b50d5 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -12,6 +12,8 @@
 
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
+from mdr_wrs_tidy_up.utils import reconfigure_object_detection_params
+
 def get_environment_objects(planning_scene_map_file):
     object_list = ObjectList()
 
@@ -71,6 +73,7 @@ def execute(self, userdata):
             rospy.loginfo('[%s] Planning scene map file not specified; not initialising KB and scene', self.state_name)
             return 'succeeded'
 
+        reconfigure_object_detection_params(0.02, 0.2)
         environment_objects = get_environment_objects(self.planning_scene_map_file)
 
         # initialising the knowledge base
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
index 65994e363..cc06f7dba 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
@@ -1,6 +1,8 @@
 import random
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
+from mdr_wrs_tidy_up.utils import reconfigure_object_detection_params
+
 def get_dict_keys_with_false_values(d):
     keys_with_false_values = []
     for k, v in d.items():
@@ -28,11 +30,13 @@ def execute(self, userdata):
             dirty_locations = get_dict_keys_with_false_values(userdata.floor_objects_cleared)
             userdata.object_location = 'floor'
             userdata.destination_locations = [random.choice(dirty_locations)]
+            reconfigure_object_detection_params(0.02, 0.2)
             return 'floor_not_cleared'
         elif False in userdata.table_objects_cleared.values():
             dirty_locations = get_dict_keys_with_false_values(userdata.table_objects_cleared)
             userdata.object_location = 'table'
             userdata.destination_locations = [random.choice(dirty_locations)]
+            reconfigure_object_detection_params(0.4, 0.6)
             return 'table_not_cleared'
         else:
             return 'tidying_done'
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
new file mode 100644
index 000000000..098a693ee
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
@@ -0,0 +1,14 @@
+import dynamic_reconfigure.client
+import rospy
+
+
+def reconfigure_object_detection_params(z_min, z_max, node_name = "/mas_perception/cloud_obstacle_detection"):
+    try:
+        client = dynamic_reconfigure.client.Client(node_name, timeout=1.5)
+    except Exception as e:
+        rospy.logerr("Service {0} does not exist".format(node_name + '/set_parameters'))
+        return False
+
+    updates = {"passthrough_limit_min_z":  z_min,
+               "passthrough_limit_max_z":  z_max}
+    client.update_configuration(updates)

From db38b3344257b3f889992b4d53c7c95acc5dc9c6 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 11 Dec 2020 20:12:48 +0100
Subject: [PATCH 202/264] Add a config file to store object detection params

---
 .../config/object_detection_params.yaml       | 12 ++++++++++
 .../scenario_states/initialise_scenario.py    |  6 ++---
 .../scenario_states/select_scanning_pose.py   |  9 ++++----
 .../ros/src/mdr_wrs_tidy_up/utils.py          | 22 +++++++++++++++++++
 4 files changed, 41 insertions(+), 8 deletions(-)
 create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
new file mode 100644
index 000000000..230926915
--- /dev/null
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
@@ -0,0 +1,12 @@
+floor:
+  z_min: 0.02
+  z_max: 0.2
+long_table_b:
+  z_min: 0.42
+  z_max: 0.6
+tall_table:
+  z_min: 0.62
+  z_max: 0.8
+shelf:
+  z_min: 0.5
+  z_max: 0.9
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index bb08b50d5..a6c20f710 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -1,5 +1,3 @@
-import yaml
-
 import rospy
 import tf
 from geometry_msgs.msg import Pose, Point, Quaternion, Vector3
@@ -12,7 +10,7 @@
 
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
-from mdr_wrs_tidy_up.utils import reconfigure_object_detection_params
+from mdr_wrs_tidy_up.utils import update_object_detection_params
 
 def get_environment_objects(planning_scene_map_file):
     object_list = ObjectList()
@@ -73,7 +71,7 @@ def execute(self, userdata):
             rospy.loginfo('[%s] Planning scene map file not specified; not initialising KB and scene', self.state_name)
             return 'succeeded'
 
-        reconfigure_object_detection_params(0.02, 0.2)
+        update_object_detection_params("floor")
         environment_objects = get_environment_objects(self.planning_scene_map_file)
 
         # initialising the knowledge base
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
index cc06f7dba..2058df14a 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
@@ -1,7 +1,7 @@
 import random
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
-from mdr_wrs_tidy_up.utils import reconfigure_object_detection_params
+from mdr_wrs_tidy_up.utils import update_object_detection_params
 
 def get_dict_keys_with_false_values(d):
     keys_with_false_values = []
@@ -30,13 +30,14 @@ def execute(self, userdata):
             dirty_locations = get_dict_keys_with_false_values(userdata.floor_objects_cleared)
             userdata.object_location = 'floor'
             userdata.destination_locations = [random.choice(dirty_locations)]
-            reconfigure_object_detection_params(0.02, 0.2)
+            update_object_detection_params("floor")
             return 'floor_not_cleared'
         elif False in userdata.table_objects_cleared.values():
             dirty_locations = get_dict_keys_with_false_values(userdata.table_objects_cleared)
             userdata.object_location = 'table'
-            userdata.destination_locations = [random.choice(dirty_locations)]
-            reconfigure_object_detection_params(0.4, 0.6)
+            destination_location = random.choice(dirty_locations)
+            userdata.destination_locations = [destination_location]
+            update_object_detection_params(destination_location)
             return 'table_not_cleared'
         else:
             return 'tidying_done'
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
index 098a693ee..1e46e0464 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
@@ -1,6 +1,19 @@
 import dynamic_reconfigure.client
 import rospy
+from mas_tools.ros_utils import get_package_path
+from mas_tools.file_utils import load_yaml_file
 
+def load_obstacle_detection_params(filename=None):
+    if filename:
+        params_file = get_package_path('mdr_wrs_tidy_up', 'config', filename)
+    else:
+        params_file = get_package_path('mdr_wrs_tidy_up', 'config', 'object_detection_params')
+    params = load_yaml_file(params_file)
+    if params:
+        return params
+    else:
+        rospy.logerr("Unable to load object_detection_params from file {0}".format(params_file))
+        return None
 
 def reconfigure_object_detection_params(z_min, z_max, node_name = "/mas_perception/cloud_obstacle_detection"):
     try:
@@ -12,3 +25,12 @@ def reconfigure_object_detection_params(z_min, z_max, node_name = "/mas_percepti
     updates = {"passthrough_limit_min_z":  z_min,
                "passthrough_limit_max_z":  z_max}
     client.update_configuration(updates)
+
+def update_object_detection_params(surface, config_file=None):
+    params = load_obstacle_detection_params(config_file)
+    if params:
+        config = params[surface]
+        if config:
+            reconfigure_object_detection_params(z_min=config["z_min"], z_min=config["z_max"])
+        else:
+            rospy.logerr("Unable to load object_detection_params for surface {0}".format(surface))

From a79feef69e9cf8fe9fa9a5c3ef11ee0fca5b1e1c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 09:59:04 +0100
Subject: [PATCH 203/264] [wrs_tidy_up/utils] Fixed fn argument use

---
 .../mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py            | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
index 1e46e0464..78d1520d1 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
@@ -31,6 +31,6 @@ def update_object_detection_params(surface, config_file=None):
     if params:
         config = params[surface]
         if config:
-            reconfigure_object_detection_params(z_min=config["z_min"], z_min=config["z_max"])
+            reconfigure_object_detection_params(z_min=config["z_min"], z_max=config["z_max"])
         else:
             rospy.logerr("Unable to load object_detection_params for surface {0}".format(surface))

From bc5e4d3d2b7ccd532af7a52f0284131055c16118 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 10:21:13 +0100
Subject: [PATCH 204/264] [mdr_wrs_tidy_up/utils] Fixed path to detection
 params config file

---
 .../mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py            | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
index 78d1520d1..8767cc72c 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
@@ -7,7 +7,7 @@ def load_obstacle_detection_params(filename=None):
     if filename:
         params_file = get_package_path('mdr_wrs_tidy_up', 'config', filename)
     else:
-        params_file = get_package_path('mdr_wrs_tidy_up', 'config', 'object_detection_params')
+        params_file = get_package_path('mdr_wrs_tidy_up', 'config', 'object_detection_params.yaml')
     params = load_yaml_file(params_file)
     if params:
         return params

From 32e4b577381d3db993c279f2ff5072d380b385ae Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 10:52:12 +0100
Subject: [PATCH 205/264] [wrs_tidy_up/config] Added some missing objects to
 the scene map

In particular, I've added:
* the trays on the placing table
* the containers on the placing table (even though they're not required for
  the scenario)
* the tables have horizontal cylindrical bars below the table plane, so
  I've added a collision box for those
---
 .../config/planning_scene_map.yaml            | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
index 9b30ef77e..91d507db0 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
@@ -47,6 +47,10 @@ objects:
       position:   [1.5, -0.6, 0.39]
       orientation: [0.0, 0.0, 0.0]
       dimensions: [1.2, 0.4, 0.02]
+    - name: long_table_a_support
+      position:   [1.5, -0.6, 0.29]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [1.2, 0.4, 0.02]
     - name: long_table_a_leg1
       position:   [2.1, -0.4, 0.195]
       orientation: [0.0, 0.0, 0.0]
@@ -69,6 +73,10 @@ objects:
       position:   [0.0, 1.8, 0.59]
       orientation: [0.0, 0.0, 0.0]
       dimensions: [0.40, 0.40, 0.02]
+    - name: tall_table_support
+      position:   [0.0, 1.8, 0.49]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.40, 0.40, 0.02]
     - name: tall_table_leg1
       position:   [-0.2, 1.6, 0.295]
       orientation: [0.0, 0.0, 0.0]
@@ -91,6 +99,10 @@ objects:
       position:   [1.0, 1.8, 0.39]
       orientation: [0.0, 0.0, 0.0]
       dimensions: [1.2, 0.4, 0.02]
+    - name: long_table_b_support
+      position:   [1.0, 1.8, 0.29]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [1.2, 0.4, 0.02]
     - name: long_table_b_leg1
       position:   [0.4, 2.0, 0.195]
       orientation: [0.0, 0.0, 0.0]
@@ -107,3 +119,27 @@ objects:
       position:   [1.6, 1.6, 0.195]
       orientation: [0.0, 0.0, 0.0]
       dimensions: [0.02, 0.02, 0.40]
+
+    # object ID: wrc_tray_1
+    - name: tray_1
+      position:   [1.95, -0.6, 0.4]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.285, 0.37, 0.004]
+
+    # object ID: wrc_tray_2
+    - name: tray_2
+      position:   [1.65, -0.6, 0.4]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.285, 0.37, 0.004]
+
+    # object ID: wrc_container_a
+    - name: container_a
+      position:   [1.4, -0.6, 0.45]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.1, 0.1, 0.1]
+
+    # object ID: wrc_container_b
+    - name: container_b
+      position:   [1.1, -0.6, 0.455]
+      orientation: [0.0, 0.0, 0.0]
+      dimensions: [0.26, 0.18, 0.11]

From 4ad80ea38231db028b6e375515f6ceba0d47883b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 10:59:11 +0100
Subject: [PATCH 206/264] [wrs_tidy_up/states/init] Skipping objects with
 support in the name; added more debugging msgs

---
 .../scenario_states/initialise_scenario.py             | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index a6c20f710..34656be20 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -97,12 +97,18 @@ def execute(self, userdata):
         return 'succeeded'
 
     def __init_kb(self, object_list, userdata):
+        kb_blacklist_names = ['wall', 'leg', 'support']
+        contains_name = lambda x: sum([1 for name in kb_blacklist_names
+                                       if x.find(name) != -1]) > 0
+
         kb_objects = dict()
         try:
             for obj in object_list.objects:
-                # we don't add walls and table legs to the knowledge base
-                if obj.name.find('wall') != -1 or obj.name.find('leg') != -1:
+                # we don't add the blacklisted objects to the knowledge base
+                if contains_name(obj.name):
+                    rospy.loginfo('[%s] Skipping %s', self.state_name, obj.name)
                     continue
+                rospy.loginfo('[%s] Adding %s', self.state_name, obj.name)
                 kb_objects[obj.name] = obj
         except Exception as exc:
             rospy.logerr('[%s] Error while initialising the knowledge base', self.state_name)

From 916ebdf935cbd36a89f574947b07a74502f89ff5 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 12:28:22 +0100
Subject: [PATCH 207/264] [wrs_tidy_up/states/move_base] Fixed bug with source
 of locations

---
 .../src/mdr_wrs_tidy_up/scenario_states/move_base.py   | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
index f34d9ec28..77ec30179 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/move_base.py
@@ -22,13 +22,17 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.retry_count = 0
 
     def execute(self, userdata):
+        destination_locations = None
         if self.destination_locations is None:
-            self.destination_locations = userdata.destination_locations
-            rospy.loginfo("Using userdata's destination_locations %s", self.destination_locations)
+            destination_locations = list(userdata.destination_locations)
+            rospy.loginfo("Using userdata's destination_locations %s", destination_locations)
+        else:
+            destination_locations = list(self.destination_locations)
+            rospy.loginfo("Using predefined destination_locations %s", destination_locations)
 
         client = actionlib.SimpleActionClient(self.action_server_name, MoveBaseAction)
         client.wait_for_server()
-        for destination_location in self.destination_locations:
+        for destination_location in destination_locations:
             goal = MoveBaseGoal()
             goal.goal_type = MoveBaseGoal.NAMED_TARGET
             goal.destination_location = destination_location

From bfb476c544d5fbe10deb2366acea6a3d914839c1 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 12:29:57 +0100
Subject: [PATCH 208/264] [wrs_tidy_up/states/find_objects] Set floor location
 to 'cleared' if no objects found

---
 .../src/mdr_wrs_tidy_up/scenario_states/find_objects.py  | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 8fbcba380..3e2958aee 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -23,8 +23,11 @@ def __init__(self, save_sm_state=False, **kwargs):
                                              'failed', 'failed_after_retrying'],
                                    input_keys=['floor_objects_cleared',
                                                'table_objects_cleared',
-                                               'object_location'],
-                                   output_keys=['detected_objects'])
+                                               'object_location',
+                                               'destination_locations'],
+                                   output_keys=['detected_objects',
+                                                'floor_objects_cleared',
+                                                'table_objects_cleared'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'find_objects')
         self.object_detection_server_name = kwargs.get('object_detection_server_name',
@@ -51,6 +54,8 @@ def execute(self, userdata):
             userdata.detected_objects = self.detected_cloud_objects
 
             if not self.detected_cloud_objects:
+                current_location = userdata.destination_locations[0]
+                userdata.floor_objects_cleared[current_location] = True
                 return 'no_objects'
         else:
             pass

From ac83a44fbc69dcad5cb8eb1b2dca0e2b1e12a781 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 13:34:22 +0100
Subject: [PATCH 209/264] [wrs_tidy_up/states/pickup] Set grasping pose to top
 of object for top-down grasp

This is to prevent pushing down on the object when grasping from the top
---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py  | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index ab07db379..87cbdd1dc 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -101,6 +101,10 @@ def get_grasping_pose_and_strategy(self, object_to_pick_up):
             desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
             grasping_strategy = PickupGoal.TOP_GRASP
 
+            # we set the grasping pose along z to be the top of the object to prevent
+            # the robot pushing down the object with the gripper
+            pose.position.z += (object_to_pick_up.bounding_box.dimensions.z / 2)
+
         pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
                                                         object_to_pick_up.pose.header.frame_id)
 

From b8db1f55b0d8d0d90e15ea694ee7c7ac0f1b1f3d Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 17:20:05 +0100
Subject: [PATCH 210/264] [wrs_tidy_up/config/object_detection_params] Reduced
 the min-z cutoff for floor objects

In particular, the value is now 0.02 (the previous value was 0.01). With
the old value, small objects (such as a pen and a fork) were missed by
the detection component.
---
 .../mdr_wrs_tidy_up/config/object_detection_params.yaml         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
index 230926915..5a8614702 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
@@ -1,5 +1,5 @@
 floor:
-  z_min: 0.02
+  z_min: 0.01
   z_max: 0.2
 long_table_b:
   z_min: 0.42

From 37bc5bcbf3ad6c7e76c0746c9c6c46d429a909c0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 17:21:46 +0100
Subject: [PATCH 211/264] [wrs_tidy_up/states/find_objects] Sleep for 0.5s
 after clearing the object cache

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py     | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 3e2958aee..52a0426ab 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -45,6 +45,7 @@ def execute(self, userdata):
 
             rospy.loginfo('[%s] Resetting cloud obstacle cache and waiting a bit', self.state_name)
             self.obstacle_cache_reset_pub.publish(Bool(data=True))
+            rospy.sleep(0.5)
 
             rospy.loginfo('[%s] Waiting for cloud obstacle detection', self.state_name)
             while last_msg_time == self.last_cloud_object_detection_time:
@@ -53,6 +54,7 @@ def execute(self, userdata):
             rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
             userdata.detected_objects = self.detected_cloud_objects
 
+            # if no objects are seen in the current view, we register the location as "cleared"
             if not self.detected_cloud_objects:
                 current_location = userdata.destination_locations[0]
                 userdata.floor_objects_cleared[current_location] = True

From c60f7ef5bd5584599d6ea59646a5b1ae1132fafa Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Sun, 13 Dec 2020 17:22:42 +0100
Subject: [PATCH 212/264] [wrs_tidy_up/states/select_object] Ignore z when
 looking for closest object

---
 .../select_object_for_picking.py               | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
index 0f6a73db6..d2728f3f8 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
@@ -19,21 +19,19 @@ def execute(self, userdata):
         closest_obj_index = 0
         closest_obj_distance = float('inf')
         for index, obj in enumerate(userdata.detected_objects):
-            while not rospy.is_shutdown():
-                try:
-                    (obj_position, obj_orientation) = self.tf_listener.lookupTransform('/base_link',
-                                                                                       obj.pose.header.frame_id,
-                                                                                       rospy.Time(0))
-                    break
-                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-                    continue
+            try:
+                pose_base_link = self.tf_listener.transformPose('base_link', obj.pose)
+            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as exc:
+                rospy.logerr('[%s] %s', self.state_name, str(exc))
+                continue
 
-            distance_to_obj = np.linalg.norm(np.array(obj_position))
+            # we want to grasp the closest object along the xy-plane
+            distance_to_obj = np.linalg.norm(np.array([pose_base_link.pose.position.x,
+                                                       pose_base_link.pose.position.y]))
 
             if distance_to_obj < closest_obj_distance:
                 closest_obj_index = index
                 closest_obj_distance = distance_to_obj
 
         userdata.selected_object = userdata.detected_objects[closest_obj_index]
-
         return 'succeeded'

From cc615627e45dc853675ca3f6e104a4797bbb487e Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 14 Dec 2020 11:48:30 +0100
Subject: [PATCH 213/264] [wrs_tidy_up/config/sm] Removed bin_b as a possible
 placing location

bin_a seems to be easier for placing, so it's better to keep only that
one
---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml   | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 2903fa043..6e7229861 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -147,7 +147,7 @@ state_descriptions:
                 value: ['tray_1', 'tray_2']
             - argument:
                 name: available_bins
-                value: ['bin_a', 'bin_b']
+                value: ['bin_a']
             - argument:
                 name: tray_locations
                 value:
@@ -157,7 +157,6 @@ state_descriptions:
                 name: bin_locations
                 value:
                     bin_a: bin_a
-                    bin_b: bin_b
     - state:
         name: MOVE_TO_STORAGE_LOCATION
         state_module_name: mdr_wrs_tidy_up.scenario_states.move_base

From a90f77ef07b1cd71c0f8cdee9811c80fb4f04889 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 14 Dec 2020 11:53:01 +0100
Subject: [PATCH 214/264] [wrd_tidy_up/states/find_objects] Filter objects
 based on height; for now, use same detection method for all locations

The height-based filtering is to avoid Gazebo issues with grasping the
pitcher, which seems to stick to the gripper
---
 .../scenario_states/find_objects.py           | 39 +++++++++++--------
 1 file changed, 22 insertions(+), 17 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 52a0426ab..8a1d22d5d 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -36,31 +36,36 @@ def __init__(self, save_sm_state=False, **kwargs):
                                                          '/mas_perception/cloud_obstacle_detection/obstacle_objects')
         self.cloud_obstacle_cache_reset_topic = kwargs.get('cloud_obstacle_cache_reset_topic',
                                                            '/mas_perception/cloud_obstacle_detection/reset_cache')
+        self.max_allowed_obj_height_cm = kwargs.get('max_allowed_obj_height_cm', 0.15)
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.__init_ros_components()
 
     def execute(self, userdata):
-        if userdata.object_location == 'floor':
-            last_msg_time = self.last_cloud_object_detection_time
+        last_msg_time = self.last_cloud_object_detection_time
 
-            rospy.loginfo('[%s] Resetting cloud obstacle cache and waiting a bit', self.state_name)
-            self.obstacle_cache_reset_pub.publish(Bool(data=True))
-            rospy.sleep(0.5)
+        rospy.loginfo('[%s] Resetting cloud obstacle cache and waiting a bit', self.state_name)
+        self.obstacle_cache_reset_pub.publish(Bool(data=True))
+        rospy.sleep(0.5)
+
+        rospy.loginfo('[%s] Waiting for cloud obstacle detection', self.state_name)
+        while last_msg_time == self.last_cloud_object_detection_time:
+            rospy.sleep(0.05)
 
-            rospy.loginfo('[%s] Waiting for cloud obstacle detection', self.state_name)
-            while last_msg_time == self.last_cloud_object_detection_time:
-                rospy.sleep(0.05)
+        rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
 
-            rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
-            userdata.detected_objects = self.detected_cloud_objects
+        # workaround for large objects (such as the pitcher) sticking to the gripper in Gazebo
+        rospy.loginfo('[%s] Filtering detected objects using height threshold %f',
+                      self.state_name, self.max_allowed_obj_height_cm)
+        filtered_objects = [obj for obj in self.detected_cloud_objects
+                            if obj.dimensions.vector.z <= self.max_allowed_obj_height_cm]
+        userdata.detected_objects = filtered_objects
+        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
 
-            # if no objects are seen in the current view, we register the location as "cleared"
-            if not self.detected_cloud_objects:
-                current_location = userdata.destination_locations[0]
-                userdata.floor_objects_cleared[current_location] = True
-                return 'no_objects'
-        else:
-            pass
+        # if no objects are seen in the current view, we register the location as "cleared"
+        if not filtered_objects:
+            current_location = userdata.destination_locations[0]
+            userdata.floor_objects_cleared[current_location] = True
+            return 'no_objects'
 
         return 'succeeded'
 

From 345b8c6c3e920cf75b7153f3ad7efcb5b95902bd Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 14 Dec 2020 11:54:17 +0100
Subject: [PATCH 215/264] [wrs_tidy_up/states/pickup] Increases grasp offset to
 10cm; set the z-orientation correctly during a top-down grasp

---
 .../scenario_states/pickup_object.py               | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index 87cbdd1dc..ca08d9dcc 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -28,7 +28,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.pickup_server_name = kwargs.get('pickup_server_name', 'pickup_server')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
-        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.1)
         self.retry_count = 0
         self.__init_ros_components()
 
@@ -85,9 +85,6 @@ def get_grasping_pose_and_strategy(self, object_to_pick_up):
         pose.position.y = object_to_pick_up.bounding_box.center.y
         pose.position.z = object_to_pick_up.bounding_box.center.z
 
-        gripper_orientation_z = np.arctan2(object_to_pick_up.bounding_box.dimensions.y,
-                                           object_to_pick_up.bounding_box.dimensions.x)
-
         grasping_strategy = None
         # this orientation guarantees a sideways grasp and
         # alignment along the longest axis of the object
@@ -98,12 +95,19 @@ def get_grasping_pose_and_strategy(self, object_to_pick_up):
         # this orientation guarantees a top-down grasp and
         # alignment along the longest axis of the object
         else:
+            object_pose_in_base_link = self.tf_listener.transformPose('base_link', object_to_pick_up.pose)
+            euler_orientation = tf.transformations.euler_from_quaternion([object_pose_in_base_link.pose.orientation.x,
+                                                                          object_pose_in_base_link.pose.orientation.y,
+                                                                          object_pose_in_base_link.pose.orientation.z,
+                                                                          object_pose_in_base_link.pose.orientation.w])
+            gripper_orientation_z = euler_orientation[2]
+
             desired_gripper_orientation_base_link = (np.pi, 0, gripper_orientation_z)
             grasping_strategy = PickupGoal.TOP_GRASP
 
             # we set the grasping pose along z to be the top of the object to prevent
             # the robot pushing down the object with the gripper
-            pose.position.z += (object_to_pick_up.bounding_box.dimensions.z / 2)
+            pose.position.z += (object_to_pick_up.dimensions.vector.z / 2)
 
         pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
                                                         object_to_pick_up.pose.header.frame_id)

From 7aca6c927a728496c9e8a6a4be11ef28b2d2ff39 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 14 Dec 2020 11:55:00 +0100
Subject: [PATCH 216/264] [environments/wrs20-arena] Moved one inspection pose
 a bit further from the table

Otherwise there were cases in which objects were lying on the pose and
move_base couldn't find a plan
---
 mdr_environments/wrs20-arena/navigation_goals.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_environments/wrs20-arena/navigation_goals.yaml b/mdr_environments/wrs20-arena/navigation_goals.yaml
index 3f65a5991..cfed1caee 100644
--- a/mdr_environments/wrs20-arena/navigation_goals.yaml
+++ b/mdr_environments/wrs20-arena/navigation_goals.yaml
@@ -1,6 +1,6 @@
 # pose_name: [x,y,theta]
 
-table_inspection_area1: [-0.101, 0.847, 0.737]
+table_inspection_area1: [-0.101, 0.8, 0.737]
 table_inspection_area2: [0.696, 0.579, 1.608]
 table_inspection_area3: [1.367, 0.714, 1.745]
 

From 7f95a44bb1f7c3fc803c0dc176fcc96ad3e6c4bb Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Mon, 14 Dec 2020 17:36:51 +0100
Subject: [PATCH 217/264] [move_base_action] Added simple recovery behaviour if
 case goal pose is unreachable

The recovery generates a new goal pose (by adding Gaussian noise to the
original xy-position) and retries the navigation
---
 .../mdr_move_base_action/README.md            |  3 +
 .../ros/launch/move_base_action.launch        |  6 ++
 .../ros/scripts/move_base_action              | 17 +++++-
 .../src/mdr_move_base_action/action_states.py | 58 +++++++++++++++++--
 4 files changed, 77 insertions(+), 7 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md
index 7540ca566..e738af969 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md
@@ -65,6 +65,9 @@ The following parameters may be passed when launching the action server:
 * ``pose_frame``: Name of the frame in which the poses in `pose_description_file` are given (default: 'map')
 * ``safe_arm_joint_config``: Name of a configuration specified in which the robot can safely move around the environment (default: 'folded')
 * ``move_arm_server``: Name of the `move_arm` action server (default: 'move_arm_server')
+* ``timeout``: Navigation timeout (in seconds) (default: 120)
+* ``recovery_position_m_std``: If a navigation to a pose fails, an attempted recovery is to change the goal's (x, y) position a little and retry the navigation (the assumption is that the robot moves over the xy-plane)). The position is changed by adding Gaussian noise with zero mean and the given standard deviation (in meters) to the original goal position. (default: 0.2)
+* ``max_recovery_attempts``: Maximum number of recovery attempts in case of navigation failures (default 1)
 
 ### Action client
 
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
index 889b902ea..2a4d58fd1 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch
@@ -5,12 +5,18 @@
     <arg name="move_base_server" default="/move_base" />
     <arg name="pose_frame" default="map" />
     <arg name="safe_arm_joint_config" default="folded" />
+    <arg name="timeout" default="120.0" />
+    <arg name="recovery_position_m_std" default="0.2" />
+    <arg name="max_recovery_attempts" default="1" />
 
     <node pkg="mdr_move_base_action" type="move_base_action" name="move_base_server" output="screen">
         <param name="move_arm_server" value="$(arg move_arm_server)" />
         <param name="move_base_server" value="$(arg move_base_server)" />
         <param name="pose_frame" value="$(arg pose_frame)" />
         <param name="safe_arm_joint_config" value="$(arg safe_arm_joint_config)" />
+        <param name="timeout" value="$(arg timeout)" />
+        <param name="recovery_position_m_std" value="$(arg recovery_position_m_std)" />
+        <param name="max_recovery_attempts" value="$(arg max_recovery_attempts)" />
         <param name="pose_description_file" value="$(find mdr_environments)/$(arg robot_env)/navigation_goals.yaml" />
     </node>
 </launch>
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action
index 2379ab34f..a6e636e6f 100755
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action
@@ -17,6 +17,15 @@ class MoveBaseServer(object):
     * safe_arm_joint_config: Name of a configuration specified in which the robot can
                              safely move around the environment (default: 'folded')
     * move_arm_server: Name of the move_arm action server (default: 'move_arm_server')
+    * timeout: Navigation timeout (in seconds) (default: 120)
+    * recovery_position_m_std: If a navigation to a pose fails, an attempted recovery
+                               is to change the goal's (x, y) position a little and
+                               retry the navigation (the assumption is that the robot
+                               moves over the xy-plane)). The position is changed by adding
+                               Gaussian noise with zero mean and the given standard deviation
+                               (in meters) to the original goal position. (default: 0.2)
+    * max_recovery_attempts: Maximum number of recovery attempts in case of
+                             navigation failures (default 1)
 
     @author Alex Mitrevski
     @contact aleksandar.mitrevski@h-brs.de
@@ -28,13 +37,19 @@ class MoveBaseServer(object):
         move_base_server = rospy.get_param('~move_base_server', '')
         pose_description_file = rospy.get_param('~pose_description_file', '')
         pose_frame = rospy.get_param('~pose_frame', '')
+        timeout = rospy.get_param('~timeout', 120.)
+        recovery_position_m_std = rospy.get_param('~recovery_position_m_std', 0.2)
+        max_recovery_attempts = rospy.get_param('~max_recovery_attempts', 1)
 
         rospy.loginfo('[move_base] Initialising state machine')
         self.action_sm = MoveBaseSM(safe_arm_joint_config=safe_arm_joint_config,
                                     move_arm_server=move_arm_server,
                                     move_base_server=move_base_server,
                                     pose_description_file=pose_description_file,
-                                    pose_frame=pose_frame)
+                                    pose_frame=pose_frame,
+                                    timeout=timeout,
+                                    recovery_position_m_std=recovery_position_m_std,
+                                    max_recovery_attempts=max_recovery_attempts)
         rospy.loginfo('[move_base] State machine initialised')
 
         self.action_server = actionlib.SimpleActionServer('move_base_server',
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
index 94504f0b8..e42fbe105 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
@@ -1,9 +1,12 @@
 import yaml
+import numpy as np
+
 import rospy
 import actionlib
 import tf
 from geometry_msgs.msg import PoseStamped, Quaternion
 import move_base_msgs.msg as move_base_msgs
+from actionlib_msgs.msg import GoalStatus
 
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
@@ -17,6 +20,7 @@ def __init__(self, timeout=120.,
                  move_base_server='/move_base',
                  pose_description_file='',
                  pose_frame='map',
+                 recovery_position_m_std=0.2,
                  max_recovery_attempts=1):
         super(MoveBaseSM, self).__init__('MoveBase', [], max_recovery_attempts)
         self.pose = None
@@ -27,6 +31,11 @@ def __init__(self, timeout=120.,
         self.pose_frame = pose_frame
         self.timeout = timeout
         self.move_arm_client = None
+        self.move_base_goal_pub = None
+
+        self.is_recovering = False
+        self.recovery_count = 0
+        self.recovery_position_m_std = recovery_position_m_std
 
     def init(self):
         try:
@@ -35,6 +44,12 @@ def init(self):
             self.move_arm_client.wait_for_server()
         except:
             rospy.logerr('[move_base] %s server does not seem to respond', self.move_arm_server)
+
+        # we also create a goal pose publisher for debugging purposes
+        goal_pose_topic = self.move_base_server + '_goal'
+        rospy.loginfo('[move_base] Creating a goal pose publisher on topic %s', goal_pose_topic)
+        self.move_base_goal_pub = rospy.Publisher(goal_pose_topic, PoseStamped, queue_size=1)
+
         return FTSMTransitions.INITIALISED
 
     def running(self):
@@ -66,24 +81,51 @@ def running(self):
             self.result = self.set_result(False)
             return FTSMTransitions.DONE
 
+        # we attempt a recovery in which we send the robot to a slightly
+        # different pose from the one originally requested
+        if self.is_recovering:
+            pose.pose.position.x += np.random.normal(0., self.recovery_position_m_std)
+            pose.pose.position.y += np.random.normal(0., self.recovery_position_m_std)
+
         goal = move_base_msgs.MoveBaseGoal()
         goal.target_pose = pose
+        self.move_base_goal_pub.publish(pose)
 
         move_base_client = actionlib.SimpleActionClient(self.move_base_server,
                                                         move_base_msgs.MoveBaseAction)
         move_base_client.wait_for_server()
         move_base_client.send_goal(goal)
-        success = move_base_client.wait_for_result()
 
-        if success:
-            rospy.loginfo('[move_base] Pose reached successfully')
-            self.result = self.set_result(True)
-            return FTSMTransitions.DONE
+        if move_base_client.wait_for_result(rospy.Duration.from_sec(self.timeout)):
+            result = move_base_client.get_result()
+            resulting_state = move_base_client.get_state()
+            if result and resulting_state == GoalStatus.SUCCEEDED:
+                rospy.loginfo('[move_base] Pose reached successfully')
+                self.reset_state()
+                self.result = self.set_result(True)
+                return FTSMTransitions.DONE
+            else:
+                rospy.logerr('[move_base] Pose could not be reached')
+                if self.recovery_count < self.max_recovery_attempts:
+                    self.recovery_count += 1
+                    rospy.logwarn('[move_base] Attempting recovery')
+                    return FTSMTransitions.RECOVER
+        else:
+            rospy.logerr('[move_base] Pose could not be reached within %f seconds', self.timeout)
+            if self.recovery_count < self.max_recovery_attempts:
+                self.recovery_count += 1
+                rospy.logerr('[move_base] Attempting recovery')
+                return FTSMTransitions.RECOVER
 
-        rospy.logerr('[move_base] Pose could not be reached')
+        rospy.logerr('[move_base] Pose could not be reached; giving up')
+        self.reset_state()
         self.result = self.set_result(False)
         return FTSMTransitions.DONE
 
+    def recovering(self):
+        self.is_recovering = True
+        return FTSMTransitions.DONE_RECOVERING
+
     def convert_pose_name_to_coordinates(self, pose_name):
         stream = open(self.pose_description_file, 'r')
         poses = yaml.load(stream)
@@ -99,3 +141,7 @@ def set_result(self, success):
         result = MoveBaseResult()
         result.success = success
         return result
+
+    def reset_state(self):
+        self.recovery_count = 0
+        self.is_recovering = False

From c554e83645ab04494ee9ef2e92dd727ff42dd8ed Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 11:39:59 +0100
Subject: [PATCH 218/264] [environment/wrs20-arena] Updated the scanning poses

The same scanning position is now used, but with three different
orientations; this covers the complete table area and reduces the
possibility for navigation failures due to objects occupying the goal
pose
---
 mdr_environments/wrs20-arena/navigation_goals.yaml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mdr_environments/wrs20-arena/navigation_goals.yaml b/mdr_environments/wrs20-arena/navigation_goals.yaml
index cfed1caee..670a68498 100644
--- a/mdr_environments/wrs20-arena/navigation_goals.yaml
+++ b/mdr_environments/wrs20-arena/navigation_goals.yaml
@@ -1,8 +1,8 @@
 # pose_name: [x,y,theta]
 
-table_inspection_area1: [-0.101, 0.8, 0.737]
-table_inspection_area2: [0.696, 0.579, 1.608]
-table_inspection_area3: [1.367, 0.714, 1.745]
+table_inspection_area1: [0.816, 0.685, 2.235]
+table_inspection_area2: [0.816, 0.685, 1.637]
+table_inspection_area3: [0.816, 0.685, 0.942]
 
 long_table_b: [1.240, 0.978, 1.693]
 tall_table: [0.012, 0.981, 1.554]

From ab8324ffd1c9941f35048e926aea8f08eb837346 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 11:45:30 +0100
Subject: [PATCH 219/264] [wrs_tidy_up/states/find_objects] Added filtering
 based on distance to the closest table leg

The path planner generally fails when objects are too
close to the table legs; as a workaround, I decided to filter out such
objects
---
 .../scenario_states/find_objects.py           | 50 ++++++++++++++++---
 .../scenario_states/initialise_scenario.py    |  8 ---
 2 files changed, 44 insertions(+), 14 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 8a1d22d5d..d4f6d612c 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -1,4 +1,7 @@
+import numpy as np
+
 import rospy
+import tf
 import actionlib
 from std_msgs.msg import Bool
 
@@ -6,6 +9,7 @@
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
 class FindObjects(ScenarioStateBase):
+    tf_listener = None
     object_detection_server_name = None
     cloud_obstacle_detection_topic = None
     cloud_obstacle_cache_reset_topic = None
@@ -21,7 +25,8 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'no_objects',
                                              'failed', 'failed_after_retrying'],
-                                   input_keys=['floor_objects_cleared',
+                                   input_keys=['environment_objects',
+                                               'floor_objects_cleared',
                                                'table_objects_cleared',
                                                'object_location',
                                                'destination_locations'],
@@ -37,6 +42,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.cloud_obstacle_cache_reset_topic = kwargs.get('cloud_obstacle_cache_reset_topic',
                                                            '/mas_perception/cloud_obstacle_detection/reset_cache')
         self.max_allowed_obj_height_cm = kwargs.get('max_allowed_obj_height_cm', 0.15)
+        self.min_allowed_dist_to_leg = kwargs.get('min_allowed_dist_to_leg', 0.15)
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.__init_ros_components()
 
@@ -54,12 +60,10 @@ def execute(self, userdata):
         rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
 
         # workaround for large objects (such as the pitcher) sticking to the gripper in Gazebo
-        rospy.loginfo('[%s] Filtering detected objects using height threshold %f',
-                      self.state_name, self.max_allowed_obj_height_cm)
-        filtered_objects = [obj for obj in self.detected_cloud_objects
-                            if obj.dimensions.vector.z <= self.max_allowed_obj_height_cm]
+        filtered_objects = self.filter_objects_by_height(self.detected_cloud_objects)
+        filtered_objects = self.filter_objects_by_dist_to_table_legs(filtered_objects,
+                                                                     userdata.environment_objects)
         userdata.detected_objects = filtered_objects
-        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
 
         # if no objects are seen in the current view, we register the location as "cleared"
         if not filtered_objects:
@@ -79,6 +83,38 @@ def store_detected_objects(self, object_msg):
         self.detected_cloud_objects = object_msg.objects
         self.last_cloud_object_detection_time = rospy.Time.now().to_sec()
 
+    def filter_objects_by_height(self, objects):
+        rospy.loginfo('[%s] Filtering detected objects using height threshold %f',
+                      self.state_name, self.max_allowed_obj_height_cm)
+        filtered_objects = [obj for obj in objects
+                            if obj.dimensions.vector.z <= self.max_allowed_obj_height_cm]
+        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
+        return filtered_objects
+
+    def filter_objects_by_dist_to_table_legs(self, objects, environment_objects):
+        rospy.loginfo('[%s] Filtering detected objects using distance to table leg threshold %f',
+                      self.state_name, self.min_allowed_dist_to_leg)
+
+        leg_positions = []
+        for env_obj_name, env_obj in environment_objects.items():
+            if env_obj_name.find('leg') != -1:
+                leg_positions.append(np.array([env_obj.pose.pose.position.x,
+                                               env_obj.pose.pose.position.y]))
+        leg_positions = np.array(leg_positions)
+
+        filtered_objects = []
+        for obj in objects:
+            distances_to_legs = []
+            obj_pose_in_map = self.tf_listener.transformPose('map', obj.pose)
+            obj_xy_position = np.array([obj_pose_in_map.pose.position.x,
+                                        obj_pose_in_map.pose.position.y])
+            distances_to_legs = np.linalg.norm(leg_positions - obj_xy_position, axis=1)
+            if np.min(distances_to_legs) > self.min_allowed_dist_to_leg:
+                filtered_objects.append(obj)
+
+        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
+        return filtered_objects
+
     def __init_ros_components(self):
         '''Initialises:
         * an object detection action client
@@ -110,3 +146,5 @@ def __init_ros_components(self):
         rospy.sleep(0.5)
         rospy.loginfo('[%s] Publisher for topic %s created',
                       self.state_name, self.cloud_obstacle_cache_reset_topic)
+
+        self.tf_listener = tf.TransformListener()
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 34656be20..021b1ad3a 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -97,17 +97,9 @@ def execute(self, userdata):
         return 'succeeded'
 
     def __init_kb(self, object_list, userdata):
-        kb_blacklist_names = ['wall', 'leg', 'support']
-        contains_name = lambda x: sum([1 for name in kb_blacklist_names
-                                       if x.find(name) != -1]) > 0
-
         kb_objects = dict()
         try:
             for obj in object_list.objects:
-                # we don't add the blacklisted objects to the knowledge base
-                if contains_name(obj.name):
-                    rospy.loginfo('[%s] Skipping %s', self.state_name, obj.name)
-                    continue
                 rospy.loginfo('[%s] Adding %s', self.state_name, obj.name)
                 kb_objects[obj.name] = obj
         except Exception as exc:

From 2c6a8ad34e1f1ce41c351f729228e2e699e97524 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 11:47:16 +0100
Subject: [PATCH 220/264] [wrs_tidy_up/states/pickup] Re-introduced a height
 offset for top-down grasps

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py  | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index ca08d9dcc..c129ef961 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -28,7 +28,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.pickup_server_name = kwargs.get('pickup_server_name', 'pickup_server')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
-        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.1)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
         self.retry_count = 0
         self.__init_ros_components()
 
@@ -107,7 +107,7 @@ def get_grasping_pose_and_strategy(self, object_to_pick_up):
 
             # we set the grasping pose along z to be the top of the object to prevent
             # the robot pushing down the object with the gripper
-            pose.position.z += (object_to_pick_up.dimensions.vector.z / 2)
+            pose.position.z += (object_to_pick_up.dimensions.vector.z / 2) + self.grasping_height_offset
 
         pose.orientation = self.get_gripper_orientation(desired_gripper_orientation_base_link,
                                                         object_to_pick_up.pose.header.frame_id)

From 0be0855f101d669fdf50238053dc99509510d5f0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 11:47:54 +0100
Subject: [PATCH 221/264] [wrs_tidy_up/states/release] Set the release pose to
 the center of the release target

This seems more reliable than randomly sampling a position on the target
area, which might be very close to the edge and thus lead to a failure
---
 .../scenario_states/release_object.py         | 27 +++++--------------
 1 file changed, 7 insertions(+), 20 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
index aa8a41af9..4dac6ce53 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
@@ -64,27 +64,14 @@ def execute(self, userdata):
         return 'succeeded'
 
     def get_release_pose(self, release_target, grasped_object):
-        release_target_obj = ConverterFactory.convert_ros_msg(release_target)
-        manipulated_obj = ConverterFactory.convert_ros_msg(grasped_object)
-
-        action_model = Action(action_name='Throw')
-        number_of_samples = 10
-
-        base_link_origin_pose = PoseStamped(Header(1, rospy.Time(0), 'base_link'),
-                                            Pose(Point(0., 0., 0.), Quaternion(0., 0., 0., 1.)))
-        robot_pose = self.tf_listener.transformPose(release_target.pose.header.frame_id,
-                                                    base_link_origin_pose)
-        robot_pose = ConverterFactory.convert_ros_msg(robot_pose)
-        model_results = action_model.get_execution_guidelines(data_count=number_of_samples,
-                                                              frame_id=release_target.pose.header.frame_id,
-                                                              manipulated_object=manipulated_obj,
-                                                              target_object=release_target_obj,
-                                                              robot_pose=robot_pose)
-        candidate_pose_idx = np.argmax(model_results['success_probabilities'])
-        candidate_pose = model_results['candidate_poses'][candidate_pose_idx]
-        candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose)
-
+        candidate_pose = PoseStamped()
         candidate_pose.header.stamp = rospy.Time(0)
+        candidate_pose.header.frame_id = release_target.pose.header.frame_id
+
+        candidate_pose.pose.position.x = release_target.pose.pose.position.x
+        candidate_pose.pose.position.y = release_target.pose.pose.position.y
+        candidate_pose.pose.position.z = release_target.pose.pose.position.z + \
+                                         (release_target.dimensions.vector.z / 2)
         candidate_pose.pose.position.z += 0.1
 
         # use top-down orientation for throwing

From f1ad5c707ae6a83451dd7f74a52940d9adecd0f2 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 14:24:49 +0100
Subject: [PATCH 222/264] [wrs_tidy_up/config/sm] Increased grasping timeout to
 45s

In rare cases, path and motion planning takes quite a while; 45s is a
generous timeout that allows such cases to be accepted as well
---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 6e7229861..c038f725d 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -123,7 +123,7 @@ state_descriptions:
                 value: pickup_server
             - argument:
                 name: grasping_timeout_s
-                value: 30.0
+                value: 45.0
             - argument:
                 name: number_of_retries
                 value: 3

From a395fce384f0df2f37d26604b1397bbaa122bd46 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 14:34:07 +0100
Subject: [PATCH 223/264] [wrs_tidy_up/states/find_objects] Added filtering for
 objects under the tables

---
 .../scenario_states/find_objects.py           | 35 +++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index d4f6d612c..7676d27fc 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -1,4 +1,5 @@
 import numpy as np
+from shapely.geometry import Point, Polygon
 
 import rospy
 import tf
@@ -8,6 +9,13 @@
 from mas_perception_msgs.msg import ObjectList, DetectObjectsAction, DetectObjectsActionGoal
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
+def get_plane_polygon(center_position, dimensions):
+    p1 = (center_position.x - (dimensions.x / 2), center_position.y - (dimensions.y / 2))
+    p2 = (center_position.x - (dimensions.x / 2), center_position.y + (dimensions.y / 2))
+    p3 = (center_position.x + (dimensions.x / 2), center_position.y + (dimensions.y / 2))
+    p4 = (center_position.x + (dimensions.x / 2), center_position.y - (dimensions.y / 2))
+    return Polygon((p1, p2, p3, p4))
+
 class FindObjects(ScenarioStateBase):
     tf_listener = None
     object_detection_server_name = None
@@ -43,6 +51,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                                            '/mas_perception/cloud_obstacle_detection/reset_cache')
         self.max_allowed_obj_height_cm = kwargs.get('max_allowed_obj_height_cm', 0.15)
         self.min_allowed_dist_to_leg = kwargs.get('min_allowed_dist_to_leg', 0.15)
+        self.tables_to_clean_up = kwargs.get('tables_to_clean_up', ['long_table_b', 'tall_table'])
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.__init_ros_components()
 
@@ -63,6 +72,9 @@ def execute(self, userdata):
         filtered_objects = self.filter_objects_by_height(self.detected_cloud_objects)
         filtered_objects = self.filter_objects_by_dist_to_table_legs(filtered_objects,
                                                                      userdata.environment_objects)
+        if userdata.object_location == 'floor':
+            filtered_objects = self.filter_objects_under_tables(filtered_objects,
+                                                                userdata.environment_objects)
         userdata.detected_objects = filtered_objects
 
         # if no objects are seen in the current view, we register the location as "cleared"
@@ -115,6 +127,29 @@ def filter_objects_by_dist_to_table_legs(self, objects, environment_objects):
         rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
         return filtered_objects
 
+    def filter_objects_under_tables(self, objects, environment_objects):
+        rospy.loginfo('[%s] Filtering detected under tables %s',
+                      self.state_name, self.tables_to_clean_up)
+
+        table_plane_polygons = []
+        for table_name in self.tables_to_clean_up:
+            table_obj = environment_objects[table_name]
+            plane_polygon = get_plane_polygon(table_obj.pose.pose.position,
+                                              table_obj.dimensions.vector)
+            table_plane_polygons.append(plane_polygon)
+
+        point_in_any_polygon = lambda point: sum([1 for polygon in table_plane_polygons
+                                                  if polygon.contains(point)]) > 0
+
+        filtered_objects = []
+        for obj in objects:
+            obj_point = Point(obj.pose.pose.position.x, obj.pose.pose.position.y)
+            if not point_in_any_polygon(obj_point):
+                filtered_objects.append(obj)
+
+        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
+        return filtered_objects
+
     def __init_ros_components(self):
         '''Initialises:
         * an object detection action client

From 8dadd01959d9f1f3eeb91de274985bed3a33cc02 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 19:48:33 +0100
Subject: [PATCH 224/264] [wrs_tidy_up/states/find_objects] Set cleared
 locations depending on object location

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py  | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 7676d27fc..d49c4befd 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -80,7 +80,10 @@ def execute(self, userdata):
         # if no objects are seen in the current view, we register the location as "cleared"
         if not filtered_objects:
             current_location = userdata.destination_locations[0]
-            userdata.floor_objects_cleared[current_location] = True
+            if userdata.object_location == 'floor':
+                userdata.floor_objects_cleared[current_location] = True
+            elif userdata.object_location == 'table':
+                userdata.table_objects_cleared[current_location] = True
             return 'no_objects'
 
         return 'succeeded'

From 22d20830a69476b0dc520700ff8212ee035ab640 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 19:49:34 +0100
Subject: [PATCH 225/264] [wrs_tidy_up/states/find_objects] Filter objects
 based on distance to table legs only when cleaning the floor

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index d49c4befd..93e6b9a8e 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -70,9 +70,9 @@ def execute(self, userdata):
 
         # workaround for large objects (such as the pitcher) sticking to the gripper in Gazebo
         filtered_objects = self.filter_objects_by_height(self.detected_cloud_objects)
-        filtered_objects = self.filter_objects_by_dist_to_table_legs(filtered_objects,
-                                                                     userdata.environment_objects)
         if userdata.object_location == 'floor':
+            filtered_objects = self.filter_objects_by_dist_to_table_legs(filtered_objects,
+                                                                         userdata.environment_objects)
             filtered_objects = self.filter_objects_under_tables(filtered_objects,
                                                                 userdata.environment_objects)
         userdata.detected_objects = filtered_objects

From 8405d69e116bc15becfc9daa8716be3ebe15ef5c Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 19:51:08 +0100
Subject: [PATCH 226/264] [wrs_tidy_up/states/pickup] Added goal pose pub for
 debugging purposes

---
 .../mdr_wrs_tidy_up/scenario_states/pickup_object.py  | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index c129ef961..5c60b9a26 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -11,7 +11,9 @@
 
 class PickupObject(ScenarioStateBase):
     pickup_server_name = 'pickup_server'
+    pickup_goal_pose_topic = '/pickup_server/goal_pose'
     pickup_client = None
+    goal_pose_pub = None
     tf_listener = None
     grasping_timeout_s = 30.
     grasping_height_offset = 0.
@@ -27,6 +29,8 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.state_name = kwargs.get('state_name', 'pickup_object')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.pickup_server_name = kwargs.get('pickup_server_name', 'pickup_server')
+        self.pickup_goal_pose_topic = kwargs.get('pickup_goal_pose_topic',
+                                                 '/pickup_server/goal_pose')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
         self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
         self.retry_count = 0
@@ -43,6 +47,9 @@ def execute(self, userdata):
         goal.pose.pose = grasping_pose
         goal.strategy = grasping_strategy
 
+        rospy.loginfo('Publishing grasping pose on topic %s', self.pickup_goal_pose_topic)
+        self.goal_pose_pub.publish(goal.pose)
+
         rospy.loginfo('[%s] Picking up object at %s position (%f %f %f)', self.state_name,
                       goal.pose.header.frame_id, goal.pose.pose.position.x,
                       goal.pose.pose.position.y, goal.pose.pose.position.z)
@@ -144,4 +151,8 @@ def __init_ros_components(self):
         self.pickup_client.wait_for_server()
         rospy.loginfo('Client for action %s initialised', self.pickup_server_name)
 
+        rospy.loginfo('Initialising publisher for goal pose on topic %s', self.pickup_goal_pose_topic)
+        self.goal_pose_pub = rospy.Publisher(self.pickup_goal_pose_topic, PoseStamped, queue_size=1)
+        rospy.loginfo('Publisher for topic %s initialised', self.pickup_goal_pose_topic)
+
         self.tf_listener = tf.TransformListener()

From cc9b8e4c76c241f30b4dc338609937ce86a8f20b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Tue, 15 Dec 2020 22:18:17 +0100
Subject: [PATCH 227/264] [wrs_tidy_up] Added timeout of 15min for task 1

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml       |  7 +++++++
 .../scenario_states/initialise_scenario.py       |  4 +++-
 .../scenario_states/select_scanning_pose.py      | 16 +++++++++++++++-
 3 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index c038f725d..72da42909 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -43,6 +43,13 @@ state_descriptions:
             - transition:
                 name: tidying_done
                 state: INITIALISE_OBSTACLE_REMOVAL
+        arguments:
+            - argument:
+                name: tidying_timeout_s
+                value: 900.0
+            - argument:
+                name: avg_obj_cleanup_duration_s
+                value: 90.0
     - state:
         name: GO_TO_SCANNING_POSE
         state_module_name: mdr_wrs_tidy_up.scenario_states.move_base
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index 021b1ad3a..fd08b06e3 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -51,7 +51,8 @@ def __init__(self, save_sm_state=False, **kwargs):
                                    output_keys=['floor_objects_cleared',
                                                 'table_objects_cleared',
                                                 'object_location',
-                                                'environment_objects'])
+                                                'environment_objects',
+                                                'operation_start_time'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'initialise_scenario')
         self.floor_objects_cleared = kwargs.get('floor_objects_cleared', None)
@@ -66,6 +67,7 @@ def execute(self, userdata):
         userdata.floor_objects_cleared = dict(self.floor_objects_cleared)
         userdata.table_objects_cleared = dict(self.table_objects_cleared)
         userdata.object_location = self.object_location
+        userdata.operation_start_time = rospy.Time.now().to_sec()
 
         if not self.planning_scene_map_file:
             rospy.loginfo('[%s] Planning scene map file not specified; not initialising KB and scene', self.state_name)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
index 2058df14a..5fa3f86a1 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_scanning_pose.py
@@ -1,4 +1,6 @@
 import random
+
+import rospy
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
 from mdr_wrs_tidy_up.utils import update_object_detection_params
@@ -15,7 +17,8 @@ def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'select_scanning_pose',
                                    save_sm_state=save_sm_state,
                                    input_keys=['floor_objects_cleared',
-                                               'table_objects_cleared'],
+                                               'table_objects_cleared',
+                                               'operation_start_time'],
                                    output_keys=['destination_locations',
                                                 'object_location'],
                                    outcomes=['floor_not_cleared',
@@ -23,8 +26,19 @@ def __init__(self, save_sm_state=False, **kwargs):
                                              'tidying_done'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'select_scanning_pose')
+        self.tidying_timeout_s = kwargs.get('tidying_timeout_s', 900.)
+        self.avg_obj_cleanup_duration_s = kwargs.get('avg_obj_cleanup_duration_s', 90.)
 
     def execute(self, userdata):
+        current_time = rospy.Time.now().to_sec()
+        operation_duration = current_time - userdata.operation_start_time
+        if (operation_duration + self.avg_obj_cleanup_duration_s) > self.tidying_timeout_s:
+            rospy.loginfo('[%s] Interrupting tidying up in order not to exceed timeout of %f s',
+                          self.state_name, self.tidying_timeout_s)
+            return 'tidying_done'
+
+        rospy.loginfo('[%s] Will carry on tidying up; used %f seconds so far',
+                      self.state_name, operation_duration)
         dirty_locations = []
         if False in userdata.floor_objects_cleared.values():
             dirty_locations = get_dict_keys_with_false_values(userdata.floor_objects_cleared)

From db2961b141f5a746df05acc88cdba9c2bf4f493b Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 16 Dec 2020 07:04:04 +0100
Subject: [PATCH 228/264] [wrs_tidy_up/states/pickup] Removed the grasping
 offset for top-down grasps

This seemed to cause problems for large objects
---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index 5c60b9a26..b80e5bb67 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -32,7 +32,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.pickup_goal_pose_topic = kwargs.get('pickup_goal_pose_topic',
                                                  '/pickup_server/goal_pose')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
-        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.)
         self.retry_count = 0
         self.__init_ros_components()
 

From fbd19f8b0dea0bad800d5f973973b89455b88064 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 16 Dec 2020 09:42:30 +0100
Subject: [PATCH 229/264] [wrs_tidy_up/config/sm] Retry pickup only once

If things start going wrong (e.g. an object slips continuously),
the robot gets stuck in the pickup state for too long without making any
progress in the task
---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index 72da42909..db027ae16 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -133,7 +133,7 @@ state_descriptions:
                 value: 45.0
             - argument:
                 name: number_of_retries
-                value: 3
+                value: 1
     - state:
         name: SELECT_STORAGE_LOCATION
         state_module_name: mdr_wrs_tidy_up.scenario_states.select_storage_location

From 97009f1d1173c39ab3f8c0d973c052099f35e217 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 16 Dec 2020 09:59:08 +0100
Subject: [PATCH 230/264] [wrs_tidy_up/states/select_object_for_picking] Added
 debugging msg for position and size of selected object

---
 .../scenario_states/select_object_for_picking.py       | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
index d2728f3f8..37b2ef705 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_object_for_picking.py
@@ -33,5 +33,13 @@ def execute(self, userdata):
                 closest_obj_index = index
                 closest_obj_distance = distance_to_obj
 
-        userdata.selected_object = userdata.detected_objects[closest_obj_index]
+        selected_object = userdata.detected_objects[closest_obj_index]
+        rospy.loginfo('[%s] Selected object at position\n    (%f, %f, %f) \n with size\n    (%f, %f, %f)',
+                      self.state_name, selected_object.pose.pose.position.x,
+                      selected_object.pose.pose.position.y,
+                      selected_object.pose.pose.position.z,
+                      selected_object.dimensions.vector.x,
+                      selected_object.dimensions.vector.y,
+                      selected_object.dimensions.vector.z)
+        userdata.selected_object = selected_object
         return 'succeeded'

From a25f5de63652def90882aed53def1f3a83dbe77c Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Wed, 16 Dec 2020 17:12:24 +0100
Subject: [PATCH 231/264] [wrs_tidy_up/states/find_objects]  Filter large and
 flat objects on ground

---
 .../scenario_states/find_objects.py           | 43 +++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 93e6b9a8e..28ce73723 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -2,6 +2,7 @@
 from shapely.geometry import Point, Polygon
 
 import rospy
+from geometry_msgs.msg import PoseArray
 import tf
 import actionlib
 from std_msgs.msg import Bool
@@ -27,6 +28,11 @@ class FindObjects(ScenarioStateBase):
     obstacle_cache_reset_pub = None
     last_cloud_object_detection_time = None
     retry_count = 0
+    large_object_size_threshold = None
+    large_object_height_threshold = None
+    large_object_aspect_ratio_threshold = None
+    filtered_poses_topic = None
+    filtered_poses_pub = None
 
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'find_objects',
@@ -53,6 +59,11 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.min_allowed_dist_to_leg = kwargs.get('min_allowed_dist_to_leg', 0.15)
         self.tables_to_clean_up = kwargs.get('tables_to_clean_up', ['long_table_b', 'tall_table'])
         self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.large_object_size_threshold = kwargs.get('large_object_size_threshold', 0.15)
+        self.large_object_height_threshold = kwargs.get('large_object_height_threshold', 0.075)
+        self.large_object_aspect_ratio_threshold = kwargs.get('large_object_aspect_ratio_threshold', 0.4)
+        self.filtered_poses_topic = kwargs.get('filtered_poses_topic', 'filtered_object_poses')
+        self.filtered_poses_pub = rospy.Publisher(self.filtered_poses_topic, PoseArray, queue_size=1)
         self.__init_ros_components()
 
     def execute(self, userdata):
@@ -75,6 +86,7 @@ def execute(self, userdata):
                                                                          userdata.environment_objects)
             filtered_objects = self.filter_objects_under_tables(filtered_objects,
                                                                 userdata.environment_objects)
+            filtered_objects = self.filter_large_objects(filtered_objects)
         userdata.detected_objects = filtered_objects
 
         # if no objects are seen in the current view, we register the location as "cleared"
@@ -85,6 +97,13 @@ def execute(self, userdata):
             elif userdata.object_location == 'table':
                 userdata.table_objects_cleared[current_location] = True
             return 'no_objects'
+        else:
+            pose_array = PoseArray()
+            pose_array.header.frame_id = filtered_objects[0].pose.header.frame_id
+            pose_array.header.stamp = rospy.Time.now()
+            for obj in filtered_objects:
+                pose_array.poses.append(obj.pose.pose)
+            self.filtered_poses_pub.publish(pose_array)
 
         return 'succeeded'
 
@@ -153,6 +172,30 @@ def filter_objects_under_tables(self, objects, environment_objects):
         rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
         return filtered_objects
 
+    def filter_large_objects(self, objects):
+        rospy.loginfo('[%s] Filtering large objects with params: max_size(%f), max_height(%f), max_aspect_ratio(%f)',
+                      self.state_name, self.large_object_size_threshold, 
+                      self.large_object_height_threshold, 
+                      self.large_object_aspect_ratio_threshold)
+
+        filtered_objects = []
+        for obj in objects:
+            if obj.dimensions.vector.x <= self.large_object_size_threshold and \
+               obj.dimensions.vector.y <= self.large_object_size_threshold:
+               # The object is not considered as a large object
+               filtered_objects.append(obj)
+            else:
+                # Found a large object
+                if obj.dimensions.vector.z <= self.large_object_height_threshold:
+                    dimensions = obj.bounding_box.dimensions
+                    obb_aspect_ratio_xy = dimensions.y / dimensions.x if dimensions.y < dimensions.x else \
+                                          dimensions.x / dimensions.y
+                    if obb_aspect_ratio_xy <= self.large_object_aspect_ratio_threshold:
+                        filtered_objects.append(obj)
+
+        rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
+        return filtered_objects
+
     def __init_ros_components(self):
         '''Initialises:
         * an object detection action client

From 92e67e1439e417149ea3131a4f1e56eb1edbe8da Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 16 Dec 2020 17:19:00 +0100
Subject: [PATCH 232/264] [wrs_tidy_up/config/sm] Removed grasp retries

For increased reliability, it seems better to re-perceive the scene
since the previous grasping may have caused changes
---
 .../mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index db027ae16..eb65b5b4e 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -133,7 +133,7 @@ state_descriptions:
                 value: 45.0
             - argument:
                 name: number_of_retries
-                value: 1
+                value: 0
     - state:
         name: SELECT_STORAGE_LOCATION
         state_module_name: mdr_wrs_tidy_up.scenario_states.select_storage_location

From 5a558d0f472f6c05184c9daf1cfa01f8db18da40 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 17 Dec 2020 16:26:58 +0100
Subject: [PATCH 233/264] [wrs_tidy_up/states/pickup] Static objects are now
 added to the planning scene before grasping (and removed afterwards)

---
 .../scenario_states/pickup_object.py          | 44 ++++++++++++++++++-
 1 file changed, 43 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index b80e5bb67..c193f6daf 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -5,6 +5,8 @@
 import tf
 
 from geometry_msgs.msg import Pose, PoseStamped
+from mas_perception_msgs.msg import ObjectList
+from mdr_manipulation_msgs.srv import UpdatePlanningScene, UpdatePlanningSceneRequest
 from mdr_pickup_action.msg import PickupAction, PickupGoal
 
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
@@ -17,13 +19,16 @@ class PickupObject(ScenarioStateBase):
     tf_listener = None
     grasping_timeout_s = 30.
     grasping_height_offset = 0.
+    planning_scene_update_service_name = ''
+    planning_scene_update_proxy = None
 
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'pickup_object',
                                    save_sm_state=save_sm_state,
                                    outcomes=['succeeded', 'failed',
                                              'failed_after_retrying'],
-                                   input_keys=['selected_object'],
+                                   input_keys=['selected_object',
+                                               'detected_objects'],
                                    output_keys=['grasped_object'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'pickup_object')
@@ -33,11 +38,16 @@ def __init__(self, save_sm_state=False, **kwargs):
                                                  '/pickup_server/goal_pose')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
         self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.)
+        self.planning_scene_update_service_name = kwargs.get('planning_scene_update_service_name',
+                                                             '/move_arm_action/update_planning_scene')
         self.retry_count = 0
         self.__init_ros_components()
 
     def execute(self, userdata):
         object_to_pick_up = userdata.selected_object
+        objects_except_target = [obj for obj in userdata.detected_objects
+                                 if obj.name != object_to_pick_up.name]
+        self.update_planning_scene(objects_except_target, UpdatePlanningSceneRequest.ADD)
 
         goal = PickupGoal()
         goal.pose.header.frame_id = object_to_pick_up.pose.header.frame_id
@@ -60,6 +70,7 @@ def execute(self, userdata):
             if pickup_result.success:
                 rospy.loginfo('[%s] Successfully grasped object %s', self.state_name, object_to_pick_up.name)
                 userdata.grasped_object = object_to_pick_up
+                self.update_planning_scene(objects_except_target, UpdatePlanningSceneRequest.REMOVE)
                 return 'succeeded'
             else:
                 rospy.logerr('[%s] Failed to grasp object', self.state_name)
@@ -71,12 +82,35 @@ def execute(self, userdata):
         if self.retry_count == self.number_of_retries:
             rospy.logerr('[%s] Could not pick up object after retrying; giving up', self.state_name)
             self.retry_count = 0
+            self.update_planning_scene(objects_except_target, UpdatePlanningSceneRequest.REMOVE)
             return 'failed_after_retrying'
 
         rospy.loginfo('[%s] Retrying to place object', self.state_name)
         self.retry_count += 1
+        self.update_planning_scene(objects_except_target, UpdatePlanningSceneRequest.REMOVE)
         return 'failed'
 
+    def update_planning_scene(self, objects, operation):
+        object_list = ObjectList()
+        object_list.objects = objects
+
+        update_planning_scene_req = UpdatePlanningSceneRequest()
+        update_planning_scene_req.objects = object_list
+        update_planning_scene_req.operation = operation
+        if operation == UpdatePlanningSceneRequest.ADD:
+            rospy.loginfo('[%s] Adding static objects to planning scene', self.state_name)
+        elif operation == UpdatePlanningSceneRequest.REMOVE:
+            rospy.loginfo('[%s] Removing static objects from planning scene', self.state_name)
+
+        response = self.planning_scene_update_proxy(update_planning_scene_req)
+        if response is not None:
+            if response.success:
+                rospy.loginfo('[%s] Successfully updated the planning scene', self.state_name)
+            else:
+                rospy.logerr('[%s] Failed to update the planning scene', self.state_name)
+        else:
+            rospy.logerr('[%s] Response not received', self.state_name)
+
     def get_grasping_pose_and_strategy(self, object_to_pick_up):
         '''Returns a geometry_msgs.msg.Pose object representing a grasping pose
         for the given object. The grasping position is given by the center of the
@@ -151,6 +185,14 @@ def __init_ros_components(self):
         self.pickup_client.wait_for_server()
         rospy.loginfo('Client for action %s initialised', self.pickup_server_name)
 
+        rospy.loginfo('[%s] Creating a service proxy for %s',
+                      self.state_name, self.planning_scene_update_service_name)
+        rospy.wait_for_service(self.planning_scene_update_service_name)
+        self.planning_scene_update_proxy = rospy.ServiceProxy(self.planning_scene_update_service_name,
+                                                              UpdatePlanningScene)
+        rospy.loginfo('[%s] Service proxy for %s created',
+                      self.state_name, self.planning_scene_update_service_name)
+
         rospy.loginfo('Initialising publisher for goal pose on topic %s', self.pickup_goal_pose_topic)
         self.goal_pose_pub = rospy.Publisher(self.pickup_goal_pose_topic, PoseStamped, queue_size=1)
         rospy.loginfo('Publisher for topic %s initialised', self.pickup_goal_pose_topic)

From 0277294c4b8edbbd72192e3438ba6cd15694b9b1 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 17 Dec 2020 16:27:43 +0100
Subject: [PATCH 234/264] [wrs_tidy_up/states/pickup] Reintroduced a small
 grasping offset for top-down grasps

Otherwise, planning seems to fail quite often for objects on tables
(due to collisions with the table plane)
---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index c193f6daf..f3528c861 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -37,7 +37,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.pickup_goal_pose_topic = kwargs.get('pickup_goal_pose_topic',
                                                  '/pickup_server/goal_pose')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
-        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.025)
         self.planning_scene_update_service_name = kwargs.get('planning_scene_update_service_name',
                                                              '/move_arm_action/update_planning_scene')
         self.retry_count = 0

From e72d716e75c0f4aacca6504c50e6fe0c1db6a6ad Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 17 Dec 2020 21:47:34 +0100
Subject: [PATCH 235/264] [wrs_tidy_up/states/find_objects] Added object
 detection timeout; filtered objects between the two tables

---
 .../scenario_states/find_objects.py           | 29 ++++++++++++++++---
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 28ce73723..33ce45a32 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -49,6 +49,7 @@ def __init__(self, save_sm_state=False, **kwargs):
                                                 'table_objects_cleared'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'find_objects')
+        self.object_detection_timeout_s = kwargs.get('object_detection_timeout', 5.)
         self.object_detection_server_name = kwargs.get('object_detection_server_name',
                                                        '/mas_perception/detect_objects')
         self.cloud_obstacle_detection_topic = kwargs.get('cloud_obstacle_detection_topic',
@@ -74,8 +75,18 @@ def execute(self, userdata):
         rospy.sleep(0.5)
 
         rospy.loginfo('[%s] Waiting for cloud obstacle detection', self.state_name)
-        while last_msg_time == self.last_cloud_object_detection_time:
+        timeout_reached = False
+        waiting_start_time = rospy.Time.now().to_sec()
+        while abs(last_msg_time - self.last_cloud_object_detection_time) < 1e-5 and \
+              not timeout_reached:
             rospy.sleep(0.05)
+            if (rospy.Time.now().to_sec() - waiting_start_time) > self.object_detection_timeout_s:
+                timeout_reached = True
+
+        if timeout_reached:
+            rospy.loginfo('[%s] No objects detected within %f seconds; giving up',
+                          self.state_name, self.object_detection_timeout_s)
+            return 'no_objects'
 
         rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))
 
@@ -160,13 +171,23 @@ def filter_objects_under_tables(self, objects, environment_objects):
                                               table_obj.dimensions.vector)
             table_plane_polygons.append(plane_polygon)
 
+        polygon_convex_hull = None
+        if table_plane_polygons:
+            polygon_union = table_plane_polygons[0]
+            for polygon in table_plane_polygons:
+                polygon_union = polygon_union.union(polygon)
+            polygon_convex_hull = polygon_union.convex_hull
+
         point_in_any_polygon = lambda point: sum([1 for polygon in table_plane_polygons
                                                   if polygon.contains(point)]) > 0
 
+        point_in_union_convex_hull = lambda point: False if polygon_convex_hull is None \
+                                                         else polygon_convex_hull.contains(point)
+
         filtered_objects = []
         for obj in objects:
             obj_point = Point(obj.pose.pose.position.x, obj.pose.pose.position.y)
-            if not point_in_any_polygon(obj_point):
+            if not point_in_any_polygon(obj_point) and not point_in_union_convex_hull(obj_point):
                 filtered_objects.append(obj)
 
         rospy.loginfo('[%s] Keeping %d objects', self.state_name, len(filtered_objects))
@@ -174,8 +195,8 @@ def filter_objects_under_tables(self, objects, environment_objects):
 
     def filter_large_objects(self, objects):
         rospy.loginfo('[%s] Filtering large objects with params: max_size(%f), max_height(%f), max_aspect_ratio(%f)',
-                      self.state_name, self.large_object_size_threshold, 
-                      self.large_object_height_threshold, 
+                      self.state_name, self.large_object_size_threshold,
+                      self.large_object_height_threshold,
                       self.large_object_aspect_ratio_threshold)
 
         filtered_objects = []

From b88b96f407479ec27c307f96e569a53fc5c78bbf Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 09:35:01 +0100
Subject: [PATCH 236/264] [wrs_tidy_up/states/init] Fixed initialisation for
 task 2

The initialisation was crashing without checks on the arguments (as some
of them are not passed for the second task)
---
 .../scenario_states/initialise_scenario.py       | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
index fd08b06e3..e8713dde8 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/initialise_scenario.py
@@ -64,16 +64,24 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.__init_ros_components()
 
     def execute(self, userdata):
-        userdata.floor_objects_cleared = dict(self.floor_objects_cleared)
-        userdata.table_objects_cleared = dict(self.table_objects_cleared)
-        userdata.object_location = self.object_location
+        rospy.loginfo('[%s] Updating object detection parameters for floor')
+        update_object_detection_params("floor")
+
+        if self.floor_objects_cleared:
+            userdata.floor_objects_cleared = dict(self.floor_objects_cleared)
+
+        if self.table_objects_cleared:
+            userdata.table_objects_cleared = dict(self.table_objects_cleared)
+
+        if self.object_location:
+            userdata.object_location = self.object_location
+
         userdata.operation_start_time = rospy.Time.now().to_sec()
 
         if not self.planning_scene_map_file:
             rospy.loginfo('[%s] Planning scene map file not specified; not initialising KB and scene', self.state_name)
             return 'succeeded'
 
-        update_object_detection_params("floor")
         environment_objects = get_environment_objects(self.planning_scene_map_file)
 
         # initialising the knowledge base

From 4620e1dee341ba16306021f0e89d0992ba71df2a Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 09:44:05 +0100
Subject: [PATCH 237/264] [wrs_tidy_up/config/sm] Increased pickup timeout and
 removed tries for obstacle picking state

---
 .../mdr_wrs_tidy_up/config/tidy_up_sm.yaml                | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
index eb65b5b4e..c975dfb37 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/tidy_up_sm.yaml
@@ -295,9 +295,15 @@ state_descriptions:
                 name: failed_after_retrying
                 state: GO_TO_SECOND_ROOM_CORRIDOR
         arguments:
+            - argument:
+                name: pickup_server_name
+                value: pickup_server
+            - argument:
+                name: grasping_timeout_s
+                value: 45.0
             - argument:
                 name: number_of_retries
-                value: 3
+                value: 0
     - state:
         name: MOVE_TO_BOX
         state_module_name: mdr_wrs_tidy_up.scenario_states.move_base

From 51d0c6c36b185ca9924f2f5dfb4aaaa9709a5324 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 11:02:50 +0100
Subject: [PATCH 238/264] [wrs_tidy_up/states/find_objects] Set location to
 cleared if object detection timeout reached

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py  | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 33ce45a32..0df3a8a45 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -86,6 +86,11 @@ def execute(self, userdata):
         if timeout_reached:
             rospy.loginfo('[%s] No objects detected within %f seconds; giving up',
                           self.state_name, self.object_detection_timeout_s)
+            current_location = userdata.destination_locations[0]
+            if userdata.object_location == 'floor':
+                userdata.floor_objects_cleared[current_location] = True
+            elif userdata.object_location == 'table':
+                userdata.table_objects_cleared[current_location] = True
             return 'no_objects'
 
         rospy.loginfo('[%s] Detected %d objects', self.state_name, len(self.detected_cloud_objects))

From 00a833096646b62e72e21ce0033379121a517775 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 11:16:17 +0100
Subject: [PATCH 239/264] [wrs_tidy_up/config/planning_scene] Made the walls
 higher (0.6m -> 2m)

This is to prevent moveit from creating plans from outside a room
---
 .../config/planning_scene_map.yaml            | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
index 91d507db0..dfcfc2e20 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
@@ -6,29 +6,29 @@ frame_id: map
 objects:
     # object ID: wall_back
     - name: wall_back
-      position: [-0.8, 2.7, 0.3]
+      position: [-0.8, 2.7, 1.0]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [0.1, 4.8, 0.6]
+      dimensions: [0.1, 4.8, 2.0]
     # object ID: wall_front
     - name: wall_front
-      position: [3.2, 2.1, 0.3]
+      position: [3.2, 2.1, 1.0]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [0.1, 6.0, 0.6]
+      dimensions: [0.1, 6.0, 2.0]
     # object ID: wall_left
     - name: wall_left
-      position: [1.2, -0.9, 0.3]
+      position: [1.2, -0.9, 1.0]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [4.1, 0.1, 0.6]
+      dimensions: [4.1, 0.1, 2.0]
     # object ID: wall_right
     - name: wall_right
-      position: [1.2, 5.1, 0.3]
+      position: [1.2, 5.1, 1.0]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [4.1, 0.1, 0.6]
+      dimensions: [4.1, 0.1, 2.0]
     # object ID: wall_center
     - name: wall_center
-      position: [0.6, 2.1, 0.3]
+      position: [0.6, 2.1, 1.0]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [2.8, 0.1, 0.6]
+      dimensions: [2.8, 0.1, 2.0]
 
     # object ID: wrc_bin_green
     - name: bin_a

From f155e20a5179b35147fcf0ff930e3ce4c6eac224 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Fri, 18 Dec 2020 11:28:17 +0100
Subject: [PATCH 240/264] [wrs_tidy_up/config/object detection] Tweaked the
 z_min point cloud cuttoff to detect objects on tables and shelves

---
 .../mdr_wrs_tidy_up/config/object_detection_params.yaml   | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
index 5a8614702..153a2e950 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/object_detection_params.yaml
@@ -2,11 +2,11 @@ floor:
   z_min: 0.01
   z_max: 0.2
 long_table_b:
-  z_min: 0.42
+  z_min: 0.415
   z_max: 0.6
 tall_table:
-  z_min: 0.62
+  z_min: 0.615
   z_max: 0.8
 shelf:
-  z_min: 0.5
-  z_max: 0.9
+  z_min: 0.51
+  z_max: 1.0

From e304f6c85dc75f4d1eaff5dab65304793176eea0 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 12:14:12 +0100
Subject: [PATCH 241/264] [wrs_tidy_up/config/planning_scene] Made tables
 narrower

---
 .../mdr_wrs_tidy_up/config/planning_scene_map.yaml          | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
index dfcfc2e20..4816e0ab3 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/config/planning_scene_map.yaml
@@ -46,7 +46,7 @@ objects:
     - name: long_table_a
       position:   [1.5, -0.6, 0.39]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [1.2, 0.4, 0.02]
+      dimensions: [1.2, 0.4, 0.01]
     - name: long_table_a_support
       position:   [1.5, -0.6, 0.29]
       orientation: [0.0, 0.0, 0.0]
@@ -72,7 +72,7 @@ objects:
     - name: tall_table
       position:   [0.0, 1.8, 0.59]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [0.40, 0.40, 0.02]
+      dimensions: [0.40, 0.40, 0.01]
     - name: tall_table_support
       position:   [0.0, 1.8, 0.49]
       orientation: [0.0, 0.0, 0.0]
@@ -98,7 +98,7 @@ objects:
     - name: long_table_b
       position:   [1.0, 1.8, 0.39]
       orientation: [0.0, 0.0, 0.0]
-      dimensions: [1.2, 0.4, 0.02]
+      dimensions: [1.2, 0.4, 0.01]
     - name: long_table_b_support
       position:   [1.0, 1.8, 0.29]
       orientation: [0.0, 0.0, 0.0]

From c9939e91bb8b8545ea82eff7e58db00e2c4289e5 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Fri, 18 Dec 2020 12:47:40 +0100
Subject: [PATCH 242/264] [wrs_tidy_up/states/pickup] After all tests, a height
 offset of 5cm seems the most reliable in general

---
 .../ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
index f3528c861..6ca690536 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/pickup_object.py
@@ -37,7 +37,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.pickup_goal_pose_topic = kwargs.get('pickup_goal_pose_topic',
                                                  '/pickup_server/goal_pose')
         self.grasping_timeout_s = kwargs.get('grasping_timeout_s', 30.)
-        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.025)
+        self.grasping_height_offset = kwargs.get('grasping_height_offset', 0.05)
         self.planning_scene_update_service_name = kwargs.get('planning_scene_update_service_name',
                                                              '/move_arm_action/update_planning_scene')
         self.retry_count = 0

From 252f2943252ef654558140272559132b41aace12 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 10:43:54 +0100
Subject: [PATCH 243/264] Copy over the cloud object detection code from
 mas_perception_libs and remove dependency on mas_perception_libs

---
 .../mdr_cloud_object_detection/CMakeLists.txt |  60 ++
 .../mdr_cloud_object_detection/package.xml    |  32 +
 .../ros/config/ObjectDetection.cfg            |  83 ++
 .../object_detection_default_configs.yaml     |  23 +
 .../mdr_cloud_object_detection/aliases.h      |  18 +
 .../cloud_object_detection.h                  |   1 +
 .../mdr_cloud_object_detection/color.h        | 108 +++
 .../ros/src/cloud_object_detection.cpp        | 907 ++++++++++++++++++
 8 files changed, 1232 insertions(+)
 create mode 100644 mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
 create mode 100644 mdr_perception/mdr_cloud_object_detection/package.xml
 create mode 100755 mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
new file mode 100644
index 000000000..34d1f0daa
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_cloud_object_detection)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib_msgs
+  actionlib
+  dynamic_reconfigure
+  pcl_ros
+  roscpp
+  mas_navigation_tools
+  mas_perception_msgs
+)
+
+find_package(PCL 1.7 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS python)
+
+generate_dynamic_reconfigure_options(
+  ros/config/ObjectDetection.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    ros/include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    mas_navigation_tools
+)
+
+###########
+## Build ##
+###########
+include_directories(
+  ros/include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+#   ${Boost_INCLUDE_DIRS}
+)
+
+###################################
+# Node for object detection
+add_executable(cloud_object_detection
+  ros/src/cloud_object_detection.cpp
+)
+target_link_libraries(cloud_object_detection
+#   ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+#   ${OpenCV_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS cloud_object_detection
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/mdr_perception/mdr_cloud_object_detection/package.xml b/mdr_perception/mdr_cloud_object_detection/package.xml
new file mode 100644
index 000000000..804112b82
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/package.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mdr_cloud_object_detection</name>
+  <version>1.0.0</version>
+  <description>The ros package to detect object clusters in a point cloud</description>
+
+  <author email="sushant.chavan@smail.inf.h-brs.de">Sushant Chavan</author>
+  <maintainer email="sushant.chavan@smail.inf.h-brs.de">Sushant Chavan</maintainer>
+  <maintainer email="aleksandar.mitrevski@h-brs.de">Alex Mitrevski</maintainer>
+  <maintainer email="robotics@inf.h-brs.de">MAS robotics</maintainer>
+
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>dynamic_reconfigure</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>mas_navigation_tools</depend>
+  <depend>mas_perception_msgs</depend>
+
+  <!-- <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>pcl_ros</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend> -->
+  <build_export_depend>message_runtime</build_export_depend>
+
+  <!-- <exec_depend>dynamic_reconfigure</exec_depend> -->
+
+  <exec_depend>message_runtime</exec_depend>
+  <!-- <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>roscpp</exec_depend> -->
+</package>
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg b/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
new file mode 100755
index 000000000..e871cda57
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
@@ -0,0 +1,83 @@
+#!/usr/bin/env python2
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t
+
+
+PACKAGE='mdr_cloud_object_detection'
+NODE_NAME = PACKAGE
+
+gen = ParameterGenerator()
+
+# cloud filter group
+cloud_filter_params = gen.add_group("Cloud Filter")
+
+# pass-through filter parameters
+cloud_filter_params.add("passthrough_limit_min_x", double_t, 0,
+                        "Minimum field value of a point along x-axis for it to be considered", 0.0, 0.0, 10.0)
+
+cloud_filter_params.add("passthrough_limit_max_x", double_t, 0,
+                        "Maximum field value of a point along x-axis for it to be considered", 3.0, 0.0, 10.0)
+
+cloud_filter_params.add("passthrough_limit_min_y", double_t, 0,
+                        "Minimum field value of a point along y-axis for it to be considered", -1.5, -3.0, 3.0)
+
+cloud_filter_params.add("passthrough_limit_max_y", double_t, 0,
+                        "Maximum field value of a point along y-axis for it to be considered", 1.5, -3.0, 3.0)
+
+cloud_filter_params.add("passthrough_limit_min_z", double_t, 0,
+                        "Minimum field value of a point along z-axis for it to be considered", 0.025, 0.0, 5.0)
+
+cloud_filter_params.add("passthrough_limit_max_z", double_t, 0,
+                        "Maximum field value of a point along z-axis for it to be considered", 0.2, 0.0, 5.0)
+
+# voxel-grid filter parameters; note that we limit the cloud along the z-axis using the voxel filter to avoid adding
+# a third pass-through filter
+cloud_filter_params.add("voxel_limit_min_z", double_t, 0,
+                        "Minimum field value of a point along z-axis for it to be considered", 0.0, -10.0, 10.0)
+cloud_filter_params.add("voxel_limit_max_z", double_t, 0,
+                        "Maximum field value of a point along z-axis for it to be considered", 1.0, -10.0, 10.0)
+cloud_filter_params.add("voxel_leaf_size", double_t, 0, "Size of a leaf (on x,y,z) used for downsampling.",
+                        0.01, 0, 1.0)
+
+# Euclidean Clustering
+euclidean_clustering_params = gen.add_group("Euclidean Clustering")
+
+# clustering parameters
+euclidean_clustering_params.add("cluster_tolerance", double_t, 0, 
+                        "Spatial cluster tolerance as a measure in the L2 Euclidean space",
+                        0.03, 0.0, 0.1)
+euclidean_clustering_params.add("min_cluster_size", int_t, 0, 
+                        "The minimum number of points that a cluster needs to contain in order to be considered valid",
+                        5, 0, 1000)
+euclidean_clustering_params.add("max_cluster_size", int_t, 0, 
+                        "The maximum number of points that a cluster needs to contain in order to be considered valid",
+                        100000, 0, 1000000)
+
+# Object Filtering
+object_filter_params = gen.add_group("Object Filter")
+object_filter_params.add("max_bbox_edge_length", double_t, 0, 
+                        "Maximum allowed length (in meters) of any of the bounding box edges for an object.\
+                         Any clusters with a length greater that this value will not be considered as objects.",
+                        .15, 0, 1)
+object_filter_params.add("min_dist_from_occupied_cell", double_t, 0, 
+                        "Minimum required distance (in meters) of a valid object from an occupied cell in the occupancy grid map.\
+                         Any clusters with a distance less that this value will not be considered as objects.",
+                        .1, 0, 1)
+
+# Object Cache
+object_cache_params = gen.add_group("Object Cache")
+object_cache_params.add("object_cache_time", double_t, 0, 
+                        "Maximum time (in seconds) for which an object can say in cache after it is no longer visible/detected.",
+                        5, 0, 100)
+object_cache_params.add("similarity_threshold", double_t, 0, 
+                        "threshold for identifying outliers and not considering those for the similarity \
+                         a good value for threshold is 5 * <cloud_resolution>, e.g. 10cm for a cloud with 2cm resolution",
+                        2.0, 0.0, 100.0)
+object_cache_params.add("position_history_cache_size", int_t, 0, 
+                        "Maximum number of past object positions to remember",
+                        10, 0, 1000)
+object_cache_params.add("uniqueness_threshold", double_t, 0, 
+                        "Threshold to determine a new cluster is the same as a cached cluster. \
+                         If the similarity measure is below this value, the new cluster is considered as a replica of a cached cluster.",
+                        0.005, 0.0, 10.0)
+
+exit (gen.generate (PACKAGE, NODE_NAME, "ObjectDetection"))
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
new file mode 100644
index 000000000..a62e4ec61
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
@@ -0,0 +1,23 @@
+# config file loaded during launch as default configurations for the cloud filter, assuming base_link frame for
+# passthrough filter
+passthrough_limit_min_x:  0.0
+passthrough_limit_max_x:  1.5
+passthrough_limit_min_y: -0.5
+passthrough_limit_max_y:  0.5
+passthrough_limit_min_z:  0.025
+passthrough_limit_max_z:  0.2
+voxel_limit_min_z:        0.0
+voxel_limit_max_z:        1.8
+voxel_leaf_size:          0.02
+# config file loaded during launch as default configurations for the clustering algorithm
+cluster_tolerance:       0.05
+min_cluster_size:        4
+max_cluster_size:        10000
+# Object Filtering
+max_bbox_edge_length:           0.3
+min_dist_from_occupied_cell:    0.05
+# Object Cache
+object_cache_time:            5.0
+similarity_threshold:           2.0
+position_history_cache_size:    10
+uniqueness_threshold:           0.005
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h
new file mode 100644
index 000000000..b90bd297f
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * Author: Minh Nguyen
+ * Author: Sushant Vijay Chavan
+ *
+ */
+#ifndef MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
+#define MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+typedef pcl::PointXYZRGB PointT;
+typedef pcl::PointCloud<PointT> PointCloud;
+typedef pcl::PointCloud<pcl::PointNormal> PointCloudNormal;
+
+#endif  // MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
new file mode 100644
index 000000000..751cc4a71
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
@@ -0,0 +1 @@
+#include<iostream>
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h
new file mode 100644
index 000000000..5996c6b83
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h
@@ -0,0 +1,108 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sergey Alexandrov
+ * @author Sushant Vijay Chavan
+ *
+ */
+#ifndef MDR_CLOUD_OBJECT_DETECTION_COLOR_H
+#define MDR_CLOUD_OBJECT_DETECTION_COLOR_H
+
+#include <std_msgs/ColorRGBA.h>
+#include <mdr_cloud_object_detection/aliases.h>
+
+namespace mdr_cloud_object_detection
+{
+
+struct Color
+{
+    uint8_t mRed;
+    uint8_t mGreen;
+    uint8_t mBlue;
+
+    Color(float pRed, float pGreen, float pBlue)
+    : mRed(static_cast<uint8_t>(pRed * 255)), mGreen(static_cast<uint8_t>(pGreen * 255)),
+      mBlue(static_cast<uint8_t>(pBlue * 255))
+    { }
+
+    Color(int pRed, int pGreen, int pBlue)
+    : mRed(static_cast<uint8_t>(pRed)), mGreen(static_cast<uint8_t>(pGreen)), mBlue(static_cast<uint8_t>(pBlue)) { }
+
+    enum Name
+    {
+        SALMON,
+        TEAL,
+        DEEP_PINK,
+        SANGRIA,
+        SEA_BLUE,
+        SEA_GREEN,
+        SCARLET,
+        PUMPKIN,
+        JASMINE,
+        IVORY,
+        GAINSBORO,
+    };
+
+    explicit Color(Name name)
+    {
+        switch (name)
+        {
+            case SALMON:
+                mRed = 0xFA, mGreen = 0x80, mBlue = 0x72;
+                break;
+            case TEAL:
+                mRed = 0x00, mGreen = 0x80, mBlue = 0x80;
+                break;
+            case DEEP_PINK:
+                mRed = 0xFF, mGreen = 0x14, mBlue = 0x93;
+                break;
+            case SANGRIA:
+                mRed = 0x92, mGreen = 0x00, mBlue = 0x0A;
+                break;
+            case SEA_BLUE:
+                mRed = 0x00, mGreen = 0x69, mBlue = 0x94;
+                break;
+            case SEA_GREEN:
+                mRed = 0x2E, mGreen = 0x8B, mBlue = 0x57;
+                break;
+            case SCARLET:
+                mRed = 0xFF, mGreen = 0x24, mBlue = 0x00;
+                break;
+            case PUMPKIN:
+                mRed = 0xFF, mGreen = 0x75, mBlue = 0x18;
+                break;
+            case JASMINE:
+                mRed = 0xF8, mGreen = 0xDE, mBlue = 0x7E;
+                break;
+            case IVORY:
+                mRed = 0xFF, mGreen = 0xFF, mBlue = 0xF0;
+                break;
+            case GAINSBORO:
+                mRed = 0xDC, mGreen = 0xDC, mBlue = 0xDC;
+                break;
+            default:
+                mRed = 0xFF, mGreen = 0xFF, mBlue = 0xFF;
+                break;
+        }
+    }
+
+    explicit operator float() const
+    {
+        PointT point(mRed, mGreen, mBlue);
+        return point.rgb;
+    }
+
+    explicit operator std_msgs::ColorRGBA() const
+    {
+        std_msgs::ColorRGBA color;
+        color.r = mRed / 255.0f;
+        color.g = mGreen / 255.0f;
+        color.b = mBlue / 255.0f;
+        color.a = 1.0f;
+        return color;
+    }
+};
+
+}   // namespace mdr_cloud_object_detection
+
+#endif  // MDR_CLOUD_OBJECT_DETECTION_COLOR_H
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp
new file mode 100644
index 000000000..04ec5afc1
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp
@@ -0,0 +1,907 @@
+/*!
+ * @copyright 2018 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Chavan
+ *
+ * @brief script to detect objects from a point cloud
+ */
+
+#include <cmath>
+#include <actionlib/client/simple_action_client.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/common/common.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <std_msgs/Bool.h>
+#include <tf/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <mas_navigation_tools/OccupancyCheckerAction.h>
+#include <mas_perception_msgs/ObjectList.h>
+#include <mdr_cloud_object_detection/color.h>
+#include <mdr_cloud_object_detection/ObjectDetectionConfig.h>
+
+// using namespace mdr_cloud_object_detection;
+
+namespace mdr_cloud_object_detection
+{
+
+/*!
+ * @brief struct containing parameters necessary for filtering point clouds
+ */
+struct CloudPassThroughVoxelFilterParams
+{
+    /* PassThrough filter parameters
+     * limit the cloud to be filtered points outside of these x, y, z ranges will be discarded */
+    float mPassThroughLimitMinX = 0.0f;
+    float mPassThroughLimitMaxX = 0.0f;
+    float mPassThroughLimitMinY = 0.0f;
+    float mPassThroughLimitMaxY = 0.0f;
+    float mPassThroughLimitMinZ = 0.0f;
+    float mPassThroughLimitMaxZ = 0.0f;
+    /* VoxelGrid filter parameters for down-sampling the cloud, also limit the cloud along the z axis */
+    float mVoxelLimitMinZ = 0.0f;
+    float mVoxelLimitMaxZ = 0.0f;
+    float mVoxelLeafSize = 0.0f;
+};
+
+/*!
+ * @brief struct containing parameters necessary for clustering point clouds
+ */
+struct EuclideanClusterParams
+{
+    float mClusterTolerance = 0.01f;
+    unsigned int mMinClusterSize = 1;
+    unsigned int mMaxClusterSize = std::numeric_limits<unsigned int>::max();
+};
+
+/*!
+ * @brief struct containing parameters necessary for filtering the detected objects
+ */
+struct ObjectFilterParams
+{
+    float mMaxBboxEdgeLength = 0.15f;
+    float mMinDistFromOccupiedCell = 0.1f;
+};
+
+/*!
+ * @brief struct containing parameters necessary for caching the detected objects
+ */
+struct ObjectCacheParams
+{
+    float mObjectCacheTime = 5.0f;
+    float mSimilarityThreshold = 2.0f;
+    int mPositionHistoryCacheSize = 10;
+    float mUniquenessThreshold = 0.005f;
+};
+
+/*!
+ * @brief class containing definition for filtering point clouds
+ */
+class CloudPassThroughVoxelFilter
+{
+public:
+    CloudPassThroughVoxelFilter() = default;
+
+    /*! @brief set parameters relevant to filtering cloud */
+    virtual void
+    setParams(const CloudPassThroughVoxelFilterParams& pParams)
+    {
+        /* pass-through params */
+        mPassThroughFilterX.setFilterFieldName("x");
+        mPassThroughFilterX.setFilterLimits(pParams.mPassThroughLimitMinX, pParams.mPassThroughLimitMaxX);
+        mPassThroughFilterY.setFilterFieldName("y");
+        mPassThroughFilterY.setFilterLimits(pParams.mPassThroughLimitMinY, pParams.mPassThroughLimitMaxY);
+        mPassThroughFilterZ.setFilterFieldName("z");
+        mPassThroughFilterZ.setFilterLimits(pParams.mPassThroughLimitMinZ, pParams.mPassThroughLimitMaxZ);
+
+        /* filter z-axis using voxel filter instead of making another member */
+        mVoxelGridFilter.setFilterFieldName("z");
+        mVoxelGridFilter.setFilterLimits(pParams.mVoxelLimitMinZ, pParams.mVoxelLimitMaxZ);
+
+        /* voxel-grid params */
+        mVoxelGridFilter.setLeafSize(pParams.mVoxelLeafSize, pParams.mVoxelLeafSize, pParams.mVoxelLeafSize);
+    }
+
+    /*!
+    * @brief filter point cloud using passthrough and voxel filters
+    */
+    PointCloud::Ptr
+    filterCloud(const PointCloud::ConstPtr &pCloudPtr)
+    {
+        PointCloud::Ptr filteredCloudPtr = boost::make_shared<PointCloud>();
+
+        mPassThroughFilterX.setInputCloud(pCloudPtr);
+        mPassThroughFilterX.filter(*filteredCloudPtr);
+
+        mPassThroughFilterY.setInputCloud(filteredCloudPtr);
+        mPassThroughFilterY.filter(*filteredCloudPtr);
+
+        mPassThroughFilterZ.setInputCloud(filteredCloudPtr);
+        mPassThroughFilterZ.filter(*filteredCloudPtr);
+
+        mVoxelGridFilter.setInputCloud(filteredCloudPtr);
+        mVoxelGridFilter.filter(*filteredCloudPtr);
+
+        return filteredCloudPtr;
+    }
+
+private:
+    pcl::PassThrough<PointT> mPassThroughFilterX;
+    pcl::PassThrough<PointT> mPassThroughFilterY;
+    pcl::PassThrough<PointT> mPassThroughFilterZ;
+    pcl::VoxelGrid<PointT> mVoxelGridFilter;
+};
+
+class CloudObjectDetectionNode
+{
+private:
+    ros::NodeHandle mNodeHandle;
+    dynamic_reconfigure::Server<ObjectDetectionConfig> mObjectDetectionConfigServer;
+    ros::Subscriber mCloudSub;
+    ros::Subscriber mResetSub;
+    ros::Publisher mFilteredCloudPub;
+    ros::Publisher mObjectCloudPub;
+    ros::Publisher mObjectBoundsPub;
+    ros::Publisher mObjectObjectsPub;
+    actionlib::SimpleActionClient<mas_navigation_tools::OccupancyCheckerAction> mOccupancyCheckerClient;
+    tf::TransformListener mTfListener;
+    std::string mTransformTargetFrame;
+    std::string mClusterTargetFrame;
+    CloudPassThroughVoxelFilter mCloudFilter;
+    EuclideanClusterParams mClusterParams;
+    ObjectFilterParams mObjectFilterParams;
+    ObjectCacheParams mObjectCacheParams;
+    bool mPublishOrientedBBox;
+
+    unsigned int mUniqueObjectId;
+    unsigned int mCurrTime;
+    std::map<int, int> mLastSeenTimeCache;
+    std::map<int, const PointCloud::Ptr> mObjectsCache;
+    std::map<int, std::vector<Eigen::Vector4f>> mPrevPositionsCache;
+    std::map<int, mas_perception_msgs::Object> mObjectMsgCache;
+
+public:
+    CloudObjectDetectionNode(const ros::NodeHandle &pNodeHandle, const std::string &pCloudTopic,
+            const std::string &pFilteredCloudTopic,
+            const std::string &pObjectCloudTopic,
+            const std::string &pTransformTargetFrame,
+            const std::string &pClusterTargetFrame,
+            const std::string &pOccupancyCheckerActionName,
+            const std::string &pObjectsBoundsTopic,
+            const std::string &pObjectObjectsTopic,
+            bool pPublishOrientedBBox)
+    : mNodeHandle(pNodeHandle), mObjectDetectionConfigServer(mNodeHandle),
+      mTransformTargetFrame(pTransformTargetFrame), mClusterTargetFrame(pClusterTargetFrame),
+      mPublishOrientedBBox(pPublishOrientedBBox), mUniqueObjectId(0),
+      mOccupancyCheckerClient(pOccupancyCheckerActionName, true)
+    {
+        ROS_INFO("[ObjectDetectionNode] Waiting for OccupancyChecker server");
+        mOccupancyCheckerClient.waitForServer();
+
+        ROS_INFO("setting up dynamic reconfiguration server for object detection");
+        auto odCallback = boost::bind(&CloudObjectDetectionNode::objectDetectionConfigCallback, this, _1, _2);
+        mObjectDetectionConfigServer.setCallback(odCallback);
+
+        ROS_INFO("subscribing to point cloud topic and advertising processed result");
+        mCloudSub = mNodeHandle.subscribe(pCloudTopic, 1, &CloudObjectDetectionNode::cloudCallback, this);
+        mResetSub = mNodeHandle.subscribe("reset_cache", 1, &CloudObjectDetectionNode::resetCallback, this);
+        mFilteredCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pFilteredCloudTopic, 1);
+        mObjectCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pObjectCloudTopic, 1);
+        mObjectBoundsPub = mNodeHandle.advertise<visualization_msgs::MarkerArray>(pObjectsBoundsTopic, 1);
+        mObjectObjectsPub = mNodeHandle.advertise<mas_perception_msgs::ObjectList>(pObjectObjectsTopic, 1);
+    }
+
+private:
+    void
+    objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig, uint32_t pLevel)
+    {
+        // Cloud Filter params
+        CloudPassThroughVoxelFilterParams cloudFilterParams;
+        cloudFilterParams.mPassThroughLimitMinX = static_cast<float>(pConfig.passthrough_limit_min_x);
+        cloudFilterParams.mPassThroughLimitMaxX = static_cast<float>(pConfig.passthrough_limit_max_x);
+        cloudFilterParams.mPassThroughLimitMinY = static_cast<float>(pConfig.passthrough_limit_min_y);
+        cloudFilterParams.mPassThroughLimitMaxY = static_cast<float>(pConfig.passthrough_limit_max_y);
+        cloudFilterParams.mPassThroughLimitMinZ = static_cast<float>(pConfig.passthrough_limit_min_z);
+        cloudFilterParams.mPassThroughLimitMaxZ = static_cast<float>(pConfig.passthrough_limit_max_z);
+        cloudFilterParams.mVoxelLimitMinZ = static_cast<float>(pConfig.voxel_limit_min_z);
+        cloudFilterParams.mVoxelLimitMaxZ = static_cast<float>(pConfig.voxel_limit_max_z);
+        cloudFilterParams.mVoxelLeafSize = static_cast<float>(pConfig.voxel_leaf_size);
+        mCloudFilter.setParams(cloudFilterParams);
+
+        // Cloud Clustering params
+        mClusterParams.mClusterTolerance = static_cast<float>(pConfig.cluster_tolerance);
+        mClusterParams.mMinClusterSize = static_cast<unsigned int>(pConfig.min_cluster_size);
+        mClusterParams.mMaxClusterSize = static_cast<unsigned int>(pConfig.max_cluster_size);
+
+        // Object filter params
+        mObjectFilterParams.mMaxBboxEdgeLength = static_cast<float>(pConfig.max_bbox_edge_length);
+        mObjectFilterParams.mMinDistFromOccupiedCell = static_cast<float>(pConfig.min_dist_from_occupied_cell);
+
+        // Object cache params
+        mObjectCacheParams.mObjectCacheTime = static_cast<unsigned int>(pConfig.object_cache_time);
+        mObjectCacheParams.mSimilarityThreshold = static_cast<float>(pConfig.similarity_threshold);
+        mObjectCacheParams.mUniquenessThreshold = static_cast<float>(pConfig.uniqueness_threshold);
+        mObjectCacheParams.mPositionHistoryCacheSize = static_cast<float>(pConfig.position_history_cache_size);
+    }
+
+    void
+    cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pCloudMsgPtr)
+    {
+        // do not process cloud when there's no subscriber
+        if (mFilteredCloudPub.getNumSubscribers() == 0 &&
+            mObjectBoundsPub.getNumSubscribers() == 0 &&
+            mObjectObjectsPub.getNumSubscribers() == 0 &&
+            mObjectCloudPub.getNumSubscribers() == 0)
+            return;
+
+        if (!mTfListener.waitForTransform(mTransformTargetFrame, pCloudMsgPtr->header.frame_id.c_str(), pCloudMsgPtr->header.stamp, ros::Duration(1.0)) ||
+            !mTfListener.waitForTransform(mClusterTargetFrame, mTransformTargetFrame, pCloudMsgPtr->header.stamp, ros::Duration(1.0)))
+            return;
+
+        // transform the cloud to a desired frame
+        auto transformedCloudPtr = boost::make_shared<sensor_msgs::PointCloud2>();
+        if (!pcl_ros::transformPointCloud(mTransformTargetFrame, *pCloudMsgPtr, *transformedCloudPtr, mTfListener))
+        {
+            ROS_WARN("failed to transform cloud to frame '%s' from frame '%s'",
+                     mTransformTargetFrame.c_str(), pCloudMsgPtr->header.frame_id.c_str());
+            return;
+        }
+
+        // filter the cloud
+        PointCloud::Ptr pclCloudPtr = boost::make_shared<PointCloud>();
+        pcl::fromROSMsg(*transformedCloudPtr, *pclCloudPtr);
+        PointCloud::Ptr filteredCloudPtr = mCloudFilter.filterCloud(pclCloudPtr);
+
+        if (filteredCloudPtr->size() < mClusterParams.mMinClusterSize)
+        {
+            // Stop processing the point cloud if the filtered cloud does
+            // not have enough points to create even one cluster
+            return;
+        }
+
+        mCurrTime = pCloudMsgPtr->header.stamp.sec;
+
+        if (mFilteredCloudPub.getNumSubscribers() > 0)
+        {
+            // publish the filtered cloud for debugging
+            publishCloud(filteredCloudPtr, mFilteredCloudPub);
+        }
+
+        // Euclidean clustering
+        pcl::search::Search<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
+        tree->setInputCloud(filteredCloudPtr);
+        std::vector<pcl::PointIndices> cluster_indices;
+        pcl::extractEuclideanClusters(*filteredCloudPtr,
+                                      tree,
+                                      mClusterParams.mClusterTolerance,
+                                      cluster_indices,
+                                      mClusterParams.mMinClusterSize,
+                                      mClusterParams.mMaxClusterSize);
+
+        if (cluster_indices.size() <= 0)
+            return;
+
+        std::vector<PointCloud::Ptr> clusterClouds;
+        getClusterClouds(clusterClouds, filteredCloudPtr, cluster_indices);
+        filterClusterCloudsBySize(clusterClouds);
+        filterClusterCloudsNearOccupiedSpaces(clusterClouds);
+
+        processNewClusters(clusterClouds);
+        removeStaleObjects();
+
+        publishObjectCloud();
+        publishObjectBoundsMarkers();
+        publishObjectObjectMessages();
+
+        cleanup();
+    }
+
+    void
+    resetCallback(const std_msgs::Bool& reset)
+    {
+        if (reset.data)
+        {
+            ROS_WARN("Cache reset requested! Resetting the object cache!");
+            mUniqueObjectId = 0;
+            mLastSeenTimeCache.clear();
+            mObjectsCache.clear();
+            mPrevPositionsCache.clear();
+            mObjectMsgCache.clear();
+        }
+    }
+
+    void
+    filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds)
+    {
+        // Check if the size of the object is within the specified limits
+        for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+        {
+            Eigen::Vector4f min, max;
+            pcl::getMinMax3D(*(*it), min, max);
+            float sizeX = std::abs(max[0] - min[0]);
+            float sizeY = std::abs(max[1] - min[1]);
+            float sizeZ = std::abs(max[2] - min[2]);
+            if (sizeX > mObjectFilterParams.mMaxBboxEdgeLength ||
+                sizeY > mObjectFilterParams.mMaxBboxEdgeLength ||
+                sizeZ > mObjectFilterParams.mMaxBboxEdgeLength)
+            {
+                it = clusterClouds.erase(it);
+            }
+            else
+            {
+                ++it;
+            }
+        }
+    }
+
+    void
+    filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds)
+    {
+        mas_navigation_tools::OccupancyCheckerGoal goal;
+        for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+        {
+            Eigen::Vector4f centroid;
+            if (pcl::compute3DCentroid(*(*it), centroid) <= 0)
+            {
+                ROS_WARN("Could not compute centroid of point cloud. The cluster will be filtered out!");
+                it = clusterClouds.erase(it);
+            }
+            else
+            {
+                geometry_msgs::Point p;
+                p.x = centroid.x();
+                p.y = centroid.y();
+                p.z = centroid.z();
+                goal.points.push_back(p);
+                ++it;
+            }
+        }
+        goal.search_radius = mObjectFilterParams.mMinDistFromOccupiedCell;
+        mOccupancyCheckerClient.sendGoal(goal);
+        mOccupancyCheckerClient.waitForResult(ros::Duration(5.0));
+        if (mOccupancyCheckerClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+        {
+            mas_navigation_tools::OccupancyCheckerResultConstPtr result = mOccupancyCheckerClient.getResult();
+            int pos = 0;
+            for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+            {
+                if (!result->far_from_obstacle[pos++])
+                    it = clusterClouds.erase(it);
+                else
+                    ++it;
+            }
+        }
+        else
+        {
+            ROS_ERROR("Could not complete checking if the objects lie close to the occupied regions in the map. Discarding all the clusters!");
+            clusterClouds.clear();
+        }
+    }
+
+    void
+    getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
+                     PointCloud::ConstPtr filteredCloud,
+                     const std::vector<pcl::PointIndices>& cluster_indices)
+    {
+        for (const auto& idx: cluster_indices)
+        {
+            PointCloud::Ptr clusterCloud(new PointCloud);
+            for (const auto &index : idx.indices)
+                clusterCloud->push_back ((*filteredCloud)[index]);
+
+            clusterCloud->header = filteredCloud->header;
+            clusterCloud->width = clusterCloud->size();
+            clusterCloud->height = 1;
+            clusterCloud->is_dense = true;
+
+            PointCloud::Ptr transformedCloudPtr = boost::make_shared<PointCloud>();
+            if (!pcl_ros::transformPointCloud(mClusterTargetFrame, *clusterCloud, *transformedCloudPtr, mTfListener))
+            {
+                ROS_WARN("Failed to transform cluster cloud to frame '%s' from frame '%s'. This cluster will be skipped in the current frame!",
+                        mClusterTargetFrame.c_str(), clusterCloud->header.frame_id.c_str());
+                continue;
+            }
+
+            clusterClouds.push_back(transformedCloudPtr);
+        }
+    }
+
+    // Cloud comparison sample from https://stackoverflow.com/a/55930847
+    float
+    nearestDistance(const pcl::search::KdTree<PointT>& tree, const PointT& pt)
+    {
+        const int k = 1;
+        std::vector<int> indices (k);
+        std::vector<float> sqr_distances (k);
+
+        tree.nearestKSearch(pt, k, indices, sqr_distances);
+
+        return sqr_distances[0];
+    }
+    // compare cloudB to cloudA
+    // use threshold for identifying outliers and not considering those for the similarity
+    // a good value for threshold is 5 * <cloud_resolution>, e.g. 10cm for a cloud with 2cm resolution
+    float
+    getCloudSimilarity(const PointCloud& cloudA, const PointCloud& cloudB, float threshold)
+    {
+        // compare B to A
+        int num_outlier = 0;
+        pcl::search::KdTree<PointT> tree;
+        tree.setInputCloud(cloudA.makeShared());
+        auto sum = std::accumulate(cloudB.begin(), cloudB.end(), 0.0f, [&](float current_sum, const PointT& pt) {
+            const auto dist = nearestDistance(tree, pt);
+
+            if(dist < threshold)
+            {
+                return current_sum + dist;
+            }
+            else
+            {
+                num_outlier++;
+                return current_sum;
+            }
+        });
+
+        return sum / (cloudB.size() - num_outlier);
+    }
+
+    bool
+    isNewObject(const PointCloud& cloud, int& knownObjectId)
+    {
+        for (const auto& o : mObjectsCache)
+        {
+            if (getCloudSimilarity(cloud, *(o.second), mObjectCacheParams.mSimilarityThreshold) < mObjectCacheParams.mUniquenessThreshold)
+            {
+                knownObjectId = o.first;
+                return false;
+            }
+        }
+        knownObjectId = -1;
+        return true;
+    }
+
+    void
+    processNewClusters(const std::vector<PointCloud::Ptr>& clusterClouds)
+    {
+        for (const auto& cloud : clusterClouds)
+        {
+            Eigen::Vector4f centroid;
+            if (pcl::compute3DCentroid(*cloud, centroid) <= 0)
+            {
+                ROS_WARN("Could not compute centroid of point cloud. Skipping processing of potential object cluster!");
+                return;
+            }
+
+            int objectID = -1;
+            if (isNewObject(*cloud, objectID))
+            {
+                objectID = mUniqueObjectId++;
+                ROS_INFO("Adding new object: %d", objectID);
+                mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
+                std::vector<Eigen::Vector4f> prevPositions;
+                prevPositions.push_back(centroid);
+                mPrevPositionsCache.insert(std::pair<int, std::vector<Eigen::Vector4f>>(objectID, prevPositions));
+                mLastSeenTimeCache.insert(std::pair<int, int>(objectID, mCurrTime));
+            }
+            else
+            {
+                // update last known position
+                std::vector<Eigen::Vector4f>& prevPositions = mPrevPositionsCache[objectID];
+                if (prevPositions.size() > mObjectCacheParams.mPositionHistoryCacheSize)
+                {
+                    prevPositions.erase(prevPositions.begin());
+                }
+                prevPositions.push_back(centroid);
+
+                mLastSeenTimeCache[objectID] = mCurrTime;
+
+                // Update the cloud
+                mObjectsCache.erase(objectID);
+                mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
+            }
+        }
+    }
+
+    void
+    removeStaleObjects()
+    {
+        for (auto it = mLastSeenTimeCache.cbegin(); it != mLastSeenTimeCache.cend(); )
+        {
+            int id = it->first;
+            if (mCurrTime - it->second > mObjectCacheParams.mObjectCacheTime)
+            {
+                ROS_INFO("Removing Stale object: %d", id);
+                mLastSeenTimeCache.erase(it++);
+                mPrevPositionsCache.erase(id);
+                mObjectsCache.erase(id);
+            }
+            else
+            {
+                ++it;
+            }
+        }
+    }
+
+    geometry_msgs::Point
+    getGeomPoint(float x, float y, float z)
+    {
+        geometry_msgs::Point p;
+        p.x = x;
+        p.y = y;
+        p.z = z;
+        return p;
+    }
+
+    visualization_msgs::Marker
+    getObjectBoundsMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+    {
+        visualization_msgs::Marker marker;
+        marker.type = visualization_msgs::Marker::LINE_LIST;
+        marker.action = visualization_msgs::Marker::ADD;
+        marker.lifetime = ros::Duration(1.0);
+        marker.header.frame_id = mClusterTargetFrame;
+        marker.scale.x = 0.005;
+        marker.scale.y = 0.005;
+        marker.color.a = 2.0;
+        marker.ns = "";
+        marker.id = id;
+        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+        marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
+        marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
+        marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
+        marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
+        marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
+        marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
+        marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
+        marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
+        return marker;
+    }
+
+    visualization_msgs::Marker
+    getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg, int id)
+    {
+        visualization_msgs::Marker marker;
+        marker.type = visualization_msgs::Marker::LINE_LIST;
+        marker.action = visualization_msgs::Marker::ADD;
+        marker.lifetime = ros::Duration(1.0);
+        marker.header.frame_id = mClusterTargetFrame;
+        marker.scale.x = 0.005;
+        marker.scale.y = 0.005;
+        marker.ns = "";
+        marker.id = id;
+        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+
+        marker.points.push_back(objMsg.bounding_box.vertices[0]);
+        marker.points.push_back(objMsg.bounding_box.vertices[1]);
+        marker.points.push_back(objMsg.bounding_box.vertices[1]);
+        marker.points.push_back(objMsg.bounding_box.vertices[2]);
+        marker.points.push_back(objMsg.bounding_box.vertices[2]);
+        marker.points.push_back(objMsg.bounding_box.vertices[3]);
+        marker.points.push_back(objMsg.bounding_box.vertices[3]);
+        marker.points.push_back(objMsg.bounding_box.vertices[0]);
+
+        marker.points.push_back(objMsg.bounding_box.vertices[4]);
+        marker.points.push_back(objMsg.bounding_box.vertices[5]);
+        marker.points.push_back(objMsg.bounding_box.vertices[5]);
+        marker.points.push_back(objMsg.bounding_box.vertices[6]);
+        marker.points.push_back(objMsg.bounding_box.vertices[6]);
+        marker.points.push_back(objMsg.bounding_box.vertices[7]);
+        marker.points.push_back(objMsg.bounding_box.vertices[7]);
+        marker.points.push_back(objMsg.bounding_box.vertices[4]);
+
+        marker.points.push_back(objMsg.bounding_box.vertices[0]);
+        marker.points.push_back(objMsg.bounding_box.vertices[4]);
+        marker.points.push_back(objMsg.bounding_box.vertices[1]);
+        marker.points.push_back(objMsg.bounding_box.vertices[5]);
+        marker.points.push_back(objMsg.bounding_box.vertices[2]);
+        marker.points.push_back(objMsg.bounding_box.vertices[6]);
+        marker.points.push_back(objMsg.bounding_box.vertices[3]);
+        marker.points.push_back(objMsg.bounding_box.vertices[7]);
+        return marker;
+    }
+
+    visualization_msgs::Marker
+    getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg, int id)
+    {
+        visualization_msgs::Marker marker;
+        marker.type = visualization_msgs::Marker::ARROW;
+        marker.action = visualization_msgs::Marker::ADD;
+        marker.lifetime = ros::Duration(1.0);
+        marker.header.frame_id = mClusterTargetFrame;
+        marker.scale.x = .1;
+        marker.scale.y = .005;
+        marker.scale.z = .005;
+        marker.ns = "";
+        marker.id = id + 2000;
+        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+        marker.pose = objMsg.pose.pose;
+        return marker;
+    }
+
+    visualization_msgs::Marker
+    getObjectIdMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+    {
+        Eigen::Vector4f center = min + (max - min)/2.0;
+        visualization_msgs::Marker marker;
+        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+        marker.text = std::to_string(id);
+        marker.lifetime = ros::Duration(1.0);
+        marker.header.frame_id = mClusterTargetFrame;
+        marker.pose.position.x = center[0];
+        marker.pose.position.y = center[1];
+        marker.pose.position.z = center[2];
+        marker.scale.z = 0.05;
+        marker.id = id + 1000;
+        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+        return marker;
+    }
+
+    mas_perception_msgs::Object
+    createObjectMessage(PointCloud::ConstPtr cloudPtr, int id)
+    {
+        // Solution inspired from: https://stackoverflow.com/a/49705361
+        PointT minPoint;
+        PointT maxPoint;
+        PointT position;
+        Eigen::Matrix3f rotMatrix;
+        Eigen::Vector3f massCenter;
+        pcl::MomentOfInertiaEstimation<PointT> estimator;
+        estimator.setInputCloud(cloudPtr);
+        estimator.compute();
+        estimator.getOBB(minPoint, maxPoint, position, rotMatrix);
+        estimator.getMassCenter(massCenter);
+
+        Eigen::Vector3f offset(position.x, position.y, position.z);
+        std::vector<Eigen::Vector3f> points;
+        points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, minPoint.z));
+        points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, minPoint.z));
+        points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, minPoint.z));
+        points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, minPoint.z));
+        points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, maxPoint.z));
+        points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, maxPoint.z));
+        points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, maxPoint.z));
+        points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, maxPoint.z));
+
+        // Copy the cluster cloud and set the z-value to that of the mass center for all points
+        PointCloud::Ptr copy(new PointCloud);
+        pcl::copyPointCloud(*cloudPtr, *copy);
+        for(unsigned int i = 0; i < copy->points.size(); i++)
+        {
+            copy->points[i].z = massCenter.z();
+        }
+
+        mas_perception_msgs::Object msg;
+        for (unsigned int i = 0 ; i < points.size(); i++)
+        {
+            points[i] = rotMatrix * points[i] + offset;
+            geometry_msgs::Point p;
+            p.x = points[i].x();
+            p.y = points[i].y();
+            p.z = points[i].z();
+            msg.bounding_box.vertices.push_back(p);
+        }
+
+        msg.bounding_box.dimensions.x = std::abs(maxPoint.x - minPoint.x);
+        msg.bounding_box.dimensions.y = std::abs(maxPoint.y - minPoint.y);
+        msg.bounding_box.dimensions.z = std::abs(maxPoint.z - minPoint.z);
+
+        geometry_msgs::Point center;
+        center.x = massCenter.x();
+        center.y = massCenter.y();
+        center.z = massCenter.z();
+        msg.bounding_box.center = center;
+
+        // Compute the pose for the object
+        pcl::MomentOfInertiaEstimation<PointT> orientationEstimator;
+        Eigen::Vector3f major_vector, middle_vector, minor_vector;
+        orientationEstimator.setInputCloud(copy);
+        orientationEstimator.compute();
+        orientationEstimator.getEigenVectors (major_vector, middle_vector, minor_vector);
+
+        // Add pose to the Object message
+        double yaw = std::atan2(major_vector.y(), major_vector.x());
+        tf2::Quaternion orientation;
+        orientation.setRPY(0.0, 0.0, yaw);
+        msg.pose.pose.position = center;
+        msg.pose.pose.orientation = tf2::toMsg(orientation);
+        msg.pose.header.frame_id = mClusterTargetFrame;
+
+        // Add dimensions to the Object message
+        Eigen::Vector4f min, max;
+        pcl::getMinMax3D(*cloudPtr, min, max);
+        geometry_msgs::Vector3 dimensions;
+        dimensions.x = std::abs(max[0] - min[0]);
+        dimensions.y = std::abs(max[1] - min[1]);
+        dimensions.z = std::abs(max[2] - min[2]);
+        msg.dimensions.header.frame_id = mClusterTargetFrame;
+        msg.dimensions.vector = dimensions;
+
+        msg.name = std::string("object_") + std::to_string(id);
+
+        return msg;
+    }
+
+    void
+    updateObjectMsgCache()
+    {
+        if (mObjectMsgCache.empty())
+        {
+            for (const auto& c : mObjectsCache)
+            {
+                mObjectMsgCache.insert(std::pair<int, mas_perception_msgs::Object>(c.first, createObjectMessage(c.second, c.first)));
+            }
+        }
+    }
+
+    const mas_perception_msgs::Object*
+    getObjectMessage(int id)
+    {
+        updateObjectMsgCache();
+        std::map<int, mas_perception_msgs::Object>::iterator itr = mObjectMsgCache.find(id);
+        if (itr == mObjectMsgCache.end())
+        {
+            ROS_WARN("Unable to find Object message for object %d", id);
+            return nullptr;
+        }
+
+        return &(itr->second);
+    }
+
+    void
+    publishCloud(PointCloud::ConstPtr cloudPtr, const ros::Publisher& publisher)
+    {
+        // publish the cloud
+        sensor_msgs::PointCloud2::Ptr cloudMsgPtr = boost::make_shared<sensor_msgs::PointCloud2>();
+        pcl::toROSMsg(*cloudPtr, *cloudMsgPtr);
+        publisher.publish(*cloudMsgPtr);
+    }
+
+    void
+    publishObjectCloud()
+    {
+        if ((mObjectCloudPub.getNumSubscribers() <= 0) || (mObjectsCache.empty()))
+            return;
+
+        // Merge all the clusters into once cloud
+        PointCloud::Ptr mergedCloud(new PointCloud);
+        bool headerInitialized = false;
+        for (const auto& c : mObjectsCache)
+        {   if (!headerInitialized)
+            {
+                mergedCloud->header = c.second->header;
+                headerInitialized = true;
+            }
+            *mergedCloud += *(c.second);
+        }
+        mergedCloud->width = mergedCloud->size();
+        mergedCloud->height = 1;
+        mergedCloud->is_dense = true;
+
+        // publish the cloud
+        publishCloud(mergedCloud, mObjectCloudPub);
+    }
+
+    void
+    publishObjectBoundsMarkers()
+    {
+        if (mObjectBoundsPub.getNumSubscribers() <= 0)
+            return;
+
+        visualization_msgs::MarkerArray markerArray;
+        for (const auto& c : mObjectsCache)
+        {
+            Eigen::Vector4f min, max;
+            pcl::getMinMax3D(*(c.second), min, max);
+            if (mPublishOrientedBBox)
+            {
+                const mas_perception_msgs::Object* objMsg = getObjectMessage(c.first);
+                if (objMsg == nullptr)
+                    continue;
+                markerArray.markers.push_back(getObjectBoundsMarker(*objMsg, c.first));
+                markerArray.markers.push_back(getObjectOrientationMarker(*objMsg, c.first));
+            }
+            else
+            {
+                markerArray.markers.push_back(getObjectBoundsMarker(min, max, c.first));
+            }
+            markerArray.markers.push_back(getObjectIdMarker(min, max, c.first));
+        }
+        // Publish the marker array
+        mObjectBoundsPub.publish(markerArray);
+    }
+
+    void
+    publishObjectObjectMessages()
+    {
+        if (mObjectObjectsPub.getNumSubscribers() <= 0)
+            return;
+
+        updateObjectMsgCache();
+        mas_perception_msgs::ObjectList objectList;
+        for (const auto& msg : mObjectMsgCache)
+        {
+            objectList.objects.push_back(msg.second);
+        }
+        mObjectObjectsPub.publish(objectList);
+    }
+
+    void cleanup()
+    {
+        mObjectMsgCache.clear();
+    }
+};
+
+}   // namespace mdr_cloud_object_detection
+
+int main(int pArgc, char** pArgv)
+{
+    ros::init(pArgc, pArgv, "cloud_object_detection");
+    ros::NodeHandle nh("~");
+
+    // load launch parameters
+    std::string cloudTopic, filteredCloudTopic, objectCloudTopic,
+                objectBoundsTopic, objectObjectsTopic, transformTargetFrame,
+                clusterTargetFrame, occupancyCheckerActionName;
+    bool publishOrientedBBox = false;
+    if (!nh.getParam("cloud_topic", cloudTopic) || cloudTopic.empty())
+    {
+        ROS_ERROR("No 'cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("filtered_cloud_topic", filteredCloudTopic) || filteredCloudTopic.empty())
+    {
+        ROS_ERROR("No 'filtered_cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("object_cloud_topic", objectCloudTopic) || objectCloudTopic.empty())
+    {
+        ROS_ERROR("No 'object_cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("objects_bounds_topic", objectBoundsTopic) || objectBoundsTopic.empty())
+    {
+        ROS_ERROR("No 'objects_bounds_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("transform_target_frame", transformTargetFrame) || transformTargetFrame.empty())
+    {
+        ROS_ERROR("No 'transform_target_frame' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("cluster_target_frame", clusterTargetFrame) || clusterTargetFrame.empty())
+    {
+        ROS_ERROR("No 'cluster_target_frame' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("occupancy_checker_action_name", occupancyCheckerActionName) || occupancyCheckerActionName.empty())
+    {
+        ROS_ERROR("No 'occupancy_checker_action_name' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    if (!nh.getParam("objects_object_topic", objectObjectsTopic) || objectObjectsTopic.empty())
+    {
+        ROS_ERROR("No 'objects_object_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+    nh.getParam("publish_oriented_bbox", publishOrientedBBox);
+
+    // run cloud filtering and object detection
+    mdr_cloud_object_detection::CloudObjectDetectionNode objectDetection(nh, cloudTopic, filteredCloudTopic,
+                                                                      objectCloudTopic, transformTargetFrame,
+                                                                      clusterTargetFrame, occupancyCheckerActionName,
+                                                                      objectBoundsTopic, objectObjectsTopic,
+                                                                      publishOrientedBBox);
+
+    while (ros::ok())
+        ros::spin();
+
+    return 0;
+}

From 76592f5dd2c58a7a1999acb80a3208462bad7ec0 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 11:43:13 +0100
Subject: [PATCH 244/264] Move individual classes to separate header/cpp files

---
 .../mdr_cloud_object_detection/CMakeLists.txt |   4 +-
 .../{aliases.h => Aliases.h}                  |   0
 .../CloudObjectDetection.h                    | 139 +++
 .../{color.h => Color.h}                      |   2 +-
 .../EuclideanClusterParams.h                  |  19 +
 .../ObjectCacheParams.h                       |  18 +
 .../ObjectFilterParams.h                      |  16 +
 .../PassThroughVoxelFilter.h                  |  36 +
 .../PassThroughVoxelFilterParams.h            |  26 +
 .../mdr_cloud_object_detection/Utils.h        |   5 +
 .../cloud_object_detection.h                  |   1 -
 .../ros/src/CloudObjectDetection.cpp          | 662 +++++++++++++
 .../ros/src/PassThroughVoxelFilter.cpp        |  40 +
 .../ros/src/cloud_object_detection.cpp        | 907 ------------------
 .../ros/src/main.cpp                          |  79 ++
 15 files changed, 1044 insertions(+), 910 deletions(-)
 rename mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/{aliases.h => Aliases.h} (100%)
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
 rename mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/{color.h => Color.h} (98%)
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
index 34d1f0daa..301429268 100644
--- a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -43,7 +43,9 @@ include_directories(
 ###################################
 # Node for object detection
 add_executable(cloud_object_detection
-  ros/src/cloud_object_detection.cpp
+  ros/src/PassThroughVoxelFilter.cpp
+  ros/src/CloudObjectDetection.cpp
+  ros/src/main.cpp
 )
 target_link_libraries(cloud_object_detection
 #   ${PROJECT_NAME}
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h
similarity index 100%
rename from mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/aliases.h
rename to mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
new file mode 100644
index 000000000..d6873b467
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
@@ -0,0 +1,139 @@
+/*!
+ * @copyright 2018 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ * @brief Class to detect objects from a point cloud
+ */
+
+#include <cmath>
+#include <actionlib/client/simple_action_client.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/common/common.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <std_msgs/Bool.h>
+#include <tf/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <mas_navigation_tools/OccupancyCheckerAction.h>
+#include <mas_perception_msgs/ObjectList.h>
+#include "mdr_cloud_object_detection/Color.h"
+#include "mdr_cloud_object_detection/Utils.h"
+#include <mdr_cloud_object_detection/ObjectDetectionConfig.h>
+
+namespace mdr_cloud_object_detection
+{
+    class CloudObjectDetection
+    {
+    private:
+        ros::NodeHandle mNodeHandle;
+        dynamic_reconfigure::Server<ObjectDetectionConfig> mObjectDetectionConfigServer;
+        ros::Subscriber mCloudSub;
+        ros::Subscriber mResetSub;
+        ros::Publisher mFilteredCloudPub;
+        ros::Publisher mObjectCloudPub;
+        ros::Publisher mObjectBoundsPub;
+        ros::Publisher mObjectObjectsPub;
+        actionlib::SimpleActionClient<mas_navigation_tools::OccupancyCheckerAction> mOccupancyCheckerClient;
+        tf::TransformListener mTfListener;
+        std::string mTransformTargetFrame;
+        std::string mClusterTargetFrame;
+        PassThroughVoxelFilter mCloudFilter;
+        EuclideanClusterParams mClusterParams;
+        ObjectFilterParams mObjectFilterParams;
+        ObjectCacheParams mObjectCacheParams;
+        bool mPublishOrientedBBox;
+
+        unsigned int mUniqueObjectId;
+        unsigned int mCurrTime;
+        std::map<int, int> mLastSeenTimeCache;
+        std::map<int, const PointCloud::Ptr> mObjectsCache;
+        std::map<int, std::vector<Eigen::Vector4f>> mPrevPositionsCache;
+        std::map<int, mas_perception_msgs::Object> mObjectMsgCache;
+
+    public:
+        CloudObjectDetection(const ros::NodeHandle &pNodeHandle,
+                             const std::string &pCloudTopic,
+                             const std::string &pFilteredCloudTopic,
+                             const std::string &pObjectCloudTopic,
+                             const std::string &pTransformTargetFrame,
+                             const std::string &pClusterTargetFrame,
+                             const std::string &pOccupancyCheckerActionName,
+                             const std::string &pObjectsBoundsTopic,
+                             const std::string &pObjectObjectsTopic,
+                             bool pPublishOrientedBBox);
+
+    private:
+        void objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig,
+                                           uint32_t pLevel);
+
+        void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pCloudMsgPtr);
+
+        void resetCallback(const std_msgs::Bool& reset);
+
+        void filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds);
+
+        void filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds);
+
+        void getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
+                              PointCloud::ConstPtr filteredCloud,
+                              const std::vector<pcl::PointIndices>& cluster_indices);
+
+        float nearestDistance(const pcl::search::KdTree<PointT>& tree,
+                              const PointT& pt);
+        
+        float getCloudSimilarity(const PointCloud& cloudA,
+                                 const PointCloud& cloudB,
+                                 float threshold);
+
+        bool isNewObject(const PointCloud& cloud,
+                         int& knownObjectId);
+
+        void processNewClusters(const std::vector<PointCloud::Ptr>& clusterClouds);
+
+        void removeStaleObjects();
+
+        geometry_msgs::Point getGeomPoint(float x,
+                                          float y,
+                                          float z);
+
+        visualization_msgs::Marker getObjectBoundsMarker(const Eigen::Vector4f& min,
+                                                         const Eigen::Vector4f& max,
+                                                         int id);
+
+        visualization_msgs::Marker getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg,
+                                                         int id);
+
+        visualization_msgs::Marker getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg,
+                                                              int id);
+
+        visualization_msgs::Marker getObjectIdMarker(const Eigen::Vector4f& min,
+                                                     const Eigen::Vector4f& max,
+                                                     int id);
+
+        mas_perception_msgs::Object createObjectMessage(PointCloud::ConstPtr cloudPtr,
+                                                        int id);
+
+        void updateObjectMsgCache();
+
+        const mas_perception_msgs::Object* getObjectMessage(int id);
+
+        void publishCloud(PointCloud::ConstPtr cloudPtr,
+                          const ros::Publisher& publisher);
+
+        void publishObjectCloud();
+
+        void publishObjectBoundsMarkers();
+
+        void publishObjectObjectMessages();
+
+        void cleanup();
+    };
+
+}   // namespace mdr_cloud_object_detection
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
similarity index 98%
rename from mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h
rename to mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
index 5996c6b83..ec856fbde 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/color.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
@@ -9,7 +9,7 @@
 #define MDR_CLOUD_OBJECT_DETECTION_COLOR_H
 
 #include <std_msgs/ColorRGBA.h>
-#include <mdr_cloud_object_detection/aliases.h>
+#include <mdr_cloud_object_detection/Aliases.h>
 
 namespace mdr_cloud_object_detection
 {
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
new file mode 100644
index 000000000..fb02c71a7
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
@@ -0,0 +1,19 @@
+#ifndef MDR_CLOUD_OBJECT_DETECTION_EUCLIDEAN_CLUSTER_PARAMS
+#define MDR_CLOUD_OBJECT_DETECTION_EUCLIDEAN_CLUSTER_PARAMS
+
+#include <limits>
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief struct containing parameters necessary for clustering point clouds
+    */
+    struct EuclideanClusterParams
+    {
+        float mClusterTolerance = 0.01f;
+        unsigned int mMinClusterSize = 1;
+        unsigned int mMaxClusterSize = std::numeric_limits<unsigned int>::max();
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_EUCLIDEAN_CLUSTER_PARAMS
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
new file mode 100644
index 000000000..938aeb81a
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
@@ -0,0 +1,18 @@
+#ifndef MDR_CLOUD_OBJECT_DETECTION_OBJECT_CACHE_PARAMS
+#define MDR_CLOUD_OBJECT_DETECTION_OBJECT_CACHE_PARAMS
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief struct containing parameters necessary for caching the detected objects
+    */
+    struct ObjectCacheParams
+    {
+        float mObjectCacheTime = 5.0f;
+        float mSimilarityThreshold = 2.0f;
+        int mPositionHistoryCacheSize = 10;
+        float mUniquenessThreshold = 0.005f;
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_OBJECT_CACHE_PARAMS
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
new file mode 100644
index 000000000..b2c3cca59
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
@@ -0,0 +1,16 @@
+#ifndef MDR_CLOUD_OBJECT_DETECTION_OBJECT_FILTERING_PARAMS
+#define MDR_CLOUD_OBJECT_DETECTION_OBJECT_FILTERING_PARAMS
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief struct containing parameters necessary for filtering the detected objects
+    */
+    struct ObjectFilterParams
+    {
+        float mMaxBboxEdgeLength = 0.15f;
+        float mMinDistFromOccupiedCell = 0.1f;
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_OBJECT_FILTERING_PARAMS
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
new file mode 100644
index 000000000..b25a8c65d
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
@@ -0,0 +1,36 @@
+#ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
+#define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
+
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+
+#include "mdr_cloud_object_detection/Aliases.h"
+#include "mdr_cloud_object_detection/PassThroughVoxelFilterParams.h"
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief class containing definition for filtering point clouds
+    */
+    class PassThroughVoxelFilter
+    {
+    public:
+        PassThroughVoxelFilter() = default;
+
+        /*! @brief set parameters relevant to filtering cloud */
+        virtual void setParams(const PassThroughVoxelFilterParams& pParams);
+
+        /*!
+        * @brief filter point cloud using passthrough and voxel filters
+        */
+        PointCloud::Ptr filterCloud(const PointCloud::ConstPtr &pCloudPtr);
+
+    private:
+        pcl::PassThrough<PointT> mPassThroughFilterX;
+        pcl::PassThrough<PointT> mPassThroughFilterY;
+        pcl::PassThrough<PointT> mPassThroughFilterZ;
+        pcl::VoxelGrid<PointT> mVoxelGridFilter;
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
new file mode 100644
index 000000000..31a7a2ed9
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
@@ -0,0 +1,26 @@
+#ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
+#define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief struct containing parameters necessary for filtering point clouds
+    */
+    struct PassThroughVoxelFilterParams
+    {
+        /* PassThrough filter parameters
+        * limit the cloud to be filtered points outside of these x, y, z ranges will be discarded */
+        float mPassThroughLimitMinX = 0.0f;
+        float mPassThroughLimitMaxX = 0.0f;
+        float mPassThroughLimitMinY = 0.0f;
+        float mPassThroughLimitMaxY = 0.0f;
+        float mPassThroughLimitMinZ = 0.0f;
+        float mPassThroughLimitMaxZ = 0.0f;
+        /* VoxelGrid filter parameters for down-sampling the cloud, also limit the cloud along the z axis */
+        float mVoxelLimitMinZ = 0.0f;
+        float mVoxelLimitMaxZ = 0.0f;
+        float mVoxelLeafSize = 0.0f;
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
new file mode 100644
index 000000000..739b709bf
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
@@ -0,0 +1,5 @@
+#include "mdr_cloud_object_detection/EuclideanClusterParams.h"
+#include "mdr_cloud_object_detection/ObjectCacheParams.h"
+#include "mdr_cloud_object_detection/ObjectFilterParams.h"
+
+#include "mdr_cloud_object_detection/PassThroughVoxelFilter.h"
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
deleted file mode 100644
index 751cc4a71..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/cloud_object_detection.h
+++ /dev/null
@@ -1 +0,0 @@
-#include<iostream>
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
new file mode 100644
index 000000000..153e1c857
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
@@ -0,0 +1,662 @@
+/*!
+ * @copyright 2018 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Chavan
+ *
+ * @brief script to detect objects from a point cloud
+ */
+
+#include "mdr_cloud_object_detection/CloudObjectDetection.h"
+
+using namespace mdr_cloud_object_detection;
+
+CloudObjectDetection::CloudObjectDetection(const ros::NodeHandle &pNodeHandle, 
+                                           const std::string &pCloudTopic,
+                                           const std::string &pFilteredCloudTopic,
+                                           const std::string &pObjectCloudTopic,
+                                           const std::string &pTransformTargetFrame,
+                                           const std::string &pClusterTargetFrame,
+                                           const std::string &pOccupancyCheckerActionName,
+                                           const std::string &pObjectsBoundsTopic,
+                                           const std::string &pObjectObjectsTopic,
+                                           bool pPublishOrientedBBox)
+: mNodeHandle(pNodeHandle)
+, mObjectDetectionConfigServer(mNodeHandle)
+, mTransformTargetFrame(pTransformTargetFrame)
+, mClusterTargetFrame(pClusterTargetFrame)
+, mPublishOrientedBBox(pPublishOrientedBBox)
+, mUniqueObjectId(0)
+, mOccupancyCheckerClient(pOccupancyCheckerActionName, true)
+{
+    ROS_INFO("[CloudObjectDetection] Waiting for OccupancyChecker server");
+    mOccupancyCheckerClient.waitForServer();
+
+    ROS_INFO("setting up dynamic reconfiguration server for object detection");
+    auto odCallback = boost::bind(&CloudObjectDetection::objectDetectionConfigCallback, this, _1, _2);
+    mObjectDetectionConfigServer.setCallback(odCallback);
+
+    ROS_INFO("subscribing to point cloud topic and advertising processed result");
+    mCloudSub = mNodeHandle.subscribe(pCloudTopic, 1, &CloudObjectDetection::cloudCallback, this);
+    mResetSub = mNodeHandle.subscribe("reset_cache", 1, &CloudObjectDetection::resetCallback, this);
+    mFilteredCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pFilteredCloudTopic, 1);
+    mObjectCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pObjectCloudTopic, 1);
+    mObjectBoundsPub = mNodeHandle.advertise<visualization_msgs::MarkerArray>(pObjectsBoundsTopic, 1);
+    mObjectObjectsPub = mNodeHandle.advertise<mas_perception_msgs::ObjectList>(pObjectObjectsTopic, 1);
+}
+
+void CloudObjectDetection::objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig,
+                                                         uint32_t pLevel)
+{
+    // Cloud Filter params
+    PassThroughVoxelFilterParams cloudFilterParams;
+    cloudFilterParams.mPassThroughLimitMinX = static_cast<float>(pConfig.passthrough_limit_min_x);
+    cloudFilterParams.mPassThroughLimitMaxX = static_cast<float>(pConfig.passthrough_limit_max_x);
+    cloudFilterParams.mPassThroughLimitMinY = static_cast<float>(pConfig.passthrough_limit_min_y);
+    cloudFilterParams.mPassThroughLimitMaxY = static_cast<float>(pConfig.passthrough_limit_max_y);
+    cloudFilterParams.mPassThroughLimitMinZ = static_cast<float>(pConfig.passthrough_limit_min_z);
+    cloudFilterParams.mPassThroughLimitMaxZ = static_cast<float>(pConfig.passthrough_limit_max_z);
+    cloudFilterParams.mVoxelLimitMinZ = static_cast<float>(pConfig.voxel_limit_min_z);
+    cloudFilterParams.mVoxelLimitMaxZ = static_cast<float>(pConfig.voxel_limit_max_z);
+    cloudFilterParams.mVoxelLeafSize = static_cast<float>(pConfig.voxel_leaf_size);
+    mCloudFilter.setParams(cloudFilterParams);
+
+    // Cloud Clustering params
+    mClusterParams.mClusterTolerance = static_cast<float>(pConfig.cluster_tolerance);
+    mClusterParams.mMinClusterSize = static_cast<unsigned int>(pConfig.min_cluster_size);
+    mClusterParams.mMaxClusterSize = static_cast<unsigned int>(pConfig.max_cluster_size);
+
+    // Object filter params
+    mObjectFilterParams.mMaxBboxEdgeLength = static_cast<float>(pConfig.max_bbox_edge_length);
+    mObjectFilterParams.mMinDistFromOccupiedCell = static_cast<float>(pConfig.min_dist_from_occupied_cell);
+
+    // Object cache params
+    mObjectCacheParams.mObjectCacheTime = static_cast<unsigned int>(pConfig.object_cache_time);
+    mObjectCacheParams.mSimilarityThreshold = static_cast<float>(pConfig.similarity_threshold);
+    mObjectCacheParams.mUniquenessThreshold = static_cast<float>(pConfig.uniqueness_threshold);
+    mObjectCacheParams.mPositionHistoryCacheSize = static_cast<float>(pConfig.position_history_cache_size);
+}
+
+void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pCloudMsgPtr)
+{
+    // do not process cloud when there's no subscriber
+    if (mFilteredCloudPub.getNumSubscribers() == 0 &&
+        mObjectBoundsPub.getNumSubscribers() == 0 &&
+        mObjectObjectsPub.getNumSubscribers() == 0 &&
+        mObjectCloudPub.getNumSubscribers() == 0)
+        return;
+
+    if (!mTfListener.waitForTransform(mTransformTargetFrame, pCloudMsgPtr->header.frame_id.c_str(), pCloudMsgPtr->header.stamp, ros::Duration(1.0)) ||
+        !mTfListener.waitForTransform(mClusterTargetFrame, mTransformTargetFrame, pCloudMsgPtr->header.stamp, ros::Duration(1.0)))
+        return;
+
+    // transform the cloud to a desired frame
+    auto transformedCloudPtr = boost::make_shared<sensor_msgs::PointCloud2>();
+    if (!pcl_ros::transformPointCloud(mTransformTargetFrame, *pCloudMsgPtr, *transformedCloudPtr, mTfListener))
+    {
+        ROS_WARN("failed to transform cloud to frame '%s' from frame '%s'",
+                 mTransformTargetFrame.c_str(), pCloudMsgPtr->header.frame_id.c_str());
+        return;
+    }
+
+    // filter the cloud
+    PointCloud::Ptr pclCloudPtr = boost::make_shared<PointCloud>();
+    pcl::fromROSMsg(*transformedCloudPtr, *pclCloudPtr);
+    PointCloud::Ptr filteredCloudPtr = mCloudFilter.filterCloud(pclCloudPtr);
+
+    if (filteredCloudPtr->size() < mClusterParams.mMinClusterSize)
+    {
+        // Stop processing the point cloud if the filtered cloud does
+        // not have enough points to create even one cluster
+        return;
+    }
+
+    mCurrTime = pCloudMsgPtr->header.stamp.sec;
+
+    if (mFilteredCloudPub.getNumSubscribers() > 0)
+    {
+        // publish the filtered cloud for debugging
+        publishCloud(filteredCloudPtr, mFilteredCloudPub);
+    }
+
+    // Euclidean clustering
+    pcl::search::Search<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
+    tree->setInputCloud(filteredCloudPtr);
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::extractEuclideanClusters(*filteredCloudPtr,
+                                  tree,
+                                  mClusterParams.mClusterTolerance,
+                                  cluster_indices,
+                                  mClusterParams.mMinClusterSize,
+                                  mClusterParams.mMaxClusterSize);
+
+    if (cluster_indices.size() <= 0)
+        return;
+
+    std::vector<PointCloud::Ptr> clusterClouds;
+    getClusterClouds(clusterClouds, filteredCloudPtr, cluster_indices);
+    filterClusterCloudsBySize(clusterClouds);
+    filterClusterCloudsNearOccupiedSpaces(clusterClouds);
+
+    processNewClusters(clusterClouds);
+    removeStaleObjects();
+
+    publishObjectCloud();
+    publishObjectBoundsMarkers();
+    publishObjectObjectMessages();
+
+    cleanup();
+}
+
+void CloudObjectDetection::resetCallback(const std_msgs::Bool& reset)
+{
+    if (reset.data)
+    {
+        ROS_WARN("Cache reset requested! Resetting the object cache!");
+        mUniqueObjectId = 0;
+        mLastSeenTimeCache.clear();
+        mObjectsCache.clear();
+        mPrevPositionsCache.clear();
+        mObjectMsgCache.clear();
+    }
+}
+
+void CloudObjectDetection::filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds)
+{
+    // Check if the size of the object is within the specified limits
+    for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+    {
+        Eigen::Vector4f min, max;
+        pcl::getMinMax3D(*(*it), min, max);
+        float sizeX = std::abs(max[0] - min[0]);
+        float sizeY = std::abs(max[1] - min[1]);
+        float sizeZ = std::abs(max[2] - min[2]);
+        if (sizeX > mObjectFilterParams.mMaxBboxEdgeLength ||
+            sizeY > mObjectFilterParams.mMaxBboxEdgeLength ||
+            sizeZ > mObjectFilterParams.mMaxBboxEdgeLength)
+        {
+            it = clusterClouds.erase(it);
+        }
+        else
+        {
+            ++it;
+        }
+    }
+}
+
+void CloudObjectDetection::filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds)
+{
+    mas_navigation_tools::OccupancyCheckerGoal goal;
+    for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+    {
+        Eigen::Vector4f centroid;
+        if (pcl::compute3DCentroid(*(*it), centroid) <= 0)
+        {
+            ROS_WARN("Could not compute centroid of point cloud. The cluster will be filtered out!");
+            it = clusterClouds.erase(it);
+        }
+        else
+        {
+            geometry_msgs::Point p;
+            p.x = centroid.x();
+            p.y = centroid.y();
+            p.z = centroid.z();
+            goal.points.push_back(p);
+            ++it;
+        }
+    }
+    goal.search_radius = mObjectFilterParams.mMinDistFromOccupiedCell;
+    mOccupancyCheckerClient.sendGoal(goal);
+    mOccupancyCheckerClient.waitForResult(ros::Duration(5.0));
+    if (mOccupancyCheckerClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+    {
+        mas_navigation_tools::OccupancyCheckerResultConstPtr result = mOccupancyCheckerClient.getResult();
+        int pos = 0;
+        for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
+        {
+            if (!result->far_from_obstacle[pos++])
+                it = clusterClouds.erase(it);
+            else
+                ++it;
+        }
+    }
+    else
+    {
+        ROS_ERROR("Could not complete checking if the objects lie close to the occupied regions in the map. Discarding all the clusters!");
+        clusterClouds.clear();
+    }
+}
+
+void CloudObjectDetection::getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
+                     PointCloud::ConstPtr filteredCloud,
+                     const std::vector<pcl::PointIndices>& cluster_indices)
+{
+    for (const auto& idx: cluster_indices)
+    {
+        PointCloud::Ptr clusterCloud(new PointCloud);
+        for (const auto &index : idx.indices)
+            clusterCloud->push_back ((*filteredCloud)[index]);
+
+        clusterCloud->header = filteredCloud->header;
+        clusterCloud->width = clusterCloud->size();
+        clusterCloud->height = 1;
+        clusterCloud->is_dense = true;
+
+        PointCloud::Ptr transformedCloudPtr = boost::make_shared<PointCloud>();
+        if (!pcl_ros::transformPointCloud(mClusterTargetFrame, *clusterCloud, *transformedCloudPtr, mTfListener))
+        {
+            ROS_WARN("Failed to transform cluster cloud to frame '%s' from frame '%s'. This cluster will be skipped in the current frame!",
+                    mClusterTargetFrame.c_str(), clusterCloud->header.frame_id.c_str());
+            continue;
+        }
+
+        clusterClouds.push_back(transformedCloudPtr);
+    }
+}
+
+// Cloud comparison sample from https://stackoverflow.com/a/55930847
+float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& tree, const PointT& pt)
+{
+    const int k = 1;
+    std::vector<int> indices (k);
+    std::vector<float> sqr_distances (k);
+
+    tree.nearestKSearch(pt, k, indices, sqr_distances);
+
+    return sqr_distances[0];
+}
+
+// compare cloudB to cloudA
+// use threshold for identifying outliers and not considering those for the similarity
+// a good value for threshold is 5 * <cloud_resolution>, e.g. 10cm for a cloud with 2cm resolution
+float CloudObjectDetection::getCloudSimilarity(const PointCloud& cloudA, const PointCloud& cloudB, float threshold)
+{
+    // compare B to A
+    int num_outlier = 0;
+    pcl::search::KdTree<PointT> tree;
+    tree.setInputCloud(cloudA.makeShared());
+    auto sum = std::accumulate(cloudB.begin(), cloudB.end(), 0.0f, [&](float current_sum, const PointT& pt) {
+        const auto dist = nearestDistance(tree, pt);
+
+        if(dist < threshold)
+        {
+            return current_sum + dist;
+        }
+        else
+        {
+            num_outlier++;
+            return current_sum;
+        }
+    });
+
+    return sum / (cloudB.size() - num_outlier);
+}
+
+bool CloudObjectDetection::isNewObject(const PointCloud& cloud, int& knownObjectId)
+{
+    for (const auto& o : mObjectsCache)
+    {
+        if (getCloudSimilarity(cloud, *(o.second), mObjectCacheParams.mSimilarityThreshold) < mObjectCacheParams.mUniquenessThreshold)
+        {
+            knownObjectId = o.first;
+            return false;
+        }
+    }
+    knownObjectId = -1;
+    return true;
+}
+
+void CloudObjectDetection::processNewClusters(const std::vector<PointCloud::Ptr>& clusterClouds)
+{
+    for (const auto& cloud : clusterClouds)
+    {
+        Eigen::Vector4f centroid;
+        if (pcl::compute3DCentroid(*cloud, centroid) <= 0)
+        {
+            ROS_WARN("Could not compute centroid of point cloud. Skipping processing of potential object cluster!");
+            return;
+        }
+
+        int objectID = -1;
+        if (isNewObject(*cloud, objectID))
+        {
+            objectID = mUniqueObjectId++;
+            ROS_INFO("Adding new object: %d", objectID);
+            mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
+            std::vector<Eigen::Vector4f> prevPositions;
+            prevPositions.push_back(centroid);
+            mPrevPositionsCache.insert(std::pair<int, std::vector<Eigen::Vector4f>>(objectID, prevPositions));
+            mLastSeenTimeCache.insert(std::pair<int, int>(objectID, mCurrTime));
+        }
+        else
+        {
+            // update last known position
+            std::vector<Eigen::Vector4f>& prevPositions = mPrevPositionsCache[objectID];
+            if (prevPositions.size() > mObjectCacheParams.mPositionHistoryCacheSize)
+            {
+                prevPositions.erase(prevPositions.begin());
+            }
+            prevPositions.push_back(centroid);
+
+            mLastSeenTimeCache[objectID] = mCurrTime;
+
+            // Update the cloud
+            mObjectsCache.erase(objectID);
+            mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
+        }
+    }
+}
+
+void CloudObjectDetection::removeStaleObjects()
+{
+    for (auto it = mLastSeenTimeCache.cbegin(); it != mLastSeenTimeCache.cend(); )
+    {
+        int id = it->first;
+        if (mCurrTime - it->second > mObjectCacheParams.mObjectCacheTime)
+        {
+            ROS_INFO("Removing Stale object: %d", id);
+            mLastSeenTimeCache.erase(it++);
+            mPrevPositionsCache.erase(id);
+            mObjectsCache.erase(id);
+        }
+        else
+        {
+            ++it;
+        }
+    }
+}
+
+geometry_msgs::Point CloudObjectDetection::getGeomPoint(float x, float y, float z)
+{
+    geometry_msgs::Point p;
+    p.x = x;
+    p.y = y;
+    p.z = z;
+    return p;
+}
+
+visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+{
+    visualization_msgs::Marker marker;
+    marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.lifetime = ros::Duration(1.0);
+    marker.header.frame_id = mClusterTargetFrame;
+    marker.scale.x = 0.005;
+    marker.scale.y = 0.005;
+    marker.color.a = 2.0;
+    marker.ns = "";
+    marker.id = id;
+    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
+    marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
+    marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
+    marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
+    marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
+    marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
+    marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
+    marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
+    return marker;
+}
+
+visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg, int id)
+{
+    visualization_msgs::Marker marker;
+    marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.lifetime = ros::Duration(1.0);
+    marker.header.frame_id = mClusterTargetFrame;
+    marker.scale.x = 0.005;
+    marker.scale.y = 0.005;
+    marker.ns = "";
+    marker.id = id;
+    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+
+    marker.points.push_back(objMsg.bounding_box.vertices[0]);
+    marker.points.push_back(objMsg.bounding_box.vertices[1]);
+    marker.points.push_back(objMsg.bounding_box.vertices[1]);
+    marker.points.push_back(objMsg.bounding_box.vertices[2]);
+    marker.points.push_back(objMsg.bounding_box.vertices[2]);
+    marker.points.push_back(objMsg.bounding_box.vertices[3]);
+    marker.points.push_back(objMsg.bounding_box.vertices[3]);
+    marker.points.push_back(objMsg.bounding_box.vertices[0]);
+
+    marker.points.push_back(objMsg.bounding_box.vertices[4]);
+    marker.points.push_back(objMsg.bounding_box.vertices[5]);
+    marker.points.push_back(objMsg.bounding_box.vertices[5]);
+    marker.points.push_back(objMsg.bounding_box.vertices[6]);
+    marker.points.push_back(objMsg.bounding_box.vertices[6]);
+    marker.points.push_back(objMsg.bounding_box.vertices[7]);
+    marker.points.push_back(objMsg.bounding_box.vertices[7]);
+    marker.points.push_back(objMsg.bounding_box.vertices[4]);
+
+    marker.points.push_back(objMsg.bounding_box.vertices[0]);
+    marker.points.push_back(objMsg.bounding_box.vertices[4]);
+    marker.points.push_back(objMsg.bounding_box.vertices[1]);
+    marker.points.push_back(objMsg.bounding_box.vertices[5]);
+    marker.points.push_back(objMsg.bounding_box.vertices[2]);
+    marker.points.push_back(objMsg.bounding_box.vertices[6]);
+    marker.points.push_back(objMsg.bounding_box.vertices[3]);
+    marker.points.push_back(objMsg.bounding_box.vertices[7]);
+    return marker;
+}
+
+visualization_msgs::Marker CloudObjectDetection::getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg, int id)
+{
+    visualization_msgs::Marker marker;
+    marker.type = visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.lifetime = ros::Duration(1.0);
+    marker.header.frame_id = mClusterTargetFrame;
+    marker.scale.x = .1;
+    marker.scale.y = .005;
+    marker.scale.z = .005;
+    marker.ns = "";
+    marker.id = id + 2000;
+    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.pose = objMsg.pose.pose;
+    return marker;
+}
+
+visualization_msgs::Marker CloudObjectDetection::getObjectIdMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+{
+    Eigen::Vector4f center = min + (max - min)/2.0;
+    visualization_msgs::Marker marker;
+    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    marker.text = std::to_string(id);
+    marker.lifetime = ros::Duration(1.0);
+    marker.header.frame_id = mClusterTargetFrame;
+    marker.pose.position.x = center[0];
+    marker.pose.position.y = center[1];
+    marker.pose.position.z = center[2];
+    marker.scale.z = 0.05;
+    marker.id = id + 1000;
+    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    return marker;
+}
+
+mas_perception_msgs::Object CloudObjectDetection::createObjectMessage(PointCloud::ConstPtr cloudPtr, int id)
+{
+    // Solution inspired from: https://stackoverflow.com/a/49705361
+    PointT minPoint;
+    PointT maxPoint;
+    PointT position;
+    Eigen::Matrix3f rotMatrix;
+    Eigen::Vector3f massCenter;
+    pcl::MomentOfInertiaEstimation<PointT> estimator;
+    estimator.setInputCloud(cloudPtr);
+    estimator.compute();
+    estimator.getOBB(minPoint, maxPoint, position, rotMatrix);
+    estimator.getMassCenter(massCenter);
+
+    Eigen::Vector3f offset(position.x, position.y, position.z);
+    std::vector<Eigen::Vector3f> points;
+    points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, minPoint.z));
+    points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, minPoint.z));
+    points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, minPoint.z));
+    points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, minPoint.z));
+    points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, maxPoint.z));
+    points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, maxPoint.z));
+    points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, maxPoint.z));
+    points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, maxPoint.z));
+
+    // Copy the cluster cloud and set the z-value to that of the mass center for all points
+    PointCloud::Ptr copy(new PointCloud);
+    pcl::copyPointCloud(*cloudPtr, *copy);
+    for(unsigned int i = 0; i < copy->points.size(); i++)
+    {
+        copy->points[i].z = massCenter.z();
+    }
+
+    mas_perception_msgs::Object msg;
+    for (unsigned int i = 0 ; i < points.size(); i++)
+    {
+        points[i] = rotMatrix * points[i] + offset;
+        geometry_msgs::Point p;
+        p.x = points[i].x();
+        p.y = points[i].y();
+        p.z = points[i].z();
+        msg.bounding_box.vertices.push_back(p);
+    }
+
+    msg.bounding_box.dimensions.x = std::abs(maxPoint.x - minPoint.x);
+    msg.bounding_box.dimensions.y = std::abs(maxPoint.y - minPoint.y);
+    msg.bounding_box.dimensions.z = std::abs(maxPoint.z - minPoint.z);
+
+    geometry_msgs::Point center;
+    center.x = massCenter.x();
+    center.y = massCenter.y();
+    center.z = massCenter.z();
+    msg.bounding_box.center = center;
+
+    // Compute the pose for the object
+    pcl::MomentOfInertiaEstimation<PointT> orientationEstimator;
+    Eigen::Vector3f major_vector, middle_vector, minor_vector;
+    orientationEstimator.setInputCloud(copy);
+    orientationEstimator.compute();
+    orientationEstimator.getEigenVectors (major_vector, middle_vector, minor_vector);
+
+    // Add pose to the Object message
+    double yaw = std::atan2(major_vector.y(), major_vector.x());
+    tf2::Quaternion orientation;
+    orientation.setRPY(0.0, 0.0, yaw);
+    msg.pose.pose.position = center;
+    msg.pose.pose.orientation = tf2::toMsg(orientation);
+    msg.pose.header.frame_id = mClusterTargetFrame;
+
+    // Add dimensions to the Object message
+    Eigen::Vector4f min, max;
+    pcl::getMinMax3D(*cloudPtr, min, max);
+    geometry_msgs::Vector3 dimensions;
+    dimensions.x = std::abs(max[0] - min[0]);
+    dimensions.y = std::abs(max[1] - min[1]);
+    dimensions.z = std::abs(max[2] - min[2]);
+    msg.dimensions.header.frame_id = mClusterTargetFrame;
+    msg.dimensions.vector = dimensions;
+
+    msg.name = std::string("object_") + std::to_string(id);
+
+    return msg;
+}
+
+void CloudObjectDetection::updateObjectMsgCache()
+{
+    if (mObjectMsgCache.empty())
+    {
+        for (const auto& c : mObjectsCache)
+        {
+            mObjectMsgCache.insert(std::pair<int, mas_perception_msgs::Object>(c.first, createObjectMessage(c.second, c.first)));
+        }
+    }
+}
+
+const mas_perception_msgs::Object* CloudObjectDetection::getObjectMessage(int id)
+{
+    updateObjectMsgCache();
+    std::map<int, mas_perception_msgs::Object>::iterator itr = mObjectMsgCache.find(id);
+    if (itr == mObjectMsgCache.end())
+    {
+        ROS_WARN("Unable to find Object message for object %d", id);
+        return nullptr;
+    }
+
+    return &(itr->second);
+}
+
+void CloudObjectDetection::publishCloud(PointCloud::ConstPtr cloudPtr, const ros::Publisher& publisher)
+{
+    // publish the cloud
+    sensor_msgs::PointCloud2::Ptr cloudMsgPtr = boost::make_shared<sensor_msgs::PointCloud2>();
+    pcl::toROSMsg(*cloudPtr, *cloudMsgPtr);
+    publisher.publish(*cloudMsgPtr);
+}
+
+void CloudObjectDetection::publishObjectCloud()
+{
+    if ((mObjectCloudPub.getNumSubscribers() <= 0) || (mObjectsCache.empty()))
+        return;
+
+    // Merge all the clusters into once cloud
+    PointCloud::Ptr mergedCloud(new PointCloud);
+    bool headerInitialized = false;
+    for (const auto& c : mObjectsCache)
+    {   if (!headerInitialized)
+        {
+            mergedCloud->header = c.second->header;
+            headerInitialized = true;
+        }
+        *mergedCloud += *(c.second);
+    }
+    mergedCloud->width = mergedCloud->size();
+    mergedCloud->height = 1;
+    mergedCloud->is_dense = true;
+
+    // publish the cloud
+    publishCloud(mergedCloud, mObjectCloudPub);
+}
+
+void CloudObjectDetection::publishObjectBoundsMarkers()
+{
+    if (mObjectBoundsPub.getNumSubscribers() <= 0)
+        return;
+
+    visualization_msgs::MarkerArray markerArray;
+    for (const auto& c : mObjectsCache)
+    {
+        Eigen::Vector4f min, max;
+        pcl::getMinMax3D(*(c.second), min, max);
+        if (mPublishOrientedBBox)
+        {
+            const mas_perception_msgs::Object* objMsg = getObjectMessage(c.first);
+            if (objMsg == nullptr)
+                continue;
+            markerArray.markers.push_back(getObjectBoundsMarker(*objMsg, c.first));
+            markerArray.markers.push_back(getObjectOrientationMarker(*objMsg, c.first));
+        }
+        else
+        {
+            markerArray.markers.push_back(getObjectBoundsMarker(min, max, c.first));
+        }
+        markerArray.markers.push_back(getObjectIdMarker(min, max, c.first));
+    }
+    // Publish the marker array
+    mObjectBoundsPub.publish(markerArray);
+}
+
+void CloudObjectDetection::publishObjectObjectMessages()
+{
+    if (mObjectObjectsPub.getNumSubscribers() <= 0)
+        return;
+
+    updateObjectMsgCache();
+    mas_perception_msgs::ObjectList objectList;
+    for (const auto& msg : mObjectMsgCache)
+    {
+        objectList.objects.push_back(msg.second);
+    }
+    mObjectObjectsPub.publish(objectList);
+}
+
+void CloudObjectDetection::cleanup()
+    {
+        mObjectMsgCache.clear();
+    }
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
new file mode 100644
index 000000000..d55bf8700
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
@@ -0,0 +1,40 @@
+#include "mdr_cloud_object_detection/PassThroughVoxelFilter.h"
+
+using namespace mdr_cloud_object_detection;
+
+void PassThroughVoxelFilter::setParams(const PassThroughVoxelFilterParams& pParams)
+{
+    /* pass-through params */
+    mPassThroughFilterX.setFilterFieldName("x");
+    mPassThroughFilterX.setFilterLimits(pParams.mPassThroughLimitMinX, pParams.mPassThroughLimitMaxX);
+    mPassThroughFilterY.setFilterFieldName("y");
+    mPassThroughFilterY.setFilterLimits(pParams.mPassThroughLimitMinY, pParams.mPassThroughLimitMaxY);
+    mPassThroughFilterZ.setFilterFieldName("z");
+    mPassThroughFilterZ.setFilterLimits(pParams.mPassThroughLimitMinZ, pParams.mPassThroughLimitMaxZ);
+
+    /* filter z-axis using voxel filter instead of making another member */
+    mVoxelGridFilter.setFilterFieldName("z");
+    mVoxelGridFilter.setFilterLimits(pParams.mVoxelLimitMinZ, pParams.mVoxelLimitMaxZ);
+
+    /* voxel-grid params */
+    mVoxelGridFilter.setLeafSize(pParams.mVoxelLeafSize, pParams.mVoxelLeafSize, pParams.mVoxelLeafSize);
+}
+
+PointCloud::Ptr PassThroughVoxelFilter::filterCloud(const PointCloud::ConstPtr &pCloudPtr)
+{
+    PointCloud::Ptr filteredCloudPtr = boost::make_shared<PointCloud>();
+
+    mPassThroughFilterX.setInputCloud(pCloudPtr);
+    mPassThroughFilterX.filter(*filteredCloudPtr);
+
+    mPassThroughFilterY.setInputCloud(filteredCloudPtr);
+    mPassThroughFilterY.filter(*filteredCloudPtr);
+
+    mPassThroughFilterZ.setInputCloud(filteredCloudPtr);
+    mPassThroughFilterZ.filter(*filteredCloudPtr);
+
+    mVoxelGridFilter.setInputCloud(filteredCloudPtr);
+    mVoxelGridFilter.filter(*filteredCloudPtr);
+
+    return filteredCloudPtr;
+}
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp
deleted file mode 100644
index 04ec5afc1..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/cloud_object_detection.cpp
+++ /dev/null
@@ -1,907 +0,0 @@
-/*!
- * @copyright 2018 Bonn-Rhein-Sieg University
- *
- * @author Sushant Chavan
- *
- * @brief script to detect objects from a point cloud
- */
-
-#include <cmath>
-#include <actionlib/client/simple_action_client.h>
-#include <eigen_conversions/eigen_msg.h>
-#include <pcl_ros/transforms.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/common/common.h>
-#include <pcl/features/moment_of_inertia_estimation.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/voxel_grid.h>
-#include <dynamic_reconfigure/server.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <std_msgs/Bool.h>
-#include <tf/transform_listener.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <mas_navigation_tools/OccupancyCheckerAction.h>
-#include <mas_perception_msgs/ObjectList.h>
-#include <mdr_cloud_object_detection/color.h>
-#include <mdr_cloud_object_detection/ObjectDetectionConfig.h>
-
-// using namespace mdr_cloud_object_detection;
-
-namespace mdr_cloud_object_detection
-{
-
-/*!
- * @brief struct containing parameters necessary for filtering point clouds
- */
-struct CloudPassThroughVoxelFilterParams
-{
-    /* PassThrough filter parameters
-     * limit the cloud to be filtered points outside of these x, y, z ranges will be discarded */
-    float mPassThroughLimitMinX = 0.0f;
-    float mPassThroughLimitMaxX = 0.0f;
-    float mPassThroughLimitMinY = 0.0f;
-    float mPassThroughLimitMaxY = 0.0f;
-    float mPassThroughLimitMinZ = 0.0f;
-    float mPassThroughLimitMaxZ = 0.0f;
-    /* VoxelGrid filter parameters for down-sampling the cloud, also limit the cloud along the z axis */
-    float mVoxelLimitMinZ = 0.0f;
-    float mVoxelLimitMaxZ = 0.0f;
-    float mVoxelLeafSize = 0.0f;
-};
-
-/*!
- * @brief struct containing parameters necessary for clustering point clouds
- */
-struct EuclideanClusterParams
-{
-    float mClusterTolerance = 0.01f;
-    unsigned int mMinClusterSize = 1;
-    unsigned int mMaxClusterSize = std::numeric_limits<unsigned int>::max();
-};
-
-/*!
- * @brief struct containing parameters necessary for filtering the detected objects
- */
-struct ObjectFilterParams
-{
-    float mMaxBboxEdgeLength = 0.15f;
-    float mMinDistFromOccupiedCell = 0.1f;
-};
-
-/*!
- * @brief struct containing parameters necessary for caching the detected objects
- */
-struct ObjectCacheParams
-{
-    float mObjectCacheTime = 5.0f;
-    float mSimilarityThreshold = 2.0f;
-    int mPositionHistoryCacheSize = 10;
-    float mUniquenessThreshold = 0.005f;
-};
-
-/*!
- * @brief class containing definition for filtering point clouds
- */
-class CloudPassThroughVoxelFilter
-{
-public:
-    CloudPassThroughVoxelFilter() = default;
-
-    /*! @brief set parameters relevant to filtering cloud */
-    virtual void
-    setParams(const CloudPassThroughVoxelFilterParams& pParams)
-    {
-        /* pass-through params */
-        mPassThroughFilterX.setFilterFieldName("x");
-        mPassThroughFilterX.setFilterLimits(pParams.mPassThroughLimitMinX, pParams.mPassThroughLimitMaxX);
-        mPassThroughFilterY.setFilterFieldName("y");
-        mPassThroughFilterY.setFilterLimits(pParams.mPassThroughLimitMinY, pParams.mPassThroughLimitMaxY);
-        mPassThroughFilterZ.setFilterFieldName("z");
-        mPassThroughFilterZ.setFilterLimits(pParams.mPassThroughLimitMinZ, pParams.mPassThroughLimitMaxZ);
-
-        /* filter z-axis using voxel filter instead of making another member */
-        mVoxelGridFilter.setFilterFieldName("z");
-        mVoxelGridFilter.setFilterLimits(pParams.mVoxelLimitMinZ, pParams.mVoxelLimitMaxZ);
-
-        /* voxel-grid params */
-        mVoxelGridFilter.setLeafSize(pParams.mVoxelLeafSize, pParams.mVoxelLeafSize, pParams.mVoxelLeafSize);
-    }
-
-    /*!
-    * @brief filter point cloud using passthrough and voxel filters
-    */
-    PointCloud::Ptr
-    filterCloud(const PointCloud::ConstPtr &pCloudPtr)
-    {
-        PointCloud::Ptr filteredCloudPtr = boost::make_shared<PointCloud>();
-
-        mPassThroughFilterX.setInputCloud(pCloudPtr);
-        mPassThroughFilterX.filter(*filteredCloudPtr);
-
-        mPassThroughFilterY.setInputCloud(filteredCloudPtr);
-        mPassThroughFilterY.filter(*filteredCloudPtr);
-
-        mPassThroughFilterZ.setInputCloud(filteredCloudPtr);
-        mPassThroughFilterZ.filter(*filteredCloudPtr);
-
-        mVoxelGridFilter.setInputCloud(filteredCloudPtr);
-        mVoxelGridFilter.filter(*filteredCloudPtr);
-
-        return filteredCloudPtr;
-    }
-
-private:
-    pcl::PassThrough<PointT> mPassThroughFilterX;
-    pcl::PassThrough<PointT> mPassThroughFilterY;
-    pcl::PassThrough<PointT> mPassThroughFilterZ;
-    pcl::VoxelGrid<PointT> mVoxelGridFilter;
-};
-
-class CloudObjectDetectionNode
-{
-private:
-    ros::NodeHandle mNodeHandle;
-    dynamic_reconfigure::Server<ObjectDetectionConfig> mObjectDetectionConfigServer;
-    ros::Subscriber mCloudSub;
-    ros::Subscriber mResetSub;
-    ros::Publisher mFilteredCloudPub;
-    ros::Publisher mObjectCloudPub;
-    ros::Publisher mObjectBoundsPub;
-    ros::Publisher mObjectObjectsPub;
-    actionlib::SimpleActionClient<mas_navigation_tools::OccupancyCheckerAction> mOccupancyCheckerClient;
-    tf::TransformListener mTfListener;
-    std::string mTransformTargetFrame;
-    std::string mClusterTargetFrame;
-    CloudPassThroughVoxelFilter mCloudFilter;
-    EuclideanClusterParams mClusterParams;
-    ObjectFilterParams mObjectFilterParams;
-    ObjectCacheParams mObjectCacheParams;
-    bool mPublishOrientedBBox;
-
-    unsigned int mUniqueObjectId;
-    unsigned int mCurrTime;
-    std::map<int, int> mLastSeenTimeCache;
-    std::map<int, const PointCloud::Ptr> mObjectsCache;
-    std::map<int, std::vector<Eigen::Vector4f>> mPrevPositionsCache;
-    std::map<int, mas_perception_msgs::Object> mObjectMsgCache;
-
-public:
-    CloudObjectDetectionNode(const ros::NodeHandle &pNodeHandle, const std::string &pCloudTopic,
-            const std::string &pFilteredCloudTopic,
-            const std::string &pObjectCloudTopic,
-            const std::string &pTransformTargetFrame,
-            const std::string &pClusterTargetFrame,
-            const std::string &pOccupancyCheckerActionName,
-            const std::string &pObjectsBoundsTopic,
-            const std::string &pObjectObjectsTopic,
-            bool pPublishOrientedBBox)
-    : mNodeHandle(pNodeHandle), mObjectDetectionConfigServer(mNodeHandle),
-      mTransformTargetFrame(pTransformTargetFrame), mClusterTargetFrame(pClusterTargetFrame),
-      mPublishOrientedBBox(pPublishOrientedBBox), mUniqueObjectId(0),
-      mOccupancyCheckerClient(pOccupancyCheckerActionName, true)
-    {
-        ROS_INFO("[ObjectDetectionNode] Waiting for OccupancyChecker server");
-        mOccupancyCheckerClient.waitForServer();
-
-        ROS_INFO("setting up dynamic reconfiguration server for object detection");
-        auto odCallback = boost::bind(&CloudObjectDetectionNode::objectDetectionConfigCallback, this, _1, _2);
-        mObjectDetectionConfigServer.setCallback(odCallback);
-
-        ROS_INFO("subscribing to point cloud topic and advertising processed result");
-        mCloudSub = mNodeHandle.subscribe(pCloudTopic, 1, &CloudObjectDetectionNode::cloudCallback, this);
-        mResetSub = mNodeHandle.subscribe("reset_cache", 1, &CloudObjectDetectionNode::resetCallback, this);
-        mFilteredCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pFilteredCloudTopic, 1);
-        mObjectCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pObjectCloudTopic, 1);
-        mObjectBoundsPub = mNodeHandle.advertise<visualization_msgs::MarkerArray>(pObjectsBoundsTopic, 1);
-        mObjectObjectsPub = mNodeHandle.advertise<mas_perception_msgs::ObjectList>(pObjectObjectsTopic, 1);
-    }
-
-private:
-    void
-    objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig, uint32_t pLevel)
-    {
-        // Cloud Filter params
-        CloudPassThroughVoxelFilterParams cloudFilterParams;
-        cloudFilterParams.mPassThroughLimitMinX = static_cast<float>(pConfig.passthrough_limit_min_x);
-        cloudFilterParams.mPassThroughLimitMaxX = static_cast<float>(pConfig.passthrough_limit_max_x);
-        cloudFilterParams.mPassThroughLimitMinY = static_cast<float>(pConfig.passthrough_limit_min_y);
-        cloudFilterParams.mPassThroughLimitMaxY = static_cast<float>(pConfig.passthrough_limit_max_y);
-        cloudFilterParams.mPassThroughLimitMinZ = static_cast<float>(pConfig.passthrough_limit_min_z);
-        cloudFilterParams.mPassThroughLimitMaxZ = static_cast<float>(pConfig.passthrough_limit_max_z);
-        cloudFilterParams.mVoxelLimitMinZ = static_cast<float>(pConfig.voxel_limit_min_z);
-        cloudFilterParams.mVoxelLimitMaxZ = static_cast<float>(pConfig.voxel_limit_max_z);
-        cloudFilterParams.mVoxelLeafSize = static_cast<float>(pConfig.voxel_leaf_size);
-        mCloudFilter.setParams(cloudFilterParams);
-
-        // Cloud Clustering params
-        mClusterParams.mClusterTolerance = static_cast<float>(pConfig.cluster_tolerance);
-        mClusterParams.mMinClusterSize = static_cast<unsigned int>(pConfig.min_cluster_size);
-        mClusterParams.mMaxClusterSize = static_cast<unsigned int>(pConfig.max_cluster_size);
-
-        // Object filter params
-        mObjectFilterParams.mMaxBboxEdgeLength = static_cast<float>(pConfig.max_bbox_edge_length);
-        mObjectFilterParams.mMinDistFromOccupiedCell = static_cast<float>(pConfig.min_dist_from_occupied_cell);
-
-        // Object cache params
-        mObjectCacheParams.mObjectCacheTime = static_cast<unsigned int>(pConfig.object_cache_time);
-        mObjectCacheParams.mSimilarityThreshold = static_cast<float>(pConfig.similarity_threshold);
-        mObjectCacheParams.mUniquenessThreshold = static_cast<float>(pConfig.uniqueness_threshold);
-        mObjectCacheParams.mPositionHistoryCacheSize = static_cast<float>(pConfig.position_history_cache_size);
-    }
-
-    void
-    cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pCloudMsgPtr)
-    {
-        // do not process cloud when there's no subscriber
-        if (mFilteredCloudPub.getNumSubscribers() == 0 &&
-            mObjectBoundsPub.getNumSubscribers() == 0 &&
-            mObjectObjectsPub.getNumSubscribers() == 0 &&
-            mObjectCloudPub.getNumSubscribers() == 0)
-            return;
-
-        if (!mTfListener.waitForTransform(mTransformTargetFrame, pCloudMsgPtr->header.frame_id.c_str(), pCloudMsgPtr->header.stamp, ros::Duration(1.0)) ||
-            !mTfListener.waitForTransform(mClusterTargetFrame, mTransformTargetFrame, pCloudMsgPtr->header.stamp, ros::Duration(1.0)))
-            return;
-
-        // transform the cloud to a desired frame
-        auto transformedCloudPtr = boost::make_shared<sensor_msgs::PointCloud2>();
-        if (!pcl_ros::transformPointCloud(mTransformTargetFrame, *pCloudMsgPtr, *transformedCloudPtr, mTfListener))
-        {
-            ROS_WARN("failed to transform cloud to frame '%s' from frame '%s'",
-                     mTransformTargetFrame.c_str(), pCloudMsgPtr->header.frame_id.c_str());
-            return;
-        }
-
-        // filter the cloud
-        PointCloud::Ptr pclCloudPtr = boost::make_shared<PointCloud>();
-        pcl::fromROSMsg(*transformedCloudPtr, *pclCloudPtr);
-        PointCloud::Ptr filteredCloudPtr = mCloudFilter.filterCloud(pclCloudPtr);
-
-        if (filteredCloudPtr->size() < mClusterParams.mMinClusterSize)
-        {
-            // Stop processing the point cloud if the filtered cloud does
-            // not have enough points to create even one cluster
-            return;
-        }
-
-        mCurrTime = pCloudMsgPtr->header.stamp.sec;
-
-        if (mFilteredCloudPub.getNumSubscribers() > 0)
-        {
-            // publish the filtered cloud for debugging
-            publishCloud(filteredCloudPtr, mFilteredCloudPub);
-        }
-
-        // Euclidean clustering
-        pcl::search::Search<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
-        tree->setInputCloud(filteredCloudPtr);
-        std::vector<pcl::PointIndices> cluster_indices;
-        pcl::extractEuclideanClusters(*filteredCloudPtr,
-                                      tree,
-                                      mClusterParams.mClusterTolerance,
-                                      cluster_indices,
-                                      mClusterParams.mMinClusterSize,
-                                      mClusterParams.mMaxClusterSize);
-
-        if (cluster_indices.size() <= 0)
-            return;
-
-        std::vector<PointCloud::Ptr> clusterClouds;
-        getClusterClouds(clusterClouds, filteredCloudPtr, cluster_indices);
-        filterClusterCloudsBySize(clusterClouds);
-        filterClusterCloudsNearOccupiedSpaces(clusterClouds);
-
-        processNewClusters(clusterClouds);
-        removeStaleObjects();
-
-        publishObjectCloud();
-        publishObjectBoundsMarkers();
-        publishObjectObjectMessages();
-
-        cleanup();
-    }
-
-    void
-    resetCallback(const std_msgs::Bool& reset)
-    {
-        if (reset.data)
-        {
-            ROS_WARN("Cache reset requested! Resetting the object cache!");
-            mUniqueObjectId = 0;
-            mLastSeenTimeCache.clear();
-            mObjectsCache.clear();
-            mPrevPositionsCache.clear();
-            mObjectMsgCache.clear();
-        }
-    }
-
-    void
-    filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds)
-    {
-        // Check if the size of the object is within the specified limits
-        for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
-        {
-            Eigen::Vector4f min, max;
-            pcl::getMinMax3D(*(*it), min, max);
-            float sizeX = std::abs(max[0] - min[0]);
-            float sizeY = std::abs(max[1] - min[1]);
-            float sizeZ = std::abs(max[2] - min[2]);
-            if (sizeX > mObjectFilterParams.mMaxBboxEdgeLength ||
-                sizeY > mObjectFilterParams.mMaxBboxEdgeLength ||
-                sizeZ > mObjectFilterParams.mMaxBboxEdgeLength)
-            {
-                it = clusterClouds.erase(it);
-            }
-            else
-            {
-                ++it;
-            }
-        }
-    }
-
-    void
-    filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds)
-    {
-        mas_navigation_tools::OccupancyCheckerGoal goal;
-        for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
-        {
-            Eigen::Vector4f centroid;
-            if (pcl::compute3DCentroid(*(*it), centroid) <= 0)
-            {
-                ROS_WARN("Could not compute centroid of point cloud. The cluster will be filtered out!");
-                it = clusterClouds.erase(it);
-            }
-            else
-            {
-                geometry_msgs::Point p;
-                p.x = centroid.x();
-                p.y = centroid.y();
-                p.z = centroid.z();
-                goal.points.push_back(p);
-                ++it;
-            }
-        }
-        goal.search_radius = mObjectFilterParams.mMinDistFromOccupiedCell;
-        mOccupancyCheckerClient.sendGoal(goal);
-        mOccupancyCheckerClient.waitForResult(ros::Duration(5.0));
-        if (mOccupancyCheckerClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
-        {
-            mas_navigation_tools::OccupancyCheckerResultConstPtr result = mOccupancyCheckerClient.getResult();
-            int pos = 0;
-            for (std::vector<PointCloud::Ptr>::iterator it = clusterClouds.begin(); it != clusterClouds.end(); )
-            {
-                if (!result->far_from_obstacle[pos++])
-                    it = clusterClouds.erase(it);
-                else
-                    ++it;
-            }
-        }
-        else
-        {
-            ROS_ERROR("Could not complete checking if the objects lie close to the occupied regions in the map. Discarding all the clusters!");
-            clusterClouds.clear();
-        }
-    }
-
-    void
-    getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
-                     PointCloud::ConstPtr filteredCloud,
-                     const std::vector<pcl::PointIndices>& cluster_indices)
-    {
-        for (const auto& idx: cluster_indices)
-        {
-            PointCloud::Ptr clusterCloud(new PointCloud);
-            for (const auto &index : idx.indices)
-                clusterCloud->push_back ((*filteredCloud)[index]);
-
-            clusterCloud->header = filteredCloud->header;
-            clusterCloud->width = clusterCloud->size();
-            clusterCloud->height = 1;
-            clusterCloud->is_dense = true;
-
-            PointCloud::Ptr transformedCloudPtr = boost::make_shared<PointCloud>();
-            if (!pcl_ros::transformPointCloud(mClusterTargetFrame, *clusterCloud, *transformedCloudPtr, mTfListener))
-            {
-                ROS_WARN("Failed to transform cluster cloud to frame '%s' from frame '%s'. This cluster will be skipped in the current frame!",
-                        mClusterTargetFrame.c_str(), clusterCloud->header.frame_id.c_str());
-                continue;
-            }
-
-            clusterClouds.push_back(transformedCloudPtr);
-        }
-    }
-
-    // Cloud comparison sample from https://stackoverflow.com/a/55930847
-    float
-    nearestDistance(const pcl::search::KdTree<PointT>& tree, const PointT& pt)
-    {
-        const int k = 1;
-        std::vector<int> indices (k);
-        std::vector<float> sqr_distances (k);
-
-        tree.nearestKSearch(pt, k, indices, sqr_distances);
-
-        return sqr_distances[0];
-    }
-    // compare cloudB to cloudA
-    // use threshold for identifying outliers and not considering those for the similarity
-    // a good value for threshold is 5 * <cloud_resolution>, e.g. 10cm for a cloud with 2cm resolution
-    float
-    getCloudSimilarity(const PointCloud& cloudA, const PointCloud& cloudB, float threshold)
-    {
-        // compare B to A
-        int num_outlier = 0;
-        pcl::search::KdTree<PointT> tree;
-        tree.setInputCloud(cloudA.makeShared());
-        auto sum = std::accumulate(cloudB.begin(), cloudB.end(), 0.0f, [&](float current_sum, const PointT& pt) {
-            const auto dist = nearestDistance(tree, pt);
-
-            if(dist < threshold)
-            {
-                return current_sum + dist;
-            }
-            else
-            {
-                num_outlier++;
-                return current_sum;
-            }
-        });
-
-        return sum / (cloudB.size() - num_outlier);
-    }
-
-    bool
-    isNewObject(const PointCloud& cloud, int& knownObjectId)
-    {
-        for (const auto& o : mObjectsCache)
-        {
-            if (getCloudSimilarity(cloud, *(o.second), mObjectCacheParams.mSimilarityThreshold) < mObjectCacheParams.mUniquenessThreshold)
-            {
-                knownObjectId = o.first;
-                return false;
-            }
-        }
-        knownObjectId = -1;
-        return true;
-    }
-
-    void
-    processNewClusters(const std::vector<PointCloud::Ptr>& clusterClouds)
-    {
-        for (const auto& cloud : clusterClouds)
-        {
-            Eigen::Vector4f centroid;
-            if (pcl::compute3DCentroid(*cloud, centroid) <= 0)
-            {
-                ROS_WARN("Could not compute centroid of point cloud. Skipping processing of potential object cluster!");
-                return;
-            }
-
-            int objectID = -1;
-            if (isNewObject(*cloud, objectID))
-            {
-                objectID = mUniqueObjectId++;
-                ROS_INFO("Adding new object: %d", objectID);
-                mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
-                std::vector<Eigen::Vector4f> prevPositions;
-                prevPositions.push_back(centroid);
-                mPrevPositionsCache.insert(std::pair<int, std::vector<Eigen::Vector4f>>(objectID, prevPositions));
-                mLastSeenTimeCache.insert(std::pair<int, int>(objectID, mCurrTime));
-            }
-            else
-            {
-                // update last known position
-                std::vector<Eigen::Vector4f>& prevPositions = mPrevPositionsCache[objectID];
-                if (prevPositions.size() > mObjectCacheParams.mPositionHistoryCacheSize)
-                {
-                    prevPositions.erase(prevPositions.begin());
-                }
-                prevPositions.push_back(centroid);
-
-                mLastSeenTimeCache[objectID] = mCurrTime;
-
-                // Update the cloud
-                mObjectsCache.erase(objectID);
-                mObjectsCache.insert(std::pair<int, const PointCloud::Ptr>(objectID, cloud));
-            }
-        }
-    }
-
-    void
-    removeStaleObjects()
-    {
-        for (auto it = mLastSeenTimeCache.cbegin(); it != mLastSeenTimeCache.cend(); )
-        {
-            int id = it->first;
-            if (mCurrTime - it->second > mObjectCacheParams.mObjectCacheTime)
-            {
-                ROS_INFO("Removing Stale object: %d", id);
-                mLastSeenTimeCache.erase(it++);
-                mPrevPositionsCache.erase(id);
-                mObjectsCache.erase(id);
-            }
-            else
-            {
-                ++it;
-            }
-        }
-    }
-
-    geometry_msgs::Point
-    getGeomPoint(float x, float y, float z)
-    {
-        geometry_msgs::Point p;
-        p.x = x;
-        p.y = y;
-        p.z = z;
-        return p;
-    }
-
-    visualization_msgs::Marker
-    getObjectBoundsMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
-    {
-        visualization_msgs::Marker marker;
-        marker.type = visualization_msgs::Marker::LINE_LIST;
-        marker.action = visualization_msgs::Marker::ADD;
-        marker.lifetime = ros::Duration(1.0);
-        marker.header.frame_id = mClusterTargetFrame;
-        marker.scale.x = 0.005;
-        marker.scale.y = 0.005;
-        marker.color.a = 2.0;
-        marker.ns = "";
-        marker.id = id;
-        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
-        marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
-        marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
-        marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
-        marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
-        marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
-        marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
-        marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
-        marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
-        return marker;
-    }
-
-    visualization_msgs::Marker
-    getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg, int id)
-    {
-        visualization_msgs::Marker marker;
-        marker.type = visualization_msgs::Marker::LINE_LIST;
-        marker.action = visualization_msgs::Marker::ADD;
-        marker.lifetime = ros::Duration(1.0);
-        marker.header.frame_id = mClusterTargetFrame;
-        marker.scale.x = 0.005;
-        marker.scale.y = 0.005;
-        marker.ns = "";
-        marker.id = id;
-        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
-
-        marker.points.push_back(objMsg.bounding_box.vertices[0]);
-        marker.points.push_back(objMsg.bounding_box.vertices[1]);
-        marker.points.push_back(objMsg.bounding_box.vertices[1]);
-        marker.points.push_back(objMsg.bounding_box.vertices[2]);
-        marker.points.push_back(objMsg.bounding_box.vertices[2]);
-        marker.points.push_back(objMsg.bounding_box.vertices[3]);
-        marker.points.push_back(objMsg.bounding_box.vertices[3]);
-        marker.points.push_back(objMsg.bounding_box.vertices[0]);
-
-        marker.points.push_back(objMsg.bounding_box.vertices[4]);
-        marker.points.push_back(objMsg.bounding_box.vertices[5]);
-        marker.points.push_back(objMsg.bounding_box.vertices[5]);
-        marker.points.push_back(objMsg.bounding_box.vertices[6]);
-        marker.points.push_back(objMsg.bounding_box.vertices[6]);
-        marker.points.push_back(objMsg.bounding_box.vertices[7]);
-        marker.points.push_back(objMsg.bounding_box.vertices[7]);
-        marker.points.push_back(objMsg.bounding_box.vertices[4]);
-
-        marker.points.push_back(objMsg.bounding_box.vertices[0]);
-        marker.points.push_back(objMsg.bounding_box.vertices[4]);
-        marker.points.push_back(objMsg.bounding_box.vertices[1]);
-        marker.points.push_back(objMsg.bounding_box.vertices[5]);
-        marker.points.push_back(objMsg.bounding_box.vertices[2]);
-        marker.points.push_back(objMsg.bounding_box.vertices[6]);
-        marker.points.push_back(objMsg.bounding_box.vertices[3]);
-        marker.points.push_back(objMsg.bounding_box.vertices[7]);
-        return marker;
-    }
-
-    visualization_msgs::Marker
-    getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg, int id)
-    {
-        visualization_msgs::Marker marker;
-        marker.type = visualization_msgs::Marker::ARROW;
-        marker.action = visualization_msgs::Marker::ADD;
-        marker.lifetime = ros::Duration(1.0);
-        marker.header.frame_id = mClusterTargetFrame;
-        marker.scale.x = .1;
-        marker.scale.y = .005;
-        marker.scale.z = .005;
-        marker.ns = "";
-        marker.id = id + 2000;
-        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
-        marker.pose = objMsg.pose.pose;
-        return marker;
-    }
-
-    visualization_msgs::Marker
-    getObjectIdMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
-    {
-        Eigen::Vector4f center = min + (max - min)/2.0;
-        visualization_msgs::Marker marker;
-        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-        marker.text = std::to_string(id);
-        marker.lifetime = ros::Duration(1.0);
-        marker.header.frame_id = mClusterTargetFrame;
-        marker.pose.position.x = center[0];
-        marker.pose.position.y = center[1];
-        marker.pose.position.z = center[2];
-        marker.scale.z = 0.05;
-        marker.id = id + 1000;
-        marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
-        return marker;
-    }
-
-    mas_perception_msgs::Object
-    createObjectMessage(PointCloud::ConstPtr cloudPtr, int id)
-    {
-        // Solution inspired from: https://stackoverflow.com/a/49705361
-        PointT minPoint;
-        PointT maxPoint;
-        PointT position;
-        Eigen::Matrix3f rotMatrix;
-        Eigen::Vector3f massCenter;
-        pcl::MomentOfInertiaEstimation<PointT> estimator;
-        estimator.setInputCloud(cloudPtr);
-        estimator.compute();
-        estimator.getOBB(minPoint, maxPoint, position, rotMatrix);
-        estimator.getMassCenter(massCenter);
-
-        Eigen::Vector3f offset(position.x, position.y, position.z);
-        std::vector<Eigen::Vector3f> points;
-        points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, minPoint.z));
-        points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, minPoint.z));
-        points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, minPoint.z));
-        points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, minPoint.z));
-        points.push_back(Eigen::Vector3f(minPoint.x, minPoint.y, maxPoint.z));
-        points.push_back(Eigen::Vector3f(minPoint.x, maxPoint.y, maxPoint.z));
-        points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, maxPoint.z));
-        points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, maxPoint.z));
-
-        // Copy the cluster cloud and set the z-value to that of the mass center for all points
-        PointCloud::Ptr copy(new PointCloud);
-        pcl::copyPointCloud(*cloudPtr, *copy);
-        for(unsigned int i = 0; i < copy->points.size(); i++)
-        {
-            copy->points[i].z = massCenter.z();
-        }
-
-        mas_perception_msgs::Object msg;
-        for (unsigned int i = 0 ; i < points.size(); i++)
-        {
-            points[i] = rotMatrix * points[i] + offset;
-            geometry_msgs::Point p;
-            p.x = points[i].x();
-            p.y = points[i].y();
-            p.z = points[i].z();
-            msg.bounding_box.vertices.push_back(p);
-        }
-
-        msg.bounding_box.dimensions.x = std::abs(maxPoint.x - minPoint.x);
-        msg.bounding_box.dimensions.y = std::abs(maxPoint.y - minPoint.y);
-        msg.bounding_box.dimensions.z = std::abs(maxPoint.z - minPoint.z);
-
-        geometry_msgs::Point center;
-        center.x = massCenter.x();
-        center.y = massCenter.y();
-        center.z = massCenter.z();
-        msg.bounding_box.center = center;
-
-        // Compute the pose for the object
-        pcl::MomentOfInertiaEstimation<PointT> orientationEstimator;
-        Eigen::Vector3f major_vector, middle_vector, minor_vector;
-        orientationEstimator.setInputCloud(copy);
-        orientationEstimator.compute();
-        orientationEstimator.getEigenVectors (major_vector, middle_vector, minor_vector);
-
-        // Add pose to the Object message
-        double yaw = std::atan2(major_vector.y(), major_vector.x());
-        tf2::Quaternion orientation;
-        orientation.setRPY(0.0, 0.0, yaw);
-        msg.pose.pose.position = center;
-        msg.pose.pose.orientation = tf2::toMsg(orientation);
-        msg.pose.header.frame_id = mClusterTargetFrame;
-
-        // Add dimensions to the Object message
-        Eigen::Vector4f min, max;
-        pcl::getMinMax3D(*cloudPtr, min, max);
-        geometry_msgs::Vector3 dimensions;
-        dimensions.x = std::abs(max[0] - min[0]);
-        dimensions.y = std::abs(max[1] - min[1]);
-        dimensions.z = std::abs(max[2] - min[2]);
-        msg.dimensions.header.frame_id = mClusterTargetFrame;
-        msg.dimensions.vector = dimensions;
-
-        msg.name = std::string("object_") + std::to_string(id);
-
-        return msg;
-    }
-
-    void
-    updateObjectMsgCache()
-    {
-        if (mObjectMsgCache.empty())
-        {
-            for (const auto& c : mObjectsCache)
-            {
-                mObjectMsgCache.insert(std::pair<int, mas_perception_msgs::Object>(c.first, createObjectMessage(c.second, c.first)));
-            }
-        }
-    }
-
-    const mas_perception_msgs::Object*
-    getObjectMessage(int id)
-    {
-        updateObjectMsgCache();
-        std::map<int, mas_perception_msgs::Object>::iterator itr = mObjectMsgCache.find(id);
-        if (itr == mObjectMsgCache.end())
-        {
-            ROS_WARN("Unable to find Object message for object %d", id);
-            return nullptr;
-        }
-
-        return &(itr->second);
-    }
-
-    void
-    publishCloud(PointCloud::ConstPtr cloudPtr, const ros::Publisher& publisher)
-    {
-        // publish the cloud
-        sensor_msgs::PointCloud2::Ptr cloudMsgPtr = boost::make_shared<sensor_msgs::PointCloud2>();
-        pcl::toROSMsg(*cloudPtr, *cloudMsgPtr);
-        publisher.publish(*cloudMsgPtr);
-    }
-
-    void
-    publishObjectCloud()
-    {
-        if ((mObjectCloudPub.getNumSubscribers() <= 0) || (mObjectsCache.empty()))
-            return;
-
-        // Merge all the clusters into once cloud
-        PointCloud::Ptr mergedCloud(new PointCloud);
-        bool headerInitialized = false;
-        for (const auto& c : mObjectsCache)
-        {   if (!headerInitialized)
-            {
-                mergedCloud->header = c.second->header;
-                headerInitialized = true;
-            }
-            *mergedCloud += *(c.second);
-        }
-        mergedCloud->width = mergedCloud->size();
-        mergedCloud->height = 1;
-        mergedCloud->is_dense = true;
-
-        // publish the cloud
-        publishCloud(mergedCloud, mObjectCloudPub);
-    }
-
-    void
-    publishObjectBoundsMarkers()
-    {
-        if (mObjectBoundsPub.getNumSubscribers() <= 0)
-            return;
-
-        visualization_msgs::MarkerArray markerArray;
-        for (const auto& c : mObjectsCache)
-        {
-            Eigen::Vector4f min, max;
-            pcl::getMinMax3D(*(c.second), min, max);
-            if (mPublishOrientedBBox)
-            {
-                const mas_perception_msgs::Object* objMsg = getObjectMessage(c.first);
-                if (objMsg == nullptr)
-                    continue;
-                markerArray.markers.push_back(getObjectBoundsMarker(*objMsg, c.first));
-                markerArray.markers.push_back(getObjectOrientationMarker(*objMsg, c.first));
-            }
-            else
-            {
-                markerArray.markers.push_back(getObjectBoundsMarker(min, max, c.first));
-            }
-            markerArray.markers.push_back(getObjectIdMarker(min, max, c.first));
-        }
-        // Publish the marker array
-        mObjectBoundsPub.publish(markerArray);
-    }
-
-    void
-    publishObjectObjectMessages()
-    {
-        if (mObjectObjectsPub.getNumSubscribers() <= 0)
-            return;
-
-        updateObjectMsgCache();
-        mas_perception_msgs::ObjectList objectList;
-        for (const auto& msg : mObjectMsgCache)
-        {
-            objectList.objects.push_back(msg.second);
-        }
-        mObjectObjectsPub.publish(objectList);
-    }
-
-    void cleanup()
-    {
-        mObjectMsgCache.clear();
-    }
-};
-
-}   // namespace mdr_cloud_object_detection
-
-int main(int pArgc, char** pArgv)
-{
-    ros::init(pArgc, pArgv, "cloud_object_detection");
-    ros::NodeHandle nh("~");
-
-    // load launch parameters
-    std::string cloudTopic, filteredCloudTopic, objectCloudTopic,
-                objectBoundsTopic, objectObjectsTopic, transformTargetFrame,
-                clusterTargetFrame, occupancyCheckerActionName;
-    bool publishOrientedBBox = false;
-    if (!nh.getParam("cloud_topic", cloudTopic) || cloudTopic.empty())
-    {
-        ROS_ERROR("No 'cloud_topic' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("filtered_cloud_topic", filteredCloudTopic) || filteredCloudTopic.empty())
-    {
-        ROS_ERROR("No 'filtered_cloud_topic' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("object_cloud_topic", objectCloudTopic) || objectCloudTopic.empty())
-    {
-        ROS_ERROR("No 'object_cloud_topic' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("objects_bounds_topic", objectBoundsTopic) || objectBoundsTopic.empty())
-    {
-        ROS_ERROR("No 'objects_bounds_topic' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("transform_target_frame", transformTargetFrame) || transformTargetFrame.empty())
-    {
-        ROS_ERROR("No 'transform_target_frame' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("cluster_target_frame", clusterTargetFrame) || clusterTargetFrame.empty())
-    {
-        ROS_ERROR("No 'cluster_target_frame' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("occupancy_checker_action_name", occupancyCheckerActionName) || occupancyCheckerActionName.empty())
-    {
-        ROS_ERROR("No 'occupancy_checker_action_name' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    if (!nh.getParam("objects_object_topic", objectObjectsTopic) || objectObjectsTopic.empty())
-    {
-        ROS_ERROR("No 'objects_object_topic' specified as parameter");
-        return EXIT_FAILURE;
-    }
-    nh.getParam("publish_oriented_bbox", publishOrientedBBox);
-
-    // run cloud filtering and object detection
-    mdr_cloud_object_detection::CloudObjectDetectionNode objectDetection(nh, cloudTopic, filteredCloudTopic,
-                                                                      objectCloudTopic, transformTargetFrame,
-                                                                      clusterTargetFrame, occupancyCheckerActionName,
-                                                                      objectBoundsTopic, objectObjectsTopic,
-                                                                      publishOrientedBBox);
-
-    while (ros::ok())
-        ros::spin();
-
-    return 0;
-}
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp
new file mode 100644
index 000000000..ab8a1e975
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp
@@ -0,0 +1,79 @@
+#include "mdr_cloud_object_detection/CloudObjectDetection.h"
+
+int main(int pArgc, char** pArgv)
+{
+    ros::init(pArgc, pArgv, "cloud_object_detection");
+    ros::NodeHandle nh("~");
+
+    // load launch parameters
+    std::string cloudTopic;
+    if (!nh.getParam("cloud_topic", cloudTopic) || cloudTopic.empty())
+    {
+        ROS_ERROR("No 'cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string filteredCloudTopic;
+    if (!nh.getParam("filtered_cloud_topic", filteredCloudTopic) || filteredCloudTopic.empty())
+    {
+        ROS_ERROR("No 'filtered_cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string objectCloudTopic;
+    if (!nh.getParam("object_cloud_topic", objectCloudTopic) || objectCloudTopic.empty())
+    {
+        ROS_ERROR("No 'object_cloud_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string objectBoundsTopic;
+    if (!nh.getParam("objects_bounds_topic", objectBoundsTopic) || objectBoundsTopic.empty())
+    {
+        ROS_ERROR("No 'objects_bounds_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string transformTargetFrame;
+    if (!nh.getParam("transform_target_frame", transformTargetFrame) || transformTargetFrame.empty())
+    {
+        ROS_ERROR("No 'transform_target_frame' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string clusterTargetFrame;
+    if (!nh.getParam("cluster_target_frame", clusterTargetFrame) || clusterTargetFrame.empty())
+    {
+        ROS_ERROR("No 'cluster_target_frame' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string occupancyCheckerActionName;
+    if (!nh.getParam("occupancy_checker_action_name", occupancyCheckerActionName) || occupancyCheckerActionName.empty())
+    {
+        ROS_ERROR("No 'occupancy_checker_action_name' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    std::string objectObjectsTopic;
+    if (!nh.getParam("objects_object_topic", objectObjectsTopic) || objectObjectsTopic.empty())
+    {
+        ROS_ERROR("No 'objects_object_topic' specified as parameter");
+        return EXIT_FAILURE;
+    }
+
+    bool publishOrientedBBox = false;
+    nh.getParam("publish_oriented_bbox", publishOrientedBBox);
+
+    // run cloud filtering and object detection
+    mdr_cloud_object_detection::CloudObjectDetection objectDetection(nh, cloudTopic, filteredCloudTopic,
+                                                                      objectCloudTopic, transformTargetFrame,
+                                                                      clusterTargetFrame, occupancyCheckerActionName,
+                                                                      objectBoundsTopic, objectObjectsTopic,
+                                                                      publishOrientedBBox);
+
+    while (ros::ok())
+        ros::spin();
+
+    return 0;
+}

From 29496e980d209e64c899e8c34b239bdec485e75a Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 18:02:55 +0100
Subject: [PATCH 245/264] Cleanup and fixes

---
 .../object_detection_default_configs.yaml     | 14 +--
 .../CloudObjectDetection.h                    | 73 ++++++++-------
 .../EuclideanClusterParams.h                  |  6 ++
 .../ObjectCacheParams.h                       |  6 ++
 .../ObjectFilterParams.h                      |  6 ++
 .../PassThroughVoxelFilter.h                  | 10 ++-
 .../PassThroughVoxelFilterParams.h            |  6 ++
 .../mdr_cloud_object_detection/Utils.h        |  9 +-
 .../ros/launch/cloud_object_detection.launch  | 38 ++++++++
 .../ros/src/CloudObjectDetection.cpp          | 89 +++++++++++--------
 .../ros/src/PassThroughVoxelFilter.cpp        |  8 +-
 .../ros/src/main.cpp                          | 35 +++++---
 12 files changed, 206 insertions(+), 94 deletions(-)
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch

diff --git a/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
index a62e4ec61..0a497bcf9 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
+++ b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
@@ -1,5 +1,6 @@
-# config file loaded during launch as default configurations for the cloud filter, assuming base_link frame for
-# passthrough filter
+# config file loaded during launch as default configurations for the cloud object detector
+
+# passthrough and voxel-grid filter params in base_link frame
 passthrough_limit_min_x:  0.0
 passthrough_limit_max_x:  1.5
 passthrough_limit_min_y: -0.5
@@ -9,14 +10,17 @@ passthrough_limit_max_z:  0.2
 voxel_limit_min_z:        0.0
 voxel_limit_max_z:        1.8
 voxel_leaf_size:          0.02
-# config file loaded during launch as default configurations for the clustering algorithm
+
+# Euclidean clustering params
 cluster_tolerance:       0.05
 min_cluster_size:        4
 max_cluster_size:        10000
-# Object Filtering
+
+# Object filtering params
 max_bbox_edge_length:           0.3
 min_dist_from_occupied_cell:    0.05
-# Object Cache
+
+# Object cache params
 object_cache_time:            5.0
 similarity_threshold:           2.0
 position_history_cache_size:    10
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
index d6873b467..785c3adae 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
@@ -1,46 +1,42 @@
 /*!
- * @copyright 2018 Bonn-Rhein-Sieg University
+ * @copyright 2020 Bonn-Rhein-Sieg University
  *
  * @author Sushant Vijay Chavan
  *
  * @brief Class to detect objects from a point cloud
  */
+#ifndef MDR_CLOUD_OBJECT_DETECTION_H
+#define MDR_CLOUD_OBJECT_DETECTION_H
 
-#include <cmath>
 #include <actionlib/client/simple_action_client.h>
-#include <eigen_conversions/eigen_msg.h>
-#include <pcl_ros/transforms.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/common/common.h>
-#include <pcl/features/moment_of_inertia_estimation.h>
 #include <dynamic_reconfigure/server.h>
-#include <nav_msgs/OccupancyGrid.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl_ros/transforms.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Bool.h>
-#include <tf/transform_listener.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <visualization_msgs/MarkerArray.h>
+
 #include <mas_navigation_tools/OccupancyCheckerAction.h>
 #include <mas_perception_msgs/ObjectList.h>
-#include "mdr_cloud_object_detection/Color.h"
-#include "mdr_cloud_object_detection/Utils.h"
+
+#include <mdr_cloud_object_detection/Utils.h>
+#include <mdr_cloud_object_detection/PassThroughVoxelFilter.h>
 #include <mdr_cloud_object_detection/ObjectDetectionConfig.h>
 
 namespace mdr_cloud_object_detection
 {
     class CloudObjectDetection
     {
-    private:
+    protected:
         ros::NodeHandle mNodeHandle;
+        actionlib::SimpleActionClient<mas_navigation_tools::OccupancyCheckerAction> mOccupancyCheckerClient;
         dynamic_reconfigure::Server<ObjectDetectionConfig> mObjectDetectionConfigServer;
-        ros::Subscriber mCloudSub;
-        ros::Subscriber mResetSub;
+        ros::Subscriber mCloudInSub;
+        ros::Subscriber mResetObjectCacheSub;
         ros::Publisher mFilteredCloudPub;
         ros::Publisher mObjectCloudPub;
         ros::Publisher mObjectBoundsPub;
-        ros::Publisher mObjectObjectsPub;
-        actionlib::SimpleActionClient<mas_navigation_tools::OccupancyCheckerAction> mOccupancyCheckerClient;
+        ros::Publisher mObjectListPub;
         tf::TransformListener mTfListener;
         std::string mTransformTargetFrame;
         std::string mClusterTargetFrame;
@@ -59,27 +55,23 @@ namespace mdr_cloud_object_detection
 
     public:
         CloudObjectDetection(const ros::NodeHandle &pNodeHandle,
-                             const std::string &pCloudTopic,
+                             const std::string &pCloudInTopic,
                              const std::string &pFilteredCloudTopic,
                              const std::string &pObjectCloudTopic,
                              const std::string &pTransformTargetFrame,
                              const std::string &pClusterTargetFrame,
                              const std::string &pOccupancyCheckerActionName,
                              const std::string &pObjectsBoundsTopic,
-                             const std::string &pObjectObjectsTopic,
+                             const std::string &pObjectListTopic,
                              bool pPublishOrientedBBox);
 
-    private:
+    protected:
         void objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig,
                                            uint32_t pLevel);
 
         void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& pCloudMsgPtr);
 
-        void resetCallback(const std_msgs::Bool& reset);
-
-        void filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds);
-
-        void filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds);
+        void resetObjectCacheCallback(const std_msgs::Bool& reset);
 
         void getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
                               PointCloud::ConstPtr filteredCloud,
@@ -97,11 +89,22 @@ namespace mdr_cloud_object_detection
 
         void processNewClusters(const std::vector<PointCloud::Ptr>& clusterClouds);
 
+        void filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds);
+
+        void filterClusterCloudsNearOccupiedSpaces(std::vector<PointCloud::Ptr>& clusterClouds);
+
         void removeStaleObjects();
 
-        geometry_msgs::Point getGeomPoint(float x,
-                                          float y,
-                                          float z);
+        mas_perception_msgs::Object createObjectMessage(PointCloud::ConstPtr cloudPtr,
+                                                        int id);
+
+        void updateObjectMsgCache();
+
+        const mas_perception_msgs::Object* getObjectMessage(int id);
+
+        geometry_msgs::Point getPointMsg(float x,
+                                         float y,
+                                         float z);
 
         visualization_msgs::Marker getObjectBoundsMarker(const Eigen::Vector4f& min,
                                                          const Eigen::Vector4f& max,
@@ -117,13 +120,6 @@ namespace mdr_cloud_object_detection
                                                      const Eigen::Vector4f& max,
                                                      int id);
 
-        mas_perception_msgs::Object createObjectMessage(PointCloud::ConstPtr cloudPtr,
-                                                        int id);
-
-        void updateObjectMsgCache();
-
-        const mas_perception_msgs::Object* getObjectMessage(int id);
-
         void publishCloud(PointCloud::ConstPtr cloudPtr,
                           const ros::Publisher& publisher);
 
@@ -131,9 +127,12 @@ namespace mdr_cloud_object_detection
 
         void publishObjectBoundsMarkers();
 
-        void publishObjectObjectMessages();
+        void publishObjectListMessages();
 
         void cleanup();
     };
 
 }   // namespace mdr_cloud_object_detection
+
+
+#endif  // MDR_CLOUD_OBJECT_DETECTION_H
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
index fb02c71a7..7004d0d0b 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/EuclideanClusterParams.h
@@ -1,3 +1,9 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #ifndef MDR_CLOUD_OBJECT_DETECTION_EUCLIDEAN_CLUSTER_PARAMS
 #define MDR_CLOUD_OBJECT_DETECTION_EUCLIDEAN_CLUSTER_PARAMS
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
index 938aeb81a..7a185dd0e 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectCacheParams.h
@@ -1,3 +1,9 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #ifndef MDR_CLOUD_OBJECT_DETECTION_OBJECT_CACHE_PARAMS
 #define MDR_CLOUD_OBJECT_DETECTION_OBJECT_CACHE_PARAMS
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
index b2c3cca59..a234ec3f4 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/ObjectFilterParams.h
@@ -1,3 +1,9 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #ifndef MDR_CLOUD_OBJECT_DETECTION_OBJECT_FILTERING_PARAMS
 #define MDR_CLOUD_OBJECT_DETECTION_OBJECT_FILTERING_PARAMS
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
index b25a8c65d..2d16d1b19 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
@@ -1,11 +1,17 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
 #define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
 
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
 
-#include "mdr_cloud_object_detection/Aliases.h"
-#include "mdr_cloud_object_detection/PassThroughVoxelFilterParams.h"
+#include <mdr_cloud_object_detection/Aliases.h>
+#include <mdr_cloud_object_detection/PassThroughVoxelFilterParams.h>
 
 namespace mdr_cloud_object_detection
 {
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
index 31a7a2ed9..e336089ba 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
@@ -1,3 +1,9 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
 #define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
index 739b709bf..807661254 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
@@ -1,5 +1,10 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
 #include "mdr_cloud_object_detection/EuclideanClusterParams.h"
 #include "mdr_cloud_object_detection/ObjectCacheParams.h"
 #include "mdr_cloud_object_detection/ObjectFilterParams.h"
-
-#include "mdr_cloud_object_detection/PassThroughVoxelFilter.h"
+#include "mdr_cloud_object_detection/PassThroughVoxelFilterParams.h"
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch b/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch
new file mode 100644
index 000000000..ad74d1227
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Miscellaneous parameters -->
+    <arg name="transform_target_frame" default="base_link" />
+    <arg name="cluster_target_frame" default="map"/>
+    <arg name="occupancy_checker_action_name" default="occupancy_checker"/>
+    <arg name="publish_oriented_bbox" default="true"/>
+    <arg name="object_detection_config_file"
+         default="$(find mdr_cloud_object_detection)/ros/config/object_detection_default_configs.yaml" />
+
+    <!-- Input topic names -->
+    <arg name="cloud_in_topic" default="/hsrb/head_rgbd_sensor/depth_registered/rectified_points"/>
+
+    <!-- Output topic names -->
+    <arg name="filtered_cloud_topic" default="filtered_cloud"/>
+    <arg name="object_cloud_topic" default="object_cloud"/>
+    <arg name="objects_bounds_topic" default="object_bounds"/>
+    <arg name="object_list_topic" default="object_list"/>
+
+    <node pkg="mas_navigation_tools" type="occupancy_checker" name="occupancy_checker" output="screen" ns="mas_navigation_tools">
+        <param name="action_name" type="string" value="$(arg occupancy_checker_action_name)" />
+        <param name="map_topic" type="string" value="/map" />
+    </node>
+
+    <node pkg="mdr_cloud_object_detection" type="cloud_object_detection" name="cloud_object_detection" output="screen"
+          ns="mdr_cloud_object_detection">
+        <rosparam file="$(arg object_detection_config_file)" command="load"/>
+        <param name="cloud_in_topic" type="string" value="$(arg cloud_in_topic)" />
+        <param name="filtered_cloud_topic" type="string" value="$(arg filtered_cloud_topic)" />
+        <param name="object_cloud_topic" type="string" value="$(arg object_cloud_topic)" />
+        <param name="objects_bounds_topic" type="string" value="$(arg objects_bounds_topic)" />
+        <param name="publish_oriented_bbox" type="bool" value="$(arg publish_oriented_bbox)" />
+        <param name="object_list_topic" type="string" value="$(arg object_list_topic)" />
+        <param name="transform_target_frame" type="string" value="$(arg transform_target_frame)" />
+        <param name="cluster_target_frame" type="string" value="$(arg cluster_target_frame)" />
+        <param name="occupancy_checker_action_name" type="string" value="/mas_navigation_tools/$(arg occupancy_checker_action_name)" />
+    </node>
+</launch>
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
index 153e1c857..d562a16b9 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
@@ -1,24 +1,29 @@
 /*!
- * @copyright 2018 Bonn-Rhein-Sieg University
+ * @copyright 2020 Bonn-Rhein-Sieg University
  *
- * @author Sushant Chavan
+ * @author Sushant Vijay Chavan
  *
- * @brief script to detect objects from a point cloud
+ * @brief Class to detect objects from a point cloud
  */
+#include <pcl/common/common.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
-#include "mdr_cloud_object_detection/CloudObjectDetection.h"
+#include <mdr_cloud_object_detection/Color.h>
+#include <mdr_cloud_object_detection/CloudObjectDetection.h>
 
 using namespace mdr_cloud_object_detection;
 
 CloudObjectDetection::CloudObjectDetection(const ros::NodeHandle &pNodeHandle, 
-                                           const std::string &pCloudTopic,
+                                           const std::string &pCloudInTopic,
                                            const std::string &pFilteredCloudTopic,
                                            const std::string &pObjectCloudTopic,
                                            const std::string &pTransformTargetFrame,
                                            const std::string &pClusterTargetFrame,
                                            const std::string &pOccupancyCheckerActionName,
                                            const std::string &pObjectsBoundsTopic,
-                                           const std::string &pObjectObjectsTopic,
+                                           const std::string &pObjectListTopic,
                                            bool pPublishOrientedBBox)
 : mNodeHandle(pNodeHandle)
 , mObjectDetectionConfigServer(mNodeHandle)
@@ -36,12 +41,12 @@ CloudObjectDetection::CloudObjectDetection(const ros::NodeHandle &pNodeHandle,
     mObjectDetectionConfigServer.setCallback(odCallback);
 
     ROS_INFO("subscribing to point cloud topic and advertising processed result");
-    mCloudSub = mNodeHandle.subscribe(pCloudTopic, 1, &CloudObjectDetection::cloudCallback, this);
-    mResetSub = mNodeHandle.subscribe("reset_cache", 1, &CloudObjectDetection::resetCallback, this);
+    mCloudInSub = mNodeHandle.subscribe(pCloudInTopic, 1, &CloudObjectDetection::cloudCallback, this);
+    mResetObjectCacheSub = mNodeHandle.subscribe("reset_cache", 1, &CloudObjectDetection::resetObjectCacheCallback, this);
     mFilteredCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pFilteredCloudTopic, 1);
     mObjectCloudPub = mNodeHandle.advertise<sensor_msgs::PointCloud2>(pObjectCloudTopic, 1);
     mObjectBoundsPub = mNodeHandle.advertise<visualization_msgs::MarkerArray>(pObjectsBoundsTopic, 1);
-    mObjectObjectsPub = mNodeHandle.advertise<mas_perception_msgs::ObjectList>(pObjectObjectsTopic, 1);
+    mObjectListPub = mNodeHandle.advertise<mas_perception_msgs::ObjectList>(pObjectListTopic, 1);
 }
 
 void CloudObjectDetection::objectDetectionConfigCallback(const ObjectDetectionConfig &pConfig,
@@ -81,7 +86,7 @@ void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPt
     // do not process cloud when there's no subscriber
     if (mFilteredCloudPub.getNumSubscribers() == 0 &&
         mObjectBoundsPub.getNumSubscribers() == 0 &&
-        mObjectObjectsPub.getNumSubscribers() == 0 &&
+        mObjectListPub.getNumSubscribers() == 0 &&
         mObjectCloudPub.getNumSubscribers() == 0)
         return;
 
@@ -142,12 +147,12 @@ void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPt
 
     publishObjectCloud();
     publishObjectBoundsMarkers();
-    publishObjectObjectMessages();
+    publishObjectListMessages();
 
     cleanup();
 }
 
-void CloudObjectDetection::resetCallback(const std_msgs::Bool& reset)
+void CloudObjectDetection::resetObjectCacheCallback(const std_msgs::Bool& reset)
 {
     if (reset.data)
     {
@@ -227,8 +232,8 @@ void CloudObjectDetection::filterClusterCloudsNearOccupiedSpaces(std::vector<Poi
 }
 
 void CloudObjectDetection::getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
-                     PointCloud::ConstPtr filteredCloud,
-                     const std::vector<pcl::PointIndices>& cluster_indices)
+                                            PointCloud::ConstPtr filteredCloud,
+                                            const std::vector<pcl::PointIndices>& cluster_indices)
 {
     for (const auto& idx: cluster_indices)
     {
@@ -254,7 +259,8 @@ void CloudObjectDetection::getClusterClouds(std::vector<PointCloud::Ptr>& cluste
 }
 
 // Cloud comparison sample from https://stackoverflow.com/a/55930847
-float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& tree, const PointT& pt)
+float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& tree, 
+                                            const PointT& pt)
 {
     const int k = 1;
     std::vector<int> indices (k);
@@ -268,7 +274,9 @@ float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& t
 // compare cloudB to cloudA
 // use threshold for identifying outliers and not considering those for the similarity
 // a good value for threshold is 5 * <cloud_resolution>, e.g. 10cm for a cloud with 2cm resolution
-float CloudObjectDetection::getCloudSimilarity(const PointCloud& cloudA, const PointCloud& cloudB, float threshold)
+float CloudObjectDetection::getCloudSimilarity(const PointCloud& cloudA,
+                                               const PointCloud& cloudB,
+                                               float threshold)
 {
     // compare B to A
     int num_outlier = 0;
@@ -291,7 +299,8 @@ float CloudObjectDetection::getCloudSimilarity(const PointCloud& cloudA, const P
     return sum / (cloudB.size() - num_outlier);
 }
 
-bool CloudObjectDetection::isNewObject(const PointCloud& cloud, int& knownObjectId)
+bool CloudObjectDetection::isNewObject(const PointCloud& cloud,
+                                       int& knownObjectId)
 {
     for (const auto& o : mObjectsCache)
     {
@@ -365,7 +374,9 @@ void CloudObjectDetection::removeStaleObjects()
     }
 }
 
-geometry_msgs::Point CloudObjectDetection::getGeomPoint(float x, float y, float z)
+geometry_msgs::Point CloudObjectDetection::getPointMsg(float x,
+                                                       float y,
+                                                       float z)
 {
     geometry_msgs::Point p;
     p.x = x;
@@ -374,7 +385,9 @@ geometry_msgs::Point CloudObjectDetection::getGeomPoint(float x, float y, float
     return p;
 }
 
-visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const Eigen::Vector4f& min,
+                                                                       const Eigen::Vector4f& max,
+                                                                       int id)
 {
     visualization_msgs::Marker marker;
     marker.type = visualization_msgs::Marker::LINE_LIST;
@@ -387,18 +400,19 @@ visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const Eig
     marker.ns = "";
     marker.id = id;
     marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
-    marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
-    marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
-    marker.points.push_back(getGeomPoint(min[0], max[1], min[2]));
-    marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
-    marker.points.push_back(getGeomPoint(max[0], max[1], min[2]));
-    marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
-    marker.points.push_back(getGeomPoint(max[0], min[1], min[2]));
-    marker.points.push_back(getGeomPoint(min[0], min[1], min[2]));
+    marker.points.push_back(getPointMsg(min[0], min[1], min[2]));
+    marker.points.push_back(getPointMsg(min[0], max[1], min[2]));
+    marker.points.push_back(getPointMsg(min[0], max[1], min[2]));
+    marker.points.push_back(getPointMsg(max[0], max[1], min[2]));
+    marker.points.push_back(getPointMsg(max[0], max[1], min[2]));
+    marker.points.push_back(getPointMsg(max[0], min[1], min[2]));
+    marker.points.push_back(getPointMsg(max[0], min[1], min[2]));
+    marker.points.push_back(getPointMsg(min[0], min[1], min[2]));
     return marker;
 }
 
-visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg, int id)
+visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const mas_perception_msgs::Object& objMsg,
+                                                                       int id)
 {
     visualization_msgs::Marker marker;
     marker.type = visualization_msgs::Marker::LINE_LIST;
@@ -440,7 +454,8 @@ visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const mas
     return marker;
 }
 
-visualization_msgs::Marker CloudObjectDetection::getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg, int id)
+visualization_msgs::Marker CloudObjectDetection::getObjectOrientationMarker(const mas_perception_msgs::Object& objMsg,
+                                                                            int id)
 {
     visualization_msgs::Marker marker;
     marker.type = visualization_msgs::Marker::ARROW;
@@ -457,7 +472,9 @@ visualization_msgs::Marker CloudObjectDetection::getObjectOrientationMarker(cons
     return marker;
 }
 
-visualization_msgs::Marker CloudObjectDetection::getObjectIdMarker(const Eigen::Vector4f& min, const Eigen::Vector4f& max, int id)
+visualization_msgs::Marker CloudObjectDetection::getObjectIdMarker(const Eigen::Vector4f& min,
+                                                                   const Eigen::Vector4f& max,
+                                                                   int id)
 {
     Eigen::Vector4f center = min + (max - min)/2.0;
     visualization_msgs::Marker marker;
@@ -474,7 +491,8 @@ visualization_msgs::Marker CloudObjectDetection::getObjectIdMarker(const Eigen::
     return marker;
 }
 
-mas_perception_msgs::Object CloudObjectDetection::createObjectMessage(PointCloud::ConstPtr cloudPtr, int id)
+mas_perception_msgs::Object CloudObjectDetection::createObjectMessage(PointCloud::ConstPtr cloudPtr,
+                                                                      int id)
 {
     // Solution inspired from: https://stackoverflow.com/a/49705361
     PointT minPoint;
@@ -582,7 +600,8 @@ const mas_perception_msgs::Object* CloudObjectDetection::getObjectMessage(int id
     return &(itr->second);
 }
 
-void CloudObjectDetection::publishCloud(PointCloud::ConstPtr cloudPtr, const ros::Publisher& publisher)
+void CloudObjectDetection::publishCloud(PointCloud::ConstPtr cloudPtr,
+                                        const ros::Publisher& publisher)
 {
     // publish the cloud
     sensor_msgs::PointCloud2::Ptr cloudMsgPtr = boost::make_shared<sensor_msgs::PointCloud2>();
@@ -642,9 +661,9 @@ void CloudObjectDetection::publishObjectBoundsMarkers()
     mObjectBoundsPub.publish(markerArray);
 }
 
-void CloudObjectDetection::publishObjectObjectMessages()
+void CloudObjectDetection::publishObjectListMessages()
 {
-    if (mObjectObjectsPub.getNumSubscribers() <= 0)
+    if (mObjectListPub.getNumSubscribers() <= 0)
         return;
 
     updateObjectMsgCache();
@@ -653,7 +672,7 @@ void CloudObjectDetection::publishObjectObjectMessages()
     {
         objectList.objects.push_back(msg.second);
     }
-    mObjectObjectsPub.publish(objectList);
+    mObjectListPub.publish(objectList);
 }
 
 void CloudObjectDetection::cleanup()
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
index d55bf8700..3255e8b6c 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
@@ -1,4 +1,10 @@
-#include "mdr_cloud_object_detection/PassThroughVoxelFilter.h"
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
+#include <mdr_cloud_object_detection/PassThroughVoxelFilter.h>
 
 using namespace mdr_cloud_object_detection;
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp
index ab8a1e975..d8e3f69a2 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/main.cpp
@@ -1,4 +1,10 @@
-#include "mdr_cloud_object_detection/CloudObjectDetection.h"
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
+#include <mdr_cloud_object_detection/CloudObjectDetection.h>
 
 int main(int pArgc, char** pArgv)
 {
@@ -6,10 +12,10 @@ int main(int pArgc, char** pArgv)
     ros::NodeHandle nh("~");
 
     // load launch parameters
-    std::string cloudTopic;
-    if (!nh.getParam("cloud_topic", cloudTopic) || cloudTopic.empty())
+    std::string cloudInTopic;
+    if (!nh.getParam("cloud_in_topic", cloudInTopic) || cloudInTopic.empty())
     {
-        ROS_ERROR("No 'cloud_topic' specified as parameter");
+        ROS_ERROR("No 'cloud_in_topic' specified as parameter");
         return EXIT_FAILURE;
     }
 
@@ -55,10 +61,10 @@ int main(int pArgc, char** pArgv)
         return EXIT_FAILURE;
     }
 
-    std::string objectObjectsTopic;
-    if (!nh.getParam("objects_object_topic", objectObjectsTopic) || objectObjectsTopic.empty())
+    std::string objectListTopic;
+    if (!nh.getParam("object_list_topic", objectListTopic) || objectListTopic.empty())
     {
-        ROS_ERROR("No 'objects_object_topic' specified as parameter");
+        ROS_ERROR("No 'object_list_topic' specified as parameter");
         return EXIT_FAILURE;
     }
 
@@ -66,11 +72,16 @@ int main(int pArgc, char** pArgv)
     nh.getParam("publish_oriented_bbox", publishOrientedBBox);
 
     // run cloud filtering and object detection
-    mdr_cloud_object_detection::CloudObjectDetection objectDetection(nh, cloudTopic, filteredCloudTopic,
-                                                                      objectCloudTopic, transformTargetFrame,
-                                                                      clusterTargetFrame, occupancyCheckerActionName,
-                                                                      objectBoundsTopic, objectObjectsTopic,
-                                                                      publishOrientedBBox);
+    mdr_cloud_object_detection::CloudObjectDetection objectDetection(nh,
+                                                                     cloudInTopic,
+                                                                     filteredCloudTopic,
+                                                                     objectCloudTopic,
+                                                                     transformTargetFrame,
+                                                                     clusterTargetFrame,
+                                                                     occupancyCheckerActionName,
+                                                                     objectBoundsTopic,
+                                                                     objectListTopic,
+                                                                     publishOrientedBBox);
 
     while (ros::ok())
         ros::spin();

From 282ba060cc06b8b04594df7f7cbe69a2c48638be Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 18:38:35 +0100
Subject: [PATCH 246/264] Remove comments from CMakeLists.txt and package.xml

---
 mdr_perception/mdr_cloud_object_detection/CMakeLists.txt | 3 ---
 mdr_perception/mdr_cloud_object_detection/package.xml    | 7 -------
 2 files changed, 10 deletions(-)

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
index 301429268..13cb4b1e6 100644
--- a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -37,7 +37,6 @@ include_directories(
   ros/include
   ${catkin_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
-#   ${Boost_INCLUDE_DIRS}
 )
 
 ###################################
@@ -48,9 +47,7 @@ add_executable(cloud_object_detection
   ros/src/main.cpp
 )
 target_link_libraries(cloud_object_detection
-#   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
-#   ${OpenCV_LIBRARIES}
 )
 
 #############
diff --git a/mdr_perception/mdr_cloud_object_detection/package.xml b/mdr_perception/mdr_cloud_object_detection/package.xml
index 804112b82..33dec77fa 100644
--- a/mdr_perception/mdr_cloud_object_detection/package.xml
+++ b/mdr_perception/mdr_cloud_object_detection/package.xml
@@ -19,14 +19,7 @@
   <depend>mas_navigation_tools</depend>
   <depend>mas_perception_msgs</depend>
 
-  <!-- <build_export_depend>dynamic_reconfigure</build_export_depend>
-  <build_export_depend>pcl_ros</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend> -->
   <build_export_depend>message_runtime</build_export_depend>
 
-  <!-- <exec_depend>dynamic_reconfigure</exec_depend> -->
-
   <exec_depend>message_runtime</exec_depend>
-  <!-- <exec_depend>pcl_ros</exec_depend>
-  <exec_depend>roscpp</exec_depend> -->
 </package>

From d1a5428ef8ccba74f63f4665c681d2fd12df72a0 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 20:00:20 +0100
Subject: [PATCH 247/264] Fix errors in creating the object messages for each
 cluster

---
 .../CloudObjectDetection.h                    |   3 +
 .../ros/src/CloudObjectDetection.cpp          | 104 ++++++++++--------
 2 files changed, 60 insertions(+), 47 deletions(-)

diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
index 785c3adae..67fd68ef0 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
@@ -73,6 +73,9 @@ namespace mdr_cloud_object_detection
 
         void resetObjectCacheCallback(const std_msgs::Bool& reset);
 
+        PointCloud::Ptr transformPointCloud(const PointCloud& cloudIn,
+                                            const std::string& targetFrame);
+
         void getClusterClouds(std::vector<PointCloud::Ptr>& clusterClouds,
                               PointCloud::ConstPtr filteredCloud,
                               const std::vector<pcl::PointIndices>& cluster_indices);
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
index d562a16b9..4e8ebd97b 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
@@ -95,19 +95,14 @@ void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPt
         return;
 
     // transform the cloud to a desired frame
-    auto transformedCloudPtr = boost::make_shared<sensor_msgs::PointCloud2>();
-    if (!pcl_ros::transformPointCloud(mTransformTargetFrame, *pCloudMsgPtr, *transformedCloudPtr, mTfListener))
-    {
-        ROS_WARN("failed to transform cloud to frame '%s' from frame '%s'",
-                 mTransformTargetFrame.c_str(), pCloudMsgPtr->header.frame_id.c_str());
+    PointCloud::Ptr cloudInPtr = boost::make_shared<PointCloud>();
+    pcl::fromROSMsg(*pCloudMsgPtr, *cloudInPtr);
+    PointCloud::Ptr transformedCloudPtr = transformPointCloud(*cloudInPtr, mTransformTargetFrame);
+    if (transformedCloudPtr == nullptr)
         return;
-    }
 
     // filter the cloud
-    PointCloud::Ptr pclCloudPtr = boost::make_shared<PointCloud>();
-    pcl::fromROSMsg(*transformedCloudPtr, *pclCloudPtr);
-    PointCloud::Ptr filteredCloudPtr = mCloudFilter.filterCloud(pclCloudPtr);
-
+    PointCloud::Ptr filteredCloudPtr = mCloudFilter.filterCloud(transformedCloudPtr);
     if (filteredCloudPtr->size() < mClusterParams.mMinClusterSize)
     {
         // Stop processing the point cloud if the filtered cloud does
@@ -115,14 +110,14 @@ void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPt
         return;
     }
 
-    mCurrTime = pCloudMsgPtr->header.stamp.sec;
-
     if (mFilteredCloudPub.getNumSubscribers() > 0)
     {
         // publish the filtered cloud for debugging
         publishCloud(filteredCloudPtr, mFilteredCloudPub);
     }
 
+    mCurrTime = pCloudMsgPtr->header.stamp.sec;
+
     // Euclidean clustering
     pcl::search::Search<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
     tree->setInputCloud(filteredCloudPtr);
@@ -165,6 +160,19 @@ void CloudObjectDetection::resetObjectCacheCallback(const std_msgs::Bool& reset)
     }
 }
 
+PointCloud::Ptr CloudObjectDetection::transformPointCloud(const PointCloud& cloudIn,
+                                                          const std::string& targetFrame)
+{
+    PointCloud::Ptr transformedCloudPtr = boost::make_shared<PointCloud>();
+    if (!pcl_ros::transformPointCloud(targetFrame, cloudIn, *transformedCloudPtr, mTfListener))
+    {
+        ROS_WARN("Failed to transform cloud to frame '%s' from frame '%s'.",
+                targetFrame.c_str(), cloudIn.header.frame_id.c_str());
+        return nullptr;
+    }
+    return transformedCloudPtr;
+}
+
 void CloudObjectDetection::filterClusterCloudsBySize(std::vector<PointCloud::Ptr>& clusterClouds)
 {
     // Check if the size of the object is within the specified limits
@@ -246,11 +254,10 @@ void CloudObjectDetection::getClusterClouds(std::vector<PointCloud::Ptr>& cluste
         clusterCloud->height = 1;
         clusterCloud->is_dense = true;
 
-        PointCloud::Ptr transformedCloudPtr = boost::make_shared<PointCloud>();
-        if (!pcl_ros::transformPointCloud(mClusterTargetFrame, *clusterCloud, *transformedCloudPtr, mTfListener))
+        PointCloud::Ptr transformedCloudPtr = transformPointCloud(*clusterCloud, mClusterTargetFrame);
+        if (transformedCloudPtr == nullptr)
         {
-            ROS_WARN("Failed to transform cluster cloud to frame '%s' from frame '%s'. This cluster will be skipped in the current frame!",
-                    mClusterTargetFrame.c_str(), clusterCloud->header.frame_id.c_str());
+            ROS_WARN("Failed to transform cluster cloudThis cluster will be skipped in the current frame!");
             continue;
         }
 
@@ -517,15 +524,22 @@ mas_perception_msgs::Object CloudObjectDetection::createObjectMessage(PointCloud
     points.push_back(Eigen::Vector3f(maxPoint.x, maxPoint.y, maxPoint.z));
     points.push_back(Eigen::Vector3f(maxPoint.x, minPoint.y, maxPoint.z));
 
-    // Copy the cluster cloud and set the z-value to that of the mass center for all points
-    PointCloud::Ptr copy(new PointCloud);
-    pcl::copyPointCloud(*cloudPtr, *copy);
-    for(unsigned int i = 0; i < copy->points.size(); i++)
-    {
-        copy->points[i].z = massCenter.z();
-    }
-
     mas_perception_msgs::Object msg;
+    msg.name = std::string("object_") + std::to_string(id);
+
+    // Add the cluster cloud
+    sensor_msgs::PointCloud2::Ptr cloudMsgPtr = boost::make_shared<sensor_msgs::PointCloud2>();
+    pcl::toROSMsg(*cloudPtr, *cloudMsgPtr);
+    mas_perception_msgs::ObjectView view;
+    view.point_cloud = *cloudMsgPtr;
+    msg.views.push_back(view);
+
+    // Add bounding box values
+    geometry_msgs::Point center;
+    center.x = massCenter.x();
+    center.y = massCenter.y();
+    center.z = massCenter.z();
+    msg.bounding_box.center = center;
     for (unsigned int i = 0 ; i < points.size(); i++)
     {
         points[i] = rotMatrix * points[i] + offset;
@@ -535,43 +549,39 @@ mas_perception_msgs::Object CloudObjectDetection::createObjectMessage(PointCloud
         p.z = points[i].z();
         msg.bounding_box.vertices.push_back(p);
     }
-
     msg.bounding_box.dimensions.x = std::abs(maxPoint.x - minPoint.x);
     msg.bounding_box.dimensions.y = std::abs(maxPoint.y - minPoint.y);
     msg.bounding_box.dimensions.z = std::abs(maxPoint.z - minPoint.z);
+    msg.dimensions.header.frame_id = mClusterTargetFrame;
+    msg.dimensions.vector = msg.bounding_box.dimensions;
 
-    geometry_msgs::Point center;
-    center.x = massCenter.x();
-    center.y = massCenter.y();
-    center.z = massCenter.z();
-    msg.bounding_box.center = center;
+    // Copy the cluster cloud and set the z-value to that of the mass center for all points
+    PointCloud::Ptr copy(new PointCloud);
+    pcl::copyPointCloud(*cloudPtr, *copy);
+    for(unsigned int i = 0; i < copy->points.size(); i++)
+    {
+        copy->points[i].z = massCenter.z();
+    }
 
-    // Compute the pose for the object
+    // Add pose to the Object message
+    msg.pose.header.frame_id = mClusterTargetFrame;
+    msg.pose.pose.position = center;
+    // Compute the orientation of the object in the XY plane of the mClusterTargetFrame
     pcl::MomentOfInertiaEstimation<PointT> orientationEstimator;
     Eigen::Vector3f major_vector, middle_vector, minor_vector;
     orientationEstimator.setInputCloud(copy);
     orientationEstimator.compute();
     orientationEstimator.getEigenVectors (major_vector, middle_vector, minor_vector);
-
-    // Add pose to the Object message
     double yaw = std::atan2(major_vector.y(), major_vector.x());
+    // restrict yaw to the first two quadrants of the mClusterTargetFrame
+    constexpr double pi = 3.14159265358979323846;
+    if (yaw > pi/2.0)
+        yaw -= pi;
+    else if (yaw < -pi/2.0)
+        yaw += pi;
     tf2::Quaternion orientation;
     orientation.setRPY(0.0, 0.0, yaw);
-    msg.pose.pose.position = center;
     msg.pose.pose.orientation = tf2::toMsg(orientation);
-    msg.pose.header.frame_id = mClusterTargetFrame;
-
-    // Add dimensions to the Object message
-    Eigen::Vector4f min, max;
-    pcl::getMinMax3D(*cloudPtr, min, max);
-    geometry_msgs::Vector3 dimensions;
-    dimensions.x = std::abs(max[0] - min[0]);
-    dimensions.y = std::abs(max[1] - min[1]);
-    dimensions.z = std::abs(max[2] - min[2]);
-    msg.dimensions.header.frame_id = mClusterTargetFrame;
-    msg.dimensions.vector = dimensions;
-
-    msg.name = std::string("object_") + std::to_string(id);
 
     return msg;
 }

From eca2500251d55a0e2111f78c93671eafc0e23988 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Sat, 19 Dec 2020 20:25:06 +0100
Subject: [PATCH 248/264] Use the launch file of the occupancy_checker

---
 .../ros/launch/cloud_object_detection.launch           | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch b/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch
index ad74d1227..0bdd175f7 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch
+++ b/mdr_perception/mdr_cloud_object_detection/ros/launch/cloud_object_detection.launch
@@ -10,6 +10,7 @@
 
     <!-- Input topic names -->
     <arg name="cloud_in_topic" default="/hsrb/head_rgbd_sensor/depth_registered/rectified_points"/>
+    <arg name="map_topic" default="/map"/>
 
     <!-- Output topic names -->
     <arg name="filtered_cloud_topic" default="filtered_cloud"/>
@@ -17,10 +18,11 @@
     <arg name="objects_bounds_topic" default="object_bounds"/>
     <arg name="object_list_topic" default="object_list"/>
 
-    <node pkg="mas_navigation_tools" type="occupancy_checker" name="occupancy_checker" output="screen" ns="mas_navigation_tools">
-        <param name="action_name" type="string" value="$(arg occupancy_checker_action_name)" />
-        <param name="map_topic" type="string" value="/map" />
-    </node>
+    <!-- Launch the occupancy_checker -->
+    <include file="$(find mas_navigation_tools)/ros/launch/occupancy_checker.launch">
+        <arg name="action_name" value="$(arg occupancy_checker_action_name)"/>
+        <arg name="map_topic" value="$(arg map_topic)"/>
+    </include>
 
     <node pkg="mdr_cloud_object_detection" type="cloud_object_detection" name="cloud_object_detection" output="screen"
           ns="mdr_cloud_object_detection">

From d3a6abc4fa4a3078787f8b2cbc1a6bc3e055b552 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 30 Dec 2020 14:49:16 +0100
Subject: [PATCH 249/264] [find_people/package.xml] Removed pip packages from
 the dependency list

rosdep is apparently not able to find any of them
---
 .../mdr_perception_actions/mdr_find_people/package.xml     | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
index d2c8b54ef..7d03a0536 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/package.xml
@@ -26,11 +26,4 @@
   <run_depend>message_runtime</run_depend>
   <run_depend>mas_perception_msgs</run_depend>
   <run_depend>mas_perception_libs</run_depend>
-
-  <run_depend>numpy</run_depend>
-  <run_depend>torch</run_depend>
-  <run_depend>scikit-learn</run_depend>
-  <run_depend>face_recognition</run_depend>
-  <run_depend>python-pcl</run_depend>
-
 </package>

From 047af4e7f3c06f16c47728bef29c5dbff3876113 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 30 Dec 2020 16:19:50 +0100
Subject: [PATCH 250/264] [cloud_object_detection] Added tf2_geometry_msgs to
 the list of dependencies

---
 mdr_perception/mdr_cloud_object_detection/CMakeLists.txt | 1 +
 mdr_perception/mdr_cloud_object_detection/package.xml    | 1 +
 2 files changed, 2 insertions(+)

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
index 13cb4b1e6..33ec8c4c2 100644
--- a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   actionlib_msgs
   actionlib
   dynamic_reconfigure
+  tf2_geometry_msgs
   pcl_ros
   roscpp
   mas_navigation_tools
diff --git a/mdr_perception/mdr_cloud_object_detection/package.xml b/mdr_perception/mdr_cloud_object_detection/package.xml
index 33dec77fa..b776a5443 100644
--- a/mdr_perception/mdr_cloud_object_detection/package.xml
+++ b/mdr_perception/mdr_cloud_object_detection/package.xml
@@ -14,6 +14,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>dynamic_reconfigure</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>pcl_ros</depend>
   <depend>roscpp</depend>
   <depend>mas_navigation_tools</depend>

From 59717ba25488c2597a68c44e88e0fb1fc4c27cd3 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 30 Dec 2020 17:51:16 +0100
Subject: [PATCH 251/264] [cloud_object_detection/CMakeLists] Added catkin
 dependency on dynamic_reconfigure

---
 mdr_perception/mdr_cloud_object_detection/CMakeLists.txt | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
index 33ec8c4c2..fdc3cd256 100644
--- a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -28,6 +28,7 @@ catkin_package(
   LIBRARIES
     ${PROJECT_NAME}
   CATKIN_DEPENDS
+    dynamic_reconfigure
     mas_navigation_tools
 )
 
@@ -47,6 +48,9 @@ add_executable(cloud_object_detection
   ros/src/CloudObjectDetection.cpp
   ros/src/main.cpp
 )
+
+add_dependencies(cloud_object_detection ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
 target_link_libraries(cloud_object_detection
   ${catkin_LIBRARIES}
 )

From b3b77c27409d7983e655212b98c123273aa7e391 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 30 Dec 2020 19:45:25 +0100
Subject: [PATCH 252/264] [manipulation_msgs] Added actionlib to the list of
 dependencies

The build is failing otherwise (even though the package doesn't have a
**direct** dependency on it)
---
 mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt | 2 ++
 mdr_msgs/mdr_manipulation_msgs/package.xml    | 1 +
 2 files changed, 3 insertions(+)

diff --git a/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt b/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
index 6f0529b65..acb40c51a 100644
--- a/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
+++ b/mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt
@@ -3,6 +3,7 @@ project(mdr_manipulation_msgs)
 
 find_package(catkin REQUIRED COMPONENTS
   message_generation
+  actionlib_msgs
   geometry_msgs
   mas_perception_msgs
 )
@@ -14,6 +15,7 @@ add_service_files(FILES
 )
 
 generate_messages(DEPENDENCIES
+  actionlib_msgs
   geometry_msgs
   mas_perception_msgs
 )
diff --git a/mdr_msgs/mdr_manipulation_msgs/package.xml b/mdr_msgs/mdr_manipulation_msgs/package.xml
index 85643e635..df9f3d670 100644
--- a/mdr_msgs/mdr_manipulation_msgs/package.xml
+++ b/mdr_msgs/mdr_manipulation_msgs/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>message_generation</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>mas_perception_msgs</build_depend>
 

From f0a55bc969fcf67069b8e7f68fb849a1c09c94a2 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 31 Dec 2020 01:28:57 +0100
Subject: [PATCH 253/264] [hri_behaviours/receive_object] Bug fixes

* Fixed incorrect object category setting
* Removed unreachable outcome
---
 .../ros/src/mdr_hri_behaviours/receive_object.py               | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
index 82ee64664..bc45229d3 100644
--- a/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
+++ b/mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/receive_object.py
@@ -59,7 +59,7 @@ def execute(self, userdata):
 
             object_category = self.object_category
             if not object_to_transport:
-                object_to_category = userdata.object_category
+                object_category = userdata.object_category
 
             estimated_size_m = self._get_estimated_object_size(self.gripper_controller.get_opening_angle())
             self._insert_object_in_kb(object_to_transport, object_category, estimated_size_m)
@@ -72,7 +72,6 @@ def execute(self, userdata):
                 return 'failed_after_retrying'
             self.retry_count += 1
             return 'failed'
-        return 'succeeded'
 
     def _get_estimated_object_size(self, angle):
         '''Get estimate of object size from gripper opening angle, assuming

From 41e0e48f3be01bdfe205dd45c043ea2ebb175264 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 31 Dec 2020 11:31:00 +0100
Subject: [PATCH 254/264] Resolved various Coday issues (mostly unused imports)

---
 .../ros/src/CloudObjectDetection.cpp                      | 6 +++---
 .../ros/src/mdr_move_arm_action/dmp.py                    | 8 ++------
 .../ros/src/mdr_find_people/find_people.py                | 2 +-
 .../src/mdr_wrs_tidy_up/scenario_states/find_objects.py   | 2 +-
 .../src/mdr_wrs_tidy_up/scenario_states/release_object.py | 6 +-----
 .../scenario_states/select_storage_location.py            | 1 -
 .../mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py      | 4 ++--
 7 files changed, 10 insertions(+), 19 deletions(-)

diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
index 4e8ebd97b..8e1d069a4 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
@@ -15,7 +15,7 @@
 
 using namespace mdr_cloud_object_detection;
 
-CloudObjectDetection::CloudObjectDetection(const ros::NodeHandle &pNodeHandle, 
+CloudObjectDetection::CloudObjectDetection(const ros::NodeHandle &pNodeHandle,
                                            const std::string &pCloudInTopic,
                                            const std::string &pFilteredCloudTopic,
                                            const std::string &pObjectCloudTopic,
@@ -129,7 +129,7 @@ void CloudObjectDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPt
                                   mClusterParams.mMinClusterSize,
                                   mClusterParams.mMaxClusterSize);
 
-    if (cluster_indices.size() <= 0)
+    if (cluster_indices.size() == 0)
         return;
 
     std::vector<PointCloud::Ptr> clusterClouds;
@@ -266,7 +266,7 @@ void CloudObjectDetection::getClusterClouds(std::vector<PointCloud::Ptr>& cluste
 }
 
 // Cloud comparison sample from https://stackoverflow.com/a/55930847
-float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& tree, 
+float CloudObjectDetection::nearestDistance(const pcl::search::KdTree<PointT>& tree,
                                             const PointT& pt)
 {
     const int k = 1;
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 14f54bd71..2dfbbaf21 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -6,9 +6,7 @@
 import tf
 from ros_dmp.roll_dmp import RollDmp
 import pydmps
-
-import yaml
-
+from mas_tools.file_utils import load_yaml_file
 
 class DMPExecutor(object):
     def __init__(self, dmp_name, tau):
@@ -273,9 +271,7 @@ def instantiate_dmp(self, initial_pose, goal_pose):
         initial_pose: numpy.ndarray -- initial pose coordinates of end-effector
         goal_pose: numpy.ndarray -- target pose coordinates of end-effector
         '''
-        with open(self.dmp_name) as f:
-            dmp_weights_dict = yaml.load(f)
-
+        dmp_weights_dict = load_yaml_file(self.dmp_name)
         n_dmps, n_bfs = len(dmp_weights_dict), len(dmp_weights_dict['x'])
 
         dmp_weights = np.zeros((n_dmps, n_bfs))
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index 068ab74a9..ac8a14633 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -12,7 +12,7 @@
 
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase, TorchImageDetector
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz, crop_organized_cloud_msg
+                                      crop_cloud_to_xyz
 from mas_perception_libs.visualization import draw_labeled_boxes
 
 import dataset_interface.object_detection.transforms as T
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
index 0df3a8a45..4c178c474 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/find_objects.py
@@ -7,7 +7,7 @@
 import actionlib
 from std_msgs.msg import Bool
 
-from mas_perception_msgs.msg import ObjectList, DetectObjectsAction, DetectObjectsActionGoal
+from mas_perception_msgs.msg import ObjectList, DetectObjectsAction
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
 
 def get_plane_polygon(center_position, dimensions):
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
index 4dac6ce53..326c79452 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/release_object.py
@@ -3,11 +3,7 @@
 import rospy
 import actionlib
 
-from std_msgs.msg import Header
-from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped
-
-from action_execution.action import Action
-from ae_ros_message_converter.converter_factory import ConverterFactory
+from geometry_msgs.msg import PoseStamped
 
 from mdr_place_action.msg import PlaceAction, PlaceGoal
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
index 007b9f221..65ad81d10 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/scenario_states/select_storage_location.py
@@ -1,5 +1,4 @@
 import random
-import rospy
 from mas_tools.ros_utils import get_package_path
 from mas_tools.file_utils import load_yaml_file
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
index 8767cc72c..c37e37980 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_wrs_tidy_up/ros/src/mdr_wrs_tidy_up/utils.py
@@ -18,8 +18,8 @@ def load_obstacle_detection_params(filename=None):
 def reconfigure_object_detection_params(z_min, z_max, node_name = "/mas_perception/cloud_obstacle_detection"):
     try:
         client = dynamic_reconfigure.client.Client(node_name, timeout=1.5)
-    except Exception as e:
-        rospy.logerr("Service {0} does not exist".format(node_name + '/set_parameters'))
+    except Exception as exc:
+        rospy.logerr("Service {0} does not exist: {1}".format(node_name + '/set_parameters', str(exc)))
         return False
 
     updates = {"passthrough_limit_min_z":  z_min,

From 11b656d3d8ccb60fa87b51bfa144b223febb1916 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Thu, 31 Dec 2020 12:16:27 +0100
Subject: [PATCH 255/264] [Codacy-Fix] Remove unused imports

---
 .../mdr_place_action/ros/src/mdr_place_action/action_states.py | 2 +-
 .../mdr_find_people/ros/src/mdr_find_people/find_people.py     | 3 +--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
index 33d058960..343768acc 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
@@ -10,7 +10,7 @@
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
 from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
-from mdr_place_action.msg import PlaceFeedback, PlaceResult
+from mdr_place_action.msg import PlaceResult
 
 class PlaceSM(ActionSMBase):
     def __init__(self, timeout=120.0,
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
index ac8a14633..8d37b1d80 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/find_people.py
@@ -11,8 +11,7 @@
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase, TorchImageDetector
-from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz
+from mas_perception_libs.utils import cloud_msg_to_cv_image, crop_cloud_to_xyz
 from mas_perception_libs.visualization import draw_labeled_boxes
 
 import dataset_interface.object_detection.transforms as T

From 1466b329146b937df36cb6e0da9de82793940548 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Thu, 31 Dec 2020 12:17:15 +0100
Subject: [PATCH 256/264] [Codacy] Add configuration files to disable some
 tests in pylint and bandit

---
 .bandit   | 2 ++
 .pylintrc | 3 +++
 2 files changed, 5 insertions(+)
 create mode 100644 .bandit
 create mode 100644 .pylintrc

diff --git a/.bandit b/.bandit
new file mode 100644
index 000000000..86d07e6bb
--- /dev/null
+++ b/.bandit
@@ -0,0 +1,2 @@
+[bandit]
+skips: B403,B301,B311
diff --git a/.pylintrc b/.pylintrc
new file mode 100644
index 000000000..7c3c52d53
--- /dev/null
+++ b/.pylintrc
@@ -0,0 +1,3 @@
+disable=
+    no-self-use,
+    bare-except

From 12a312d1775d5090f2eaa0763d7dd3e3a8702682 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 31 Dec 2020 12:27:21 +0100
Subject: [PATCH 257/264] Resolved further Codacy issues (also older ones)

---
 .../src/mdr_move_arm_action/action_states.py  | 10 +++--
 .../src/mdr_pickup_action/action_states.py    | 19 ++++++----
 .../action_states.py                          |  5 ++-
 .../src/mdr_move_base_action/action_states.py |  9 ++---
 .../src/mdr_detect_person/action_states.py    |  8 ++--
 .../mdr_gender_recognition/action_states.py   |  5 ++-
 .../action_states.py                          | 10 ++---
 .../scenario_states/enter.py                  |  1 -
 .../scenario_states/move_base.py              |  1 -
 .../action_states.py                          |  9 +++--
 .../describe_crowd/action_states.py           |  1 -
 .../speech_recognition/action_states.py       |  4 --
 .../common/src/weather_api/weather_api.py     |  1 -
 .../src/speech_matching/speech_matching.py    | 37 ++++++++-----------
 14 files changed, 55 insertions(+), 65 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
index 50acb86f5..cfe102fcd 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py
@@ -33,8 +33,9 @@ def init(self):
             rospy.loginfo('[move_arm] Initialising group %s', self.arm_name)
             self.arm = moveit_commander.MoveGroupCommander(self.arm_name)
             rospy.loginfo('[move_arm] Group %s initialised', self.arm_name)
-        except:
-            rospy.logerr('[move_arm] %s could not be initialised', self.arm_name)
+        except Exception as exc:
+            rospy.logerr('[move_arm] %s could not be initialised: %s',
+                         self.arm_name, str(exc))
             return FTSMTransitions.INIT_FAILED
 
         if self.whole_body_name:
@@ -45,8 +46,9 @@ def init(self):
                 self.whole_body.set_planning_time(5)
                 self.end_effector = self.whole_body.get_end_effector_link()
                 rospy.loginfo('[move_arm] Group %s initialised', self.whole_body_name)
-            except:
-                rospy.logerr('[move_arm] %s could not be initialised', self.whole_body_name)
+            except Exception as exc:
+                rospy.logerr('[move_arm] %s could not be initialised: %s',
+                             self.whole_body_name, str(exc))
                 return FTSMTransitions.INIT_FAILED
         else:
             rospy.loginfo('[move_arm] whole_body_name not specified; not initialising whole body group')
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
index 4faf61985..699343bac 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
@@ -28,7 +28,7 @@ def __init__(self, timeout=120.0,
                  move_forward_server='move_forward_server',
                  base_elbow_offset=-1.,
                  arm_base_offset=-1.,
-                 grasping_orientation=list(),
+                 grasping_orientation=None,
                  grasping_dmp='',
                  dmp_tau=1.,
                  number_of_retries=0,
@@ -68,22 +68,25 @@ def init(self):
             self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction)
             rospy.loginfo('[pickup] Waiting for %s server', self.move_arm_server)
             self.move_arm_client.wait_for_server()
-        except:
-            rospy.logerr('[pickup] %s server does not seem to respond', self.move_arm_server)
+        except Exception as exc:
+            rospy.logerr('[pickup] %s server does not seem to respond: %s',
+                         self.move_arm_server, str(exc))
 
         try:
             self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction)
             rospy.loginfo('[pickup] Waiting for %s server', self.move_base_server)
             self.move_base_client.wait_for_server()
-        except:
-            rospy.logerr('[pickup] %s server does not seem to respond', self.move_base_server)
+        except Exception as exc:
+            rospy.logerr('[pickup] %s server does not seem to respond: %s',
+                         self.move_base_server, str(exc))
 
         try:
             self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction)
             rospy.loginfo('[pickup] Waiting for %s server', self.move_forward_server)
             self.move_forward_client.wait_for_server()
-        except:
-            rospy.logerr('[pickup] %s server does not seem to respond', self.move_forward_server)
+        except Exception as exc:
+            rospy.logerr('[pickup] %s server does not seem to respond: %s',
+                         self.move_forward_server, str(exc))
 
         return FTSMTransitions.INITIALISED
 
@@ -99,7 +102,7 @@ def running(self):
             # y position of the goal pose to the elbow offset
             pose_base_link.pose.position.y = self.base_elbow_offset
 
-        if self.grasping_orientation:
+        if self.grasping_orientation is not None:
             pose_base_link.pose.orientation.x = self.grasping_orientation[0]
             pose_base_link.pose.orientation.y = self.grasping_orientation[1]
             pose_base_link.pose.orientation.z = self.grasping_orientation[2]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/src/mdr_push_pull_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/src/mdr_push_pull_object_action/action_states.py
index b6abbb237..7da4f0e09 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/src/mdr_push_pull_object_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_push_pull_object_action/ros/src/mdr_push_pull_object_action/action_states.py
@@ -46,8 +46,9 @@ def init(self):
             self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction)
             rospy.loginfo('[push_pull] Waiting for %s server', self.move_arm_server)
             self.move_arm_client.wait_for_server()
-        except:
-            rospy.logerr('[push_pull] %s server does not seem to respond', self.move_arm_server)
+        except Exception as exc:
+            rospy.logerr('[push_pull] %s server does not seem to respond %s',
+                         self.move_arm_server, str(exc))
 
         rospy.loginfo('[push_pull] Creating a %s publisher', self.cmd_vel_topic)
         self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
index e42fbe105..6d0a60beb 100644
--- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py
@@ -1,4 +1,3 @@
-import yaml
 import numpy as np
 
 import rospy
@@ -8,6 +7,8 @@
 import move_base_msgs.msg as move_base_msgs
 from actionlib_msgs.msg import GoalStatus
 
+from mas_tools.file_utils import load_yaml_file
+
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
 from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal
@@ -127,14 +128,12 @@ def recovering(self):
         return FTSMTransitions.DONE_RECOVERING
 
     def convert_pose_name_to_coordinates(self, pose_name):
-        stream = open(self.pose_description_file, 'r')
-        poses = yaml.load(stream)
-        stream.close()
+        poses = load_yaml_file(self.pose_description_file)
         try:
             coordinates = poses[pose_name]
             return coordinates
         except:
-            rospy.logerr('Pose name "%s" does not exist' % (pose_name))
+            rospy.logerr('Pose name "%s" does not exist', pose_name)
             return None
 
     def set_result(self, success):
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py
index e12541931..12c42b5dc 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py
@@ -8,7 +8,7 @@
 
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
-from mdr_detect_person.msg import DetectPersonFeedback, DetectPersonResult
+from mdr_detect_person.msg import DetectPersonResult
 from mdr_perception_msgs.msg import FaceBoundingBox
 from mdr_detect_person.inference import load_detection_model, detect_faces
 
@@ -28,8 +28,9 @@ def init(self):
         try:
             rospy.loginfo('[detect_person] Loading detection model %s', self.detection_model_path)
             self.face_detection = load_detection_model(detection_model_path)
-        except:
-            rospy.logerr('[detect_person] Model %s could not be loaded', self.detection_model_path)
+        except Exception as exc:
+            rospy.logerr('[detect_person] Model %s could not be loaded: %s',
+                         self.detection_model_path, str(exc))
         return FTSMTransitions.INITIALISED
 
     def running(self):
@@ -49,7 +50,6 @@ def running(self):
                 bounding_box.bounding_box_coordinates = face_coordinates.tolist()
                 bounding_boxes.append(bounding_box)
 
-                x, y, w, h = face_coordinates
             output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8')
             self.image_publisher.publish(output_ros_image)
         except:
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py
index de6e8b4cc..55ebb6b43 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py
@@ -33,8 +33,9 @@ def init(self):
             # the following two lines are necessary for avoiding https://github.com/keras-team/keras/issues/2397
             self.gender_model._make_predict_function()
             self.computation_graph = tf.get_default_graph()
-        except:
-            rospy.logerr('[recognize_gender] Failed to load model %s', self.gender_model_path)
+        except Exception as exc:
+            rospy.logerr('[recognize_gender] Failed to load model %s: %s',
+                         self.gender_model_path, str(exc))
         return FTSMTransitions.INITIALISED
 
     def running(self):
diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py
index 4bf97e825..7286fea66 100644
--- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py
@@ -6,7 +6,7 @@
 import cv2
 import tensorflow as tf
 from keras.models import load_model
-from cv_bridge import CvBridge, CvBridgeError
+from cv_bridge import CvBridge
 
 from pyftsm.ftsm import FTSMTransitions
 from mas_execution.action_sm_base import ActionSMBase
@@ -38,12 +38,12 @@ def init(self):
             # https://github.com/keras-team/keras/issues/2397
             self.emotion_model._make_predict_function()
             self.computation_graph = tf.get_default_graph()
-        except:
-            rospy.logerr('[recognize_emotion] Model %s could not be loaded', self.emotion_model_path)
+        except Exception as exc:
+            rospy.logerr('[recognize_emotion] Model %s could not be loaded: %s',
+                         self.emotion_model_path, str(exc))
         return FTSMTransitions.INITIALISED
 
     def running(self):
-        number_of_faces = self.goal.number_of_faces
         bounding_boxes = self.goal.bounding_boxes
         emotions = []
 
@@ -60,8 +60,6 @@ def running(self):
             predicted_emotion = self.__predict_emotion(face)
             emotions.append(predicted_emotion)
 
-            rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h),
-                                    (0, 0, 255), 2)
             cv2.putText(rgb_image, predicted_emotion, (x, y - 30),
                         cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0),
                         1, cv2.LINE_AA)
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/enter.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/enter.py
index 538911d3b..9a117d9e4 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/enter.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/enter.py
@@ -1,5 +1,4 @@
 import rospy
-from std_msgs.msg import String
 from actionlib import SimpleActionClient
 from mdr_enter_door_action.msg import EnterDoorAction, EnterDoorGoal
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/move_base.py
index 74792b568..bcabacc19 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/move_base.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/src/mdr_robot_inspection/scenario_states/move_base.py
@@ -1,6 +1,5 @@
 import rospy
 import actionlib
-from std_msgs.msg import String
 
 from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal
 from mas_execution_manager.scenario_state_base import ScenarioStateBase
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/action_states.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/action_states.py
index 3af619716..8c56f281d 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/action_states.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/action_states.py
@@ -1,6 +1,5 @@
 import rospy
 import smach
-import smach_ros
 from std_msgs.msg import String
 
 
@@ -60,10 +59,12 @@ class DescribeResults(smach.State):
     def __init__(self, topic, crowd_size, men, women):
         smach.State.__init__(self, outcomes=['succeeded'])
         self.pub = rospy.Publisher(topic, String, queue_size=10)
+        self.crowd_size = crowd_size
+        self.men = men
+        self.women = women
 
     def execute(self, userdata):
-        rospy.loginfo("Waiting for %i seconds" % self.timeout)
         msg = String("The crowd of %i people is composed of %i men and %i women"
-                     % (crowd_size, men, women))
-        self.pub.publish()
+                     % (self.crowd_size, self.men, self.women))
+        self.pub.publish(msg)
         return 'succeeded'
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/describe_crowd/action_states.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/describe_crowd/action_states.py
index b2637f0e2..f3d6c17e5 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/describe_crowd/action_states.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/describe_crowd/action_states.py
@@ -1,6 +1,5 @@
 import rospy
 import smach
-import smach_ros
 from std_msgs.msg import String
 from mdr_detect_person.msg import DetectPersonAction, DetectPersonGoal
 
diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/speech_recognition/action_states.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/speech_recognition/action_states.py
index 65f2315fe..85a1f8f97 100644
--- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/speech_recognition/action_states.py
+++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/src/mdr_speech_person_recognition/speech_recognition/action_states.py
@@ -1,10 +1,6 @@
 import rospy
 import smach
-import smach_ros
 from std_msgs.msg import String
-from actionlib import SimpleActionClient
-
-from mdr_answer_action.msg import AnswerGoal, AnswerAction
 
 
 def say(publisher, message):
diff --git a/mdr_speech/mdr_question_answering/common/src/weather_api/weather_api.py b/mdr_speech/mdr_question_answering/common/src/weather_api/weather_api.py
index b7836ad07..0edee4453 100644
--- a/mdr_speech/mdr_question_answering/common/src/weather_api/weather_api.py
+++ b/mdr_speech/mdr_question_answering/common/src/weather_api/weather_api.py
@@ -97,7 +97,6 @@ def condition_to_phrase(condition, temperature):
             cond_adj = cond_data['conditions']['adjectives']
             cond_subs = cond_data['conditions']['substantives']
             cond_subs_sg = cond_data['conditions']['substantives_sg']
-            sond_subs_pl = cond_data['conditions']['substantives_pl']
 
         # format condition: remove parentheses and whitespace, make lowercase
         condition = re.sub(r'\([^)]*\)', '', condition).strip().lower()
diff --git a/mdr_speech/mdr_speech_matching/common/src/speech_matching/speech_matching.py b/mdr_speech/mdr_speech_matching/common/src/speech_matching/speech_matching.py
index 12013cd5f..2553a3842 100644
--- a/mdr_speech/mdr_speech_matching/common/src/speech_matching/speech_matching.py
+++ b/mdr_speech/mdr_speech_matching/common/src/speech_matching/speech_matching.py
@@ -11,42 +11,36 @@
 """
 
 class SpeechMatching(object):
-
     def __init__(self, threshold=90):
         self.threshold = threshold
-
         self.gt_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../', 'config/'))
-
         self.available_files = []
         for filename in os.listdir(self.gt_dir):
             if filename.endswith(".txt"):
                 self.available_files.append(filename)
 
-    """
-    The following method creates a sentence pool of a text file containing a
-    list of words.
-    """
     @staticmethod
     def load_pool(filename):
+        """The following method creates a sentence pool of a text file containing a
+        list of words.
+        """
         file = open(filename, "r")
         sentences = []
         for line in file:
             sentences.append(line.strip())
         return sentences
 
-    """
-    The following method changes the initially set threshold.
-    """
     def set_threshold(self, new_threhold_value):
+        """The following method changes the initially set threshold.
+        """
         self.threshold = new_threhold_value
 
-    """
-    The following method returns a boolean value.
-    If there is at least one sentence in one of the two sentence pools with a
-    Levenshtein distance equal or greater than the threshold then this method
-    returns True otherwise False.
-    """
     def find_match(self, input_sentence):
+        """The following method returns a boolean value.
+        If there is at least one sentence in one of the two sentence pools with a
+        Levenshtein distance equal or greater than the threshold then this method
+        returns True otherwise False.
+        """
         for filename in self.available_files:
             file_dir = os.path.join(self.gt_dir, filename)
             sentence_pool = SpeechMatching.load_pool(file_dir)
@@ -55,13 +49,12 @@ def find_match(self, input_sentence):
                 return True
         return False
 
-    """
-    The following method returns a matching sentence, its Levenshtein distance
-    and the filename in which the match can be found. In case the
-    match is below the threshold, the return sentence is empty, the distance is
-    zero and the category is 'nothing'.
-    """
     def match_sentence(self, input_sentence):
+        """The following method returns a matching sentence, its Levenshtein distance
+        and the filename in which the match can be found. In case the
+        match is below the threshold, the return sentence is empty, the distance is
+        zero and the category is 'nothing'.
+        """
         best_match = []
         best_match_filename = ""
         for filename in self.available_files:

From ab9dc65aadb7d0041dac97700b39ad3efcd5bb87 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Thu, 31 Dec 2020 12:40:56 +0100
Subject: [PATCH 258/264] [Codacy] add comments to the bandit config file

---
 .bandit | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/.bandit b/.bandit
index 86d07e6bb..b9e67d08c 100644
--- a/.bandit
+++ b/.bandit
@@ -1,2 +1,5 @@
 [bandit]
-skips: B403,B301,B311
+skips:
+    B403, # `import_pickle` (Consider possible security implications associated with these modules.)
+    B301, # `pickle` (Pickle and modules that wrap it can be unsafe when used to deserialize untrusted data, possible security issue.)
+    B311  # `random` (Standard pseudo-random generators are not suitable for security/cryptographic purposes.)

From 97f3043d33dbdaa4a965de9aa4fadee0823be3ad Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 31 Dec 2020 13:04:06 +0100
Subject: [PATCH 259/264] [README] Added a link to mas_tutorials

---
 README.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/README.md b/README.md
index 73a6b446f..65a5808c3 100644
--- a/README.md
+++ b/README.md
@@ -55,6 +55,8 @@ All packages in our domestic code base start with the `mdr_` suffix; this stands
 
 ## Getting started
 
+### Development setup
+
 b-it-bots members can use [these instructions](https://github.com/b-it-bots/dev-env#setup) to setup a complete development environment for all our robots.
 
 For external users, the following instructions should get you a working system:
@@ -85,6 +87,10 @@ For external users, the following instructions should get you a working system:
 
 If you encounter any problems, please check the list of [issues](https://github.com/b-it-bots/mas_domestic_robotics/issues) and open a new one if you don't see a discussion of the problem there.
 
+### Tutorials
+
+A selection of tutorials that explain various architectural decisions and describe some of the commonly used components can be found in the [mas_tutorials](https://github.com/b-it-bots/mas_tutorials) repository.
+
 ## Docker images
 
 If you want to test locally without installing our software and its dependencies, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/).

From 3ed96cf87107e029f77b65592eca5bf3c06ee896 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Thu, 31 Dec 2020 13:26:53 +0100
Subject: [PATCH 260/264] [.bandit] Passed warnings as a list and removed
 [bandit] tag

---
 .bandit | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/.bandit b/.bandit
index b9e67d08c..5bff613c5 100644
--- a/.bandit
+++ b/.bandit
@@ -1,5 +1 @@
-[bandit]
-skips:
-    B403, # `import_pickle` (Consider possible security implications associated with these modules.)
-    B301, # `pickle` (Pickle and modules that wrap it can be unsafe when used to deserialize untrusted data, possible security issue.)
-    B311  # `random` (Standard pseudo-random generators are not suitable for security/cryptographic purposes.)
+skips: ['B403', 'B301', 'B311']

From 405b3b967f807cb02a73ed1f4154cf07e177ebe5 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Thu, 31 Dec 2020 13:40:13 +0100
Subject: [PATCH 261/264] [.bandit] Add comments about the skipped tests

---
 .bandit | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/.bandit b/.bandit
index 5bff613c5..fea9fb17a 100644
--- a/.bandit
+++ b/.bandit
@@ -1 +1,4 @@
-skips: ['B403', 'B301', 'B311']
+skips: 
+    - 'B403' # `import_pickle` (Consider possible security implications associated with these modules.)
+    - 'B301' # `pickle` (Pickle and modules that wrap it can be unsafe when used to deserialize untrusted data, possible security issue.)
+    - 'B311' # `random` (Standard pseudo-random generators are not suitable for security/cryptographic purposes.)

From 787b31c9bee9480389f77318e133a3f9c6cbbe2a Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Wed, 6 Jan 2021 09:49:06 +0100
Subject: [PATCH 262/264] Fix review findings

---
 .../mdr_cloud_object_detection/CMakeLists.txt |   3 +-
 .../mdr_cloud_object_detection/package.xml    |   1 +
 .../ros/config/ObjectDetection.cfg            |  14 +--
 .../object_detection_default_configs.yaml     |  14 +--
 .../mdr_cloud_object_detection/Aliases.h      |  18 ---
 .../CloudObjectDetection.h                    |   4 +-
 .../mdr_cloud_object_detection/Color.h        | 108 ------------------
 .../CropBoxVoxelFilter.h                      |  40 +++++++
 ...terParams.h => CropBoxVoxelFilterParams.h} |  22 ++--
 .../PassThroughVoxelFilter.h                  |  42 -------
 .../mdr_cloud_object_detection/Utils.h        |   2 +-
 .../ros/src/CloudObjectDetection.cpp          |  24 ++--
 .../ros/src/CropBoxVoxelFilter.cpp            |  42 +++++++
 .../ros/src/PassThroughVoxelFilter.cpp        |  46 --------
 .../scenario_states/get_user_data.py          |   2 +-
 15 files changed, 126 insertions(+), 256 deletions(-)
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilter.h
 rename mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/{PassThroughVoxelFilterParams.h => CropBoxVoxelFilterParams.h} (50%)
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
 create mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/CropBoxVoxelFilter.cpp
 delete mode 100644 mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp

diff --git a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
index fdc3cd256..2ae208f72 100644
--- a/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
+++ b/mdr_perception/mdr_cloud_object_detection/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   mas_navigation_tools
   mas_perception_msgs
+  mas_perception_libs
 )
 
 find_package(PCL 1.7 REQUIRED)
@@ -44,7 +45,7 @@ include_directories(
 ###################################
 # Node for object detection
 add_executable(cloud_object_detection
-  ros/src/PassThroughVoxelFilter.cpp
+  ros/src/CropBoxVoxelFilter.cpp
   ros/src/CloudObjectDetection.cpp
   ros/src/main.cpp
 )
diff --git a/mdr_perception/mdr_cloud_object_detection/package.xml b/mdr_perception/mdr_cloud_object_detection/package.xml
index b776a5443..00152d584 100644
--- a/mdr_perception/mdr_cloud_object_detection/package.xml
+++ b/mdr_perception/mdr_cloud_object_detection/package.xml
@@ -19,6 +19,7 @@
   <depend>roscpp</depend>
   <depend>mas_navigation_tools</depend>
   <depend>mas_perception_msgs</depend>
+  <depend>mas_perception_libs</depend>
 
   <build_export_depend>message_runtime</build_export_depend>
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg b/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
index e871cda57..f64134041 100755
--- a/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
+++ b/mdr_perception/mdr_cloud_object_detection/ros/config/ObjectDetection.cfg
@@ -10,23 +10,23 @@ gen = ParameterGenerator()
 # cloud filter group
 cloud_filter_params = gen.add_group("Cloud Filter")
 
-# pass-through filter parameters
-cloud_filter_params.add("passthrough_limit_min_x", double_t, 0,
+# crop-box filter parameters
+cloud_filter_params.add("cropbox_limit_min_x", double_t, 0,
                         "Minimum field value of a point along x-axis for it to be considered", 0.0, 0.0, 10.0)
 
-cloud_filter_params.add("passthrough_limit_max_x", double_t, 0,
+cloud_filter_params.add("cropbox_limit_max_x", double_t, 0,
                         "Maximum field value of a point along x-axis for it to be considered", 3.0, 0.0, 10.0)
 
-cloud_filter_params.add("passthrough_limit_min_y", double_t, 0,
+cloud_filter_params.add("cropbox_limit_min_y", double_t, 0,
                         "Minimum field value of a point along y-axis for it to be considered", -1.5, -3.0, 3.0)
 
-cloud_filter_params.add("passthrough_limit_max_y", double_t, 0,
+cloud_filter_params.add("cropbox_limit_max_y", double_t, 0,
                         "Maximum field value of a point along y-axis for it to be considered", 1.5, -3.0, 3.0)
 
-cloud_filter_params.add("passthrough_limit_min_z", double_t, 0,
+cloud_filter_params.add("cropbox_limit_min_z", double_t, 0,
                         "Minimum field value of a point along z-axis for it to be considered", 0.025, 0.0, 5.0)
 
-cloud_filter_params.add("passthrough_limit_max_z", double_t, 0,
+cloud_filter_params.add("cropbox_limit_max_z", double_t, 0,
                         "Maximum field value of a point along z-axis for it to be considered", 0.2, 0.0, 5.0)
 
 # voxel-grid filter parameters; note that we limit the cloud along the z-axis using the voxel filter to avoid adding
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
index 0a497bcf9..24e1e31ad 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
+++ b/mdr_perception/mdr_cloud_object_detection/ros/config/object_detection_default_configs.yaml
@@ -1,12 +1,12 @@
 # config file loaded during launch as default configurations for the cloud object detector
 
-# passthrough and voxel-grid filter params in base_link frame
-passthrough_limit_min_x:  0.0
-passthrough_limit_max_x:  1.5
-passthrough_limit_min_y: -0.5
-passthrough_limit_max_y:  0.5
-passthrough_limit_min_z:  0.025
-passthrough_limit_max_z:  0.2
+# cropbox and voxel-grid filter params in base_link frame
+cropbox_limit_min_x:  0.0
+cropbox_limit_max_x:  1.5
+cropbox_limit_min_y: -0.5
+cropbox_limit_max_y:  0.5
+cropbox_limit_min_z:  0.025
+cropbox_limit_max_z:  0.2
 voxel_limit_min_z:        0.0
 voxel_limit_max_z:        1.8
 voxel_leaf_size:          0.02
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h
deleted file mode 100644
index b90bd297f..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Aliases.h
+++ /dev/null
@@ -1,18 +0,0 @@
-/*
- * Copyright 2020 Bonn-Rhein-Sieg University
- *
- * Author: Minh Nguyen
- * Author: Sushant Vijay Chavan
- *
- */
-#ifndef MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
-#define MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
-
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-
-typedef pcl::PointXYZRGB PointT;
-typedef pcl::PointCloud<PointT> PointCloud;
-typedef pcl::PointCloud<pcl::PointNormal> PointCloudNormal;
-
-#endif  // MDR_CLOUD_OBJECT_DETECTION_ALIASES_H
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
index 67fd68ef0..180e5745f 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CloudObjectDetection.h
@@ -20,7 +20,7 @@
 #include <mas_perception_msgs/ObjectList.h>
 
 #include <mdr_cloud_object_detection/Utils.h>
-#include <mdr_cloud_object_detection/PassThroughVoxelFilter.h>
+#include <mdr_cloud_object_detection/CropBoxVoxelFilter.h>
 #include <mdr_cloud_object_detection/ObjectDetectionConfig.h>
 
 namespace mdr_cloud_object_detection
@@ -40,7 +40,7 @@ namespace mdr_cloud_object_detection
         tf::TransformListener mTfListener;
         std::string mTransformTargetFrame;
         std::string mClusterTargetFrame;
-        PassThroughVoxelFilter mCloudFilter;
+        CropBoxVoxelFilter mCloudFilter;
         EuclideanClusterParams mClusterParams;
         ObjectFilterParams mObjectFilterParams;
         ObjectCacheParams mObjectCacheParams;
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
deleted file mode 100644
index ec856fbde..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Color.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*!
- * @copyright 2020 Bonn-Rhein-Sieg University
- *
- * @author Sergey Alexandrov
- * @author Sushant Vijay Chavan
- *
- */
-#ifndef MDR_CLOUD_OBJECT_DETECTION_COLOR_H
-#define MDR_CLOUD_OBJECT_DETECTION_COLOR_H
-
-#include <std_msgs/ColorRGBA.h>
-#include <mdr_cloud_object_detection/Aliases.h>
-
-namespace mdr_cloud_object_detection
-{
-
-struct Color
-{
-    uint8_t mRed;
-    uint8_t mGreen;
-    uint8_t mBlue;
-
-    Color(float pRed, float pGreen, float pBlue)
-    : mRed(static_cast<uint8_t>(pRed * 255)), mGreen(static_cast<uint8_t>(pGreen * 255)),
-      mBlue(static_cast<uint8_t>(pBlue * 255))
-    { }
-
-    Color(int pRed, int pGreen, int pBlue)
-    : mRed(static_cast<uint8_t>(pRed)), mGreen(static_cast<uint8_t>(pGreen)), mBlue(static_cast<uint8_t>(pBlue)) { }
-
-    enum Name
-    {
-        SALMON,
-        TEAL,
-        DEEP_PINK,
-        SANGRIA,
-        SEA_BLUE,
-        SEA_GREEN,
-        SCARLET,
-        PUMPKIN,
-        JASMINE,
-        IVORY,
-        GAINSBORO,
-    };
-
-    explicit Color(Name name)
-    {
-        switch (name)
-        {
-            case SALMON:
-                mRed = 0xFA, mGreen = 0x80, mBlue = 0x72;
-                break;
-            case TEAL:
-                mRed = 0x00, mGreen = 0x80, mBlue = 0x80;
-                break;
-            case DEEP_PINK:
-                mRed = 0xFF, mGreen = 0x14, mBlue = 0x93;
-                break;
-            case SANGRIA:
-                mRed = 0x92, mGreen = 0x00, mBlue = 0x0A;
-                break;
-            case SEA_BLUE:
-                mRed = 0x00, mGreen = 0x69, mBlue = 0x94;
-                break;
-            case SEA_GREEN:
-                mRed = 0x2E, mGreen = 0x8B, mBlue = 0x57;
-                break;
-            case SCARLET:
-                mRed = 0xFF, mGreen = 0x24, mBlue = 0x00;
-                break;
-            case PUMPKIN:
-                mRed = 0xFF, mGreen = 0x75, mBlue = 0x18;
-                break;
-            case JASMINE:
-                mRed = 0xF8, mGreen = 0xDE, mBlue = 0x7E;
-                break;
-            case IVORY:
-                mRed = 0xFF, mGreen = 0xFF, mBlue = 0xF0;
-                break;
-            case GAINSBORO:
-                mRed = 0xDC, mGreen = 0xDC, mBlue = 0xDC;
-                break;
-            default:
-                mRed = 0xFF, mGreen = 0xFF, mBlue = 0xFF;
-                break;
-        }
-    }
-
-    explicit operator float() const
-    {
-        PointT point(mRed, mGreen, mBlue);
-        return point.rgb;
-    }
-
-    explicit operator std_msgs::ColorRGBA() const
-    {
-        std_msgs::ColorRGBA color;
-        color.r = mRed / 255.0f;
-        color.g = mGreen / 255.0f;
-        color.b = mBlue / 255.0f;
-        color.a = 1.0f;
-        return color;
-    }
-};
-
-}   // namespace mdr_cloud_object_detection
-
-#endif  // MDR_CLOUD_OBJECT_DETECTION_COLOR_H
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilter.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilter.h
new file mode 100644
index 000000000..7da758195
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilter.h
@@ -0,0 +1,40 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
+#ifndef MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER
+#define MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER
+
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/voxel_grid.h>
+
+#include <mas_perception_libs/aliases.h>
+#include <mdr_cloud_object_detection/CropBoxVoxelFilterParams.h>
+
+namespace mdr_cloud_object_detection
+{
+    /*!
+    * @brief class containing definition for filtering point clouds
+    */
+    class CropBoxVoxelFilter
+    {
+    public:
+        CropBoxVoxelFilter() = default;
+
+        /*! @brief set parameters relevant to filtering cloud */
+        virtual void setParams(const CropBoxVoxelFilterParams& pParams);
+
+        /*!
+        * @brief filter point cloud using crop-box and voxel filters
+        */
+        PointCloud::Ptr filterCloud(const PointCloud::ConstPtr &pCloudPtr);
+
+    private:
+        pcl::CropBox<PointT> mCropboxFilter;
+        pcl::VoxelGrid<PointT> mVoxelGridFilter;
+    };
+}
+
+#endif // MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilterParams.h
similarity index 50%
rename from mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
rename to mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilterParams.h
index e336089ba..6c7bf6fba 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilterParams.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/CropBoxVoxelFilterParams.h
@@ -4,24 +4,24 @@
  * @author Sushant Vijay Chavan
  *
  */
-#ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
-#define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
+#ifndef MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER_PARAMS
+#define MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER_PARAMS
 
 namespace mdr_cloud_object_detection
 {
     /*!
     * @brief struct containing parameters necessary for filtering point clouds
     */
-    struct PassThroughVoxelFilterParams
+    struct CropBoxVoxelFilterParams
     {
-        /* PassThrough filter parameters
+        /* CropBox filter parameters
         * limit the cloud to be filtered points outside of these x, y, z ranges will be discarded */
-        float mPassThroughLimitMinX = 0.0f;
-        float mPassThroughLimitMaxX = 0.0f;
-        float mPassThroughLimitMinY = 0.0f;
-        float mPassThroughLimitMaxY = 0.0f;
-        float mPassThroughLimitMinZ = 0.0f;
-        float mPassThroughLimitMaxZ = 0.0f;
+        float mCropBoxLimitMinX = 0.0f;
+        float mCropBoxLimitMaxX = 0.0f;
+        float mCropBoxLimitMinY = 0.0f;
+        float mCropBoxLimitMaxY = 0.0f;
+        float mCropBoxLimitMinZ = 0.0f;
+        float mCropBoxLimitMaxZ = 0.0f;
         /* VoxelGrid filter parameters for down-sampling the cloud, also limit the cloud along the z axis */
         float mVoxelLimitMinZ = 0.0f;
         float mVoxelLimitMaxZ = 0.0f;
@@ -29,4 +29,4 @@ namespace mdr_cloud_object_detection
     };
 }
 
-#endif // MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER_PARAMS
+#endif // MDR_CLOUD_OBJECT_DETECTION_CROP_BOX_VOXEL_FILTER_PARAMS
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
deleted file mode 100644
index 2d16d1b19..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/PassThroughVoxelFilter.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*!
- * @copyright 2020 Bonn-Rhein-Sieg University
- *
- * @author Sushant Vijay Chavan
- *
- */
-#ifndef MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
-#define MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
-
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/voxel_grid.h>
-
-#include <mdr_cloud_object_detection/Aliases.h>
-#include <mdr_cloud_object_detection/PassThroughVoxelFilterParams.h>
-
-namespace mdr_cloud_object_detection
-{
-    /*!
-    * @brief class containing definition for filtering point clouds
-    */
-    class PassThroughVoxelFilter
-    {
-    public:
-        PassThroughVoxelFilter() = default;
-
-        /*! @brief set parameters relevant to filtering cloud */
-        virtual void setParams(const PassThroughVoxelFilterParams& pParams);
-
-        /*!
-        * @brief filter point cloud using passthrough and voxel filters
-        */
-        PointCloud::Ptr filterCloud(const PointCloud::ConstPtr &pCloudPtr);
-
-    private:
-        pcl::PassThrough<PointT> mPassThroughFilterX;
-        pcl::PassThrough<PointT> mPassThroughFilterY;
-        pcl::PassThrough<PointT> mPassThroughFilterZ;
-        pcl::VoxelGrid<PointT> mVoxelGridFilter;
-    };
-}
-
-#endif // MDR_CLOUD_OBJECT_DETECTION_PASS_THROUGH_VOXEL_FILTER
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
index 807661254..b632c280f 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
+++ b/mdr_perception/mdr_cloud_object_detection/ros/include/mdr_cloud_object_detection/Utils.h
@@ -7,4 +7,4 @@
 #include "mdr_cloud_object_detection/EuclideanClusterParams.h"
 #include "mdr_cloud_object_detection/ObjectCacheParams.h"
 #include "mdr_cloud_object_detection/ObjectFilterParams.h"
-#include "mdr_cloud_object_detection/PassThroughVoxelFilterParams.h"
+#include "mdr_cloud_object_detection/CropBoxVoxelFilterParams.h"
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
index 8e1d069a4..932e553a2 100644
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CloudObjectDetection.cpp
@@ -10,7 +10,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
-#include <mdr_cloud_object_detection/Color.h>
+#include <mas_perception_libs/color.h>
 #include <mdr_cloud_object_detection/CloudObjectDetection.h>
 
 using namespace mdr_cloud_object_detection;
@@ -53,13 +53,13 @@ void CloudObjectDetection::objectDetectionConfigCallback(const ObjectDetectionCo
                                                          uint32_t pLevel)
 {
     // Cloud Filter params
-    PassThroughVoxelFilterParams cloudFilterParams;
-    cloudFilterParams.mPassThroughLimitMinX = static_cast<float>(pConfig.passthrough_limit_min_x);
-    cloudFilterParams.mPassThroughLimitMaxX = static_cast<float>(pConfig.passthrough_limit_max_x);
-    cloudFilterParams.mPassThroughLimitMinY = static_cast<float>(pConfig.passthrough_limit_min_y);
-    cloudFilterParams.mPassThroughLimitMaxY = static_cast<float>(pConfig.passthrough_limit_max_y);
-    cloudFilterParams.mPassThroughLimitMinZ = static_cast<float>(pConfig.passthrough_limit_min_z);
-    cloudFilterParams.mPassThroughLimitMaxZ = static_cast<float>(pConfig.passthrough_limit_max_z);
+    CropBoxVoxelFilterParams cloudFilterParams;
+    cloudFilterParams.mCropBoxLimitMinX = static_cast<float>(pConfig.cropbox_limit_min_x);
+    cloudFilterParams.mCropBoxLimitMaxX = static_cast<float>(pConfig.cropbox_limit_max_x);
+    cloudFilterParams.mCropBoxLimitMinY = static_cast<float>(pConfig.cropbox_limit_min_y);
+    cloudFilterParams.mCropBoxLimitMaxY = static_cast<float>(pConfig.cropbox_limit_max_y);
+    cloudFilterParams.mCropBoxLimitMinZ = static_cast<float>(pConfig.cropbox_limit_min_z);
+    cloudFilterParams.mCropBoxLimitMaxZ = static_cast<float>(pConfig.cropbox_limit_max_z);
     cloudFilterParams.mVoxelLimitMinZ = static_cast<float>(pConfig.voxel_limit_min_z);
     cloudFilterParams.mVoxelLimitMaxZ = static_cast<float>(pConfig.voxel_limit_max_z);
     cloudFilterParams.mVoxelLeafSize = static_cast<float>(pConfig.voxel_leaf_size);
@@ -406,7 +406,7 @@ visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const Eig
     marker.color.a = 2.0;
     marker.ns = "";
     marker.id = id;
-    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.color = std_msgs::ColorRGBA(mas_perception_libs::Color(mas_perception_libs::Color::SCARLET));
     marker.points.push_back(getPointMsg(min[0], min[1], min[2]));
     marker.points.push_back(getPointMsg(min[0], max[1], min[2]));
     marker.points.push_back(getPointMsg(min[0], max[1], min[2]));
@@ -430,7 +430,7 @@ visualization_msgs::Marker CloudObjectDetection::getObjectBoundsMarker(const mas
     marker.scale.y = 0.005;
     marker.ns = "";
     marker.id = id;
-    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.color = std_msgs::ColorRGBA(mas_perception_libs::Color(mas_perception_libs::Color::SCARLET));
 
     marker.points.push_back(objMsg.bounding_box.vertices[0]);
     marker.points.push_back(objMsg.bounding_box.vertices[1]);
@@ -474,7 +474,7 @@ visualization_msgs::Marker CloudObjectDetection::getObjectOrientationMarker(cons
     marker.scale.z = .005;
     marker.ns = "";
     marker.id = id + 2000;
-    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.color = std_msgs::ColorRGBA(mas_perception_libs::Color(mas_perception_libs::Color::SCARLET));
     marker.pose = objMsg.pose.pose;
     return marker;
 }
@@ -494,7 +494,7 @@ visualization_msgs::Marker CloudObjectDetection::getObjectIdMarker(const Eigen::
     marker.pose.position.z = center[2];
     marker.scale.z = 0.05;
     marker.id = id + 1000;
-    marker.color = std_msgs::ColorRGBA(Color(Color::SCARLET));
+    marker.color = std_msgs::ColorRGBA(mas_perception_libs::Color(mas_perception_libs::Color::SCARLET));
     return marker;
 }
 
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/CropBoxVoxelFilter.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/CropBoxVoxelFilter.cpp
new file mode 100644
index 000000000..a8de23536
--- /dev/null
+++ b/mdr_perception/mdr_cloud_object_detection/ros/src/CropBoxVoxelFilter.cpp
@@ -0,0 +1,42 @@
+/*!
+ * @copyright 2020 Bonn-Rhein-Sieg University
+ *
+ * @author Sushant Vijay Chavan
+ *
+ */
+#include <mdr_cloud_object_detection/CropBoxVoxelFilter.h>
+
+using namespace mdr_cloud_object_detection;
+
+void CropBoxVoxelFilter::setParams(const CropBoxVoxelFilterParams& pParams)
+{
+    /* crop-box params */
+    mCropboxFilter.setMin(Eigen::Vector4f(pParams.mCropBoxLimitMinX,
+                                          pParams.mCropBoxLimitMinY,
+                                          pParams.mCropBoxLimitMinZ,
+                                          1.0));
+    mCropboxFilter.setMax(Eigen::Vector4f(pParams.mCropBoxLimitMaxX,
+                                          pParams.mCropBoxLimitMaxY,
+                                          pParams.mCropBoxLimitMaxZ,
+                                          1.0));
+
+    /* filter z-axis using voxel filter instead of making another member */
+    mVoxelGridFilter.setFilterFieldName("z");
+    mVoxelGridFilter.setFilterLimits(pParams.mVoxelLimitMinZ, pParams.mVoxelLimitMaxZ);
+
+    /* voxel-grid params */
+    mVoxelGridFilter.setLeafSize(pParams.mVoxelLeafSize, pParams.mVoxelLeafSize, pParams.mVoxelLeafSize);
+}
+
+PointCloud::Ptr CropBoxVoxelFilter::filterCloud(const PointCloud::ConstPtr &pCloudPtr)
+{
+    PointCloud::Ptr filteredCloudPtr = boost::make_shared<PointCloud>(); 
+
+    mCropboxFilter.setInputCloud(pCloudPtr);
+    mCropboxFilter.filter(*filteredCloudPtr);
+
+    mVoxelGridFilter.setInputCloud(filteredCloudPtr);
+    mVoxelGridFilter.filter(*filteredCloudPtr);
+
+    return filteredCloudPtr;
+}
diff --git a/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp b/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
deleted file mode 100644
index 3255e8b6c..000000000
--- a/mdr_perception/mdr_cloud_object_detection/ros/src/PassThroughVoxelFilter.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*!
- * @copyright 2020 Bonn-Rhein-Sieg University
- *
- * @author Sushant Vijay Chavan
- *
- */
-#include <mdr_cloud_object_detection/PassThroughVoxelFilter.h>
-
-using namespace mdr_cloud_object_detection;
-
-void PassThroughVoxelFilter::setParams(const PassThroughVoxelFilterParams& pParams)
-{
-    /* pass-through params */
-    mPassThroughFilterX.setFilterFieldName("x");
-    mPassThroughFilterX.setFilterLimits(pParams.mPassThroughLimitMinX, pParams.mPassThroughLimitMaxX);
-    mPassThroughFilterY.setFilterFieldName("y");
-    mPassThroughFilterY.setFilterLimits(pParams.mPassThroughLimitMinY, pParams.mPassThroughLimitMaxY);
-    mPassThroughFilterZ.setFilterFieldName("z");
-    mPassThroughFilterZ.setFilterLimits(pParams.mPassThroughLimitMinZ, pParams.mPassThroughLimitMaxZ);
-
-    /* filter z-axis using voxel filter instead of making another member */
-    mVoxelGridFilter.setFilterFieldName("z");
-    mVoxelGridFilter.setFilterLimits(pParams.mVoxelLimitMinZ, pParams.mVoxelLimitMaxZ);
-
-    /* voxel-grid params */
-    mVoxelGridFilter.setLeafSize(pParams.mVoxelLeafSize, pParams.mVoxelLeafSize, pParams.mVoxelLeafSize);
-}
-
-PointCloud::Ptr PassThroughVoxelFilter::filterCloud(const PointCloud::ConstPtr &pCloudPtr)
-{
-    PointCloud::Ptr filteredCloudPtr = boost::make_shared<PointCloud>();
-
-    mPassThroughFilterX.setInputCloud(pCloudPtr);
-    mPassThroughFilterX.filter(*filteredCloudPtr);
-
-    mPassThroughFilterY.setInputCloud(filteredCloudPtr);
-    mPassThroughFilterY.filter(*filteredCloudPtr);
-
-    mPassThroughFilterZ.setInputCloud(filteredCloudPtr);
-    mPassThroughFilterZ.filter(*filteredCloudPtr);
-
-    mVoxelGridFilter.setInputCloud(filteredCloudPtr);
-    mVoxelGridFilter.filter(*filteredCloudPtr);
-
-    return filteredCloudPtr;
-}
diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
index ce5ab75d0..882f7b25e 100644
--- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
+++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/get_user_data.py
@@ -72,7 +72,7 @@ def execute(self, userdata):
         rospy.logerr("[get_user_data] Query timed out!")
 
         if self.retry_count == self.number_of_retries:
-            self.say("Sorry, I could get our name. I shall proceed without it.")
+            self.say("Sorry, I could not get your name. I shall proceed without it.")
             self.retry_count = 0
             return "failed_after_retrying"
 

From d9ef475b7bc32b0258ad06ba347a2d97162ef2a6 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 6 Jan 2021 10:27:48 +0100
Subject: [PATCH 263/264] [move_arm_action/dmp] Fixed the online execution

The existing implementation was unrolling the DMP (i.e. calling the
dmp.step function) at every step of the control loop, without checking
if the intermediate goal has been reached. For this reason, the robot
was very quickly just starting to move directly towards the goal, thus
not following the trajectory at all.

In the fixed version, a DMP step is only taken if the currently pursued
goal has been reached, which means that the trajectory is actually
followed.
---
 .../ros/src/mdr_move_arm_action/dmp.py        | 34 +++++++++++++++----
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
index 2dfbbaf21..ef84be136 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py
@@ -114,13 +114,28 @@ def follow_path(self, initial_pos_odom,  goal_pose_odom):
                                  0., 0., 0.])
         goal_pose = np.array([goal[0], goal[1], goal[2], 0., 0., 0.])
 
-        current_path = initial_pose[:3]
-
         self.dmp = self.instantiate_dmp(initial_pose, goal_pose)
+        next_pose, _, _ = self.dmp.step(tau=self.tau)
+
+        current_path = initial_pose[:3]
+        next_pos = next_pose[:3]
+        current_path = np.vstack((current_path, next_pos))
 
         self.motion_completed = False
         self.motion_cancelled = False
+
+        # sequence count for the cmd vel header
         cmd_count = 0
+
+        # we want to publish the path only when it changes, which is
+        # why we keep track of the path state during execution
+        path_changed =  True
+
+        # DMP control loop:
+        #     * we unroll the trajectory step-by-step and control the robot towards
+        #       each intermediate goal (using only arm motion whenever possible, but also
+        #       including base motion if the arm joints approach a singularity)
+        #     * the execution is stopped when the end effector reaches the goal position
         while not self.motion_completed and \
               not self.motion_cancelled and \
               not rospy.is_shutdown():
@@ -135,9 +150,14 @@ def follow_path(self, initial_pos_odom,  goal_pose_odom):
                 self.motion_completed = True
                 break
 
-            next_pose, _, _ = self.dmp.step(tau=self.tau)
-            next_pos = next_pose[:3]
-            current_path = np.vstack((current_path, next_pos))
+            # if the end effector has reached the current waypoint,
+            # we unroll the trajectory further
+            distance_to_intermediate_goal = np.linalg.norm((next_pos - current_pos))
+            if distance_to_intermediate_goal <= self.goal_tolerance:
+                next_pose, _, _ = self.dmp.step(tau=self.tau)
+                next_pos = next_pose[:3]
+                current_path = np.vstack((current_path, next_pos))
+                path_changed = True
 
             vel = self.feedback_gain * (next_pos - current_pos)
 
@@ -186,7 +206,9 @@ def follow_path(self, initial_pos_odom,  goal_pose_odom):
             self.vel_publisher_arm.publish(twist_arm)
             cmd_count += 1
 
-            self.publish_path(current_path)
+            if path_changed:
+                self.publish_path(current_path)
+                path_changed = False
 
         # stop arm and base motion after converging
         twist_arm = TwistStamped()

From 2dc380992a8ec99acd956413573557a91fcad376 Mon Sep 17 00:00:00 2001
From: Alex Mitrevski <aleksandar.mitrevski@h-brs.de>
Date: Wed, 6 Jan 2021 16:57:16 +0100
Subject: [PATCH 264/264] [pickup/place_action] Fixed test for empty goal
 orientation

Both None and empty list have to be covered
---
 .../ros/src/mdr_pickup_action/action_states.py                  | 2 +-
 .../mdr_place_action/ros/src/mdr_place_action/action_states.py  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
index 699343bac..f7bbc231e 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py
@@ -102,7 +102,7 @@ def running(self):
             # y position of the goal pose to the elbow offset
             pose_base_link.pose.position.y = self.base_elbow_offset
 
-        if self.grasping_orientation is not None:
+        if self.grasping_orientation:
             pose_base_link.pose.orientation.x = self.grasping_orientation[0]
             pose_base_link.pose.orientation.y = self.grasping_orientation[1]
             pose_base_link.pose.orientation.z = self.grasping_orientation[2]
diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
index 343768acc..4f22c1c2a 100644
--- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
+++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py
@@ -72,7 +72,7 @@ def running(self):
         pose = self.goal.pose
         pose.header.stamp = rospy.Time(0)
         pose_base_link = self.tf_listener.transformPose('base_link', pose)
-        if self.placing_orientation is not None:
+        if self.placing_orientation:
             pose_base_link.pose.orientation.x = self.placing_orientation[0]
             pose_base_link.pose.orientation.y = self.placing_orientation[1]
             pose_base_link.pose.orientation.z = self.placing_orientation[2]