From 88020006d6361116d5809559232f608a37413c49 Mon Sep 17 00:00:00 2001
From: Andrej Orsula <orsula.andrej@gmail.com>
Date: Mon, 2 May 2022 17:27:24 +0200
Subject: [PATCH] 2.1.0 (#11)

* Add support for Cartesian paths (#8)

* Add support for collision objects (#9)

* Remove trimesh from ROS 2 dependencies

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

* Expose cartesian planning as a parameter in the pose goal example

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

* Add default test mesh for collision object example

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

* Move public functions before private

Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>

Co-authored-by: Nils Schulte <47043622+Schnilz@users.noreply.github.com>
---
 CMakeLists.txt                  |   1 +
 README.md                       |   4 +-
 examples/assets/suzanne.stl     | Bin 0 -> 48484 bytes
 examples/ex_collision_object.py | 101 ++++++++++++++++++
 examples/ex_pose_goal.py        |   6 +-
 pymoveit2/moveit2.py            | 182 ++++++++++++++++++++++++++++++--
 6 files changed, 283 insertions(+), 11 deletions(-)
 create mode 100644 examples/assets/suzanne.stl
 create mode 100755 examples/ex_collision_object.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2087af..aba31f2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ ament_python_install_package(pymoveit2)
 # Install examples
 set(EXAMPLES_DIR examples)
 install(PROGRAMS
+    ${EXAMPLES_DIR}/ex_collision_object.py
     ${EXAMPLES_DIR}/ex_gripper_command.py
     ${EXAMPLES_DIR}/ex_gripper.py
     ${EXAMPLES_DIR}/ex_joint_goal.py
diff --git a/README.md b/README.md
index 504c3b2..a14644e 100644
--- a/README.md
+++ b/README.md
@@ -64,8 +64,8 @@ After that, the individual scripts can be run.
 ```bash
 # Move to joint configuration
 ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
-# Move to Cartesian pose
-ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"
+# Move to Cartesian pose (motion in either joint or Cartesian space)
+ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
 # Repeatadly toggle the gripper (or use "open"/"close" actions)
 ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle"
 # Example of using MoveIt 2 Servo to move the end-effector in a circular motion
diff --git a/examples/assets/suzanne.stl b/examples/assets/suzanne.stl
new file mode 100644
index 0000000000000000000000000000000000000000..7c6e9cf1539e42dda42842427062c3eec0eea475
GIT binary patch
literal 48484
zcmbWAcYGDa*T#q5dy$?10@8yQkalPJQKg795s)TG5D<biL1{|wL_kQ8BE3nGB0}yA
zs32fKiUkXbs5EIR3Mk0?oZU?3Oz^MQ&nITNInR90nLe|#dz+3L5;t^s-xs1|hsF(x
zZuC;$!7ub3S~XOyMzt96|M~B51|b@rt?cc;P+Pxzs)fS8!lBTTNWa)a4ML$%_*nAW
zxSrDBSaNGz&x#}_6m1~H%Dxr72cOo_OU6F!D7aO>*w}Dl!gN0+AsU^ntPKNJ5t9}e
zxDFU#M+{qtRedXJkz7Yl1A{BL6&UcyF#az4j2H9GHC29CqT+P!l@PrU$4lVss4qnS
zO&z@0OTVcx_1|_BuGNGvaVSTG07DxFtl)c)7WdHg3Jh%+T;W<xK>CJp>)zLL-KuZZ
zfInJk*Y~_AoB3e2hVOY1#~3P)eL_68^MvdnuBaN%w|q!pt$5^MD8oQnNYkX17kLvf
zcuYc6+j&C007f-1xWcuvIU>ZaO6%m@0&Vn}ckU}*m&kV^R>1cz$Wb0s=0YFJ35%NO
z##J-wv<l`GA~!X0N2PVjFvu%oMc0Pm*0HRGK2)!RA;I7ZZso>w;_jr#k+^m`U$gwA
zV7{M28(D>to6?|P>{`UKn92jc51ro{DO#tCZoN5^qj0Tw<VWvL3L6Hjl5AS%k_>~#
zB*e$(w}y*=@e~-Y!bZ;H6QW49S!%+ox7EJ09euA%Ub$XPKwF=b(mwL`oTi4}xvZia
zb#W9zI99ACmaH~QP6lI-&zO*8(n{;0VGEI`=QL><R^j^2Yp8dxvA#Mjw{Dqz4O*?u
zceEhC41$&_P$pAST5XeB&GgY9vgoMs+Z+XYOlHMn0>d*5SS6XX!0^k47;a2WYc=zT
z5e<f`;HAmqdt~f&RdINCef5Zhg4ZSLd#PR#gU6KZ_nT@>iEjGe3oX+qtQF4@A;6Ht
zc;u+09*+5bD5YK<LZ0zDmgV=Gay1wQFSc|P+{%soQ27U{XO1p<%e=ahd^Wz@m@<cr
zS3*Ri{hZH!U6sk+U1u2aq@&<gZsbSGKajn_nCdfv91YSkz7vDT^i%fhvMd<ahZDmp
zxE0S4)VJPE^%a>@Z+K!Y6c6WUz%#WZggA08ZmbJ6%&qTyv(8c2_CZ#>j)CDB2CP!r
zM~L#rFB@XGUgcQa*eeXiT`*jQElpnEgs4|;n%dYYojzBky~lGD5&GjtVnSVb6^tLD
zPzPKWFZ7C<6_rIF9NqRI1&s`W6_4C2H%$`b2Qa9mTC1QBCI*iQHL=huavB&%!Qcwl
z%I1g=<-hG6nelE%o%={GC`|jv?E6lAdni=Ezoxz<h-t)YS0m9^+vqyS3OuARR&I`f
z5w;BLJ9%gswvGt_M%XgA!dkg8sX5zZ-O6FTrCLFX96krW+Yv4^deC5g71Xi%yAR0I
zm7D6hLo+xE*NT_so;ll89WZu)VMZ@X%Z_S(zOk6<?LMHs0AmgqT;WE}<Lj{Ij4YZp
zoo>Frq3@N+QP(Ro%b=QHw#x~bSn^L*@UiBO!p^VAiq}Lilx3LN1kFQ(Ib)DN+^Y&*
zPN-$646C3xjE8h>Yv|dc9lf=&zpMBn36lGcb{Dp@Nj~c<aiD{@V#}ZEJ-yITpgmG+
zjZm-W#`Htcj(Rm1^T2==ECMU~Z5VD$r4Dq^2`LP&aIJWJ+geoeA|Kb)?aJ1aye{!M
zyc^TC{Z+h=%huA~)=rLsTk#wbVrz>^`T!V@gTa00vv9YLfuRk9E4Y;#)3EoNcuC`P
zdU+GpBd`8z{Xu<uZNJ?lCS=3!d4;=prwV;7kDplSD7aN9)EKH|DP=R_y(W4)7>|Mh
zt4J_6%$BBo@bw26+Az4njl40^H;iSYTFQc<9Nxa<ww_xaw#^5%eKqPpQN)or<T=@=
zekL!R*x@0?vE;yt$0P(8$}oajZPK#+70+oQwhVbr5krE(72L{=FMsBf3P18prgl#6
z!~5XGgs)NK*CP%pj|DZ)s^9VouW!+yXD~gSYentcc$*#R)6aKczzSoZNz3-4<gH0b
zh#57{%DZ6n0mD^rD{AM45r3|g&i`u`uWGXes3ms%;B%D3gb(~Uf)?Cyd1w9FTgh_O
zq@9j}Te&fT;TZ<3{5U8r{QSrKiu&MZmUs60gYiBXTmcVL6q(Tr@f^8Wq^mwY=TrIY
zt6LQJ-NrPff$<7G2jUr0zqzh|Dx;TJYL}znRy-ylt`zC&odBaT7=|9CWxS#_4vz^8
z&oH=xTe<P2fA=#Lm7LDY@m?d%eWx}Nw8vmQNF|6?hB$s`aZYtAa$1fU-})hivEnfa
z0fw}Upf&_)1+^qtbK01M__f73*$s>#K7%W)6^~DdFS>V>hxcams`m)iQO0-M<|(g&
z{wm1Br8C;fqFHiy{bsa995!0lDrg^pw;y-!s6GbcaWG8lq_k|Sor3(~F@d2BgDYGs
z9^cLW9le+Oe=c)W(P`G_ybtER=v(Ni$D<dmny}tc@V?fK2@Gu+ytlM{tsB!^^wjgf
zDC#q;f?K)q{c$o`jx9JqpL=dR>P=p>o(+Dl&pqKD3IT;8#WE=Hm()JoO*b!kw;&}U
zE|p&@!=vK#tBD~+3{(3!gR{}1$}d$+k(yRohDk|?N^=i{E029uKl*)rpJC{34YzDx
z5vghwe&0v+$E~kCj;^ueBgJFK>+V%Ap|7%e#d`yn2SwN@c)D)e=8V|`#z$HW7_Ze=
zpHm%8Ef13}jTxLL1QcPT;OQD(Fv@Vo^b4Pci+%H|-j=@xdTPs{Ysyg?UpWtRh6|rY
zibJslinQq(1=LWQGG8@li7ZxYh^{!dqQfv{p5j$14||q_mMEt1%Y!^IW&Us+4bCUa
zq7??{&&v-$A86y?_0{Fc^T~=7i=g1?@_OvXp^u#jcW-x9g<gFgyK3{%hxDQLfgc(-
zUdL~Rb|XY6G!fAs!1a;SdF6L)GwV+-c5)Q1l@PT1nV4_{3U%yEq#GDD!QfV4BpC+S
z7~ul85Wk$xtG)r_AQ)WXT5(48AuXhL`YW|%X>at2G$-to;_rSxj!<Y{nwa`kjguqe
ze^YO*?cykS<UB`&07ErOWmw;Z*zd;?FfxN<*e$BYDPk=48CC%gQ?1;XCeAG4z1XRm
z{_EGTF*+9xV@!hOZp1=!s%gm0_4u(d32~ruL9f(}vU<q>(jgA3;8t#qOqy9l_Xp!Q
zFu3nQ%?}tz3Cw^k#0QlN>S!>AgTWQtipL~G^X32ZzHU`P&zpZMr7!7^IOd?%Cu0wB
z7Gju}CZ><~HS=1|iPnX7ypF$8Z4VSYayLFO^jBc?0fYMv2KG(@Mo`;>l!Q3AubFNE
zMg=gqf?M&Jgt+!*8?Q>0!uqc&v%{!~zE{C$vl;a&9x>H+ViMxdhMw1FYXzNn>S7v&
zjhx3P#Pv7Z=*lS!?)$Oi&4?>tfDMoPV?+uecfiokgRun+u5hiWzcP`xy*pSQf3A>@
zD%TvfWG(F-;;#v4b0Ne;<s6Iavbdg3pX0v4(NkKUJ9S+3Sp7^Ig|)(34JoaKx;#p`
z_1(eh7#O9%Kw3yANQ-<n4BGuLY@eYFgDbcd7*H5SyDJOj<J0Tux#LTLVSPtGf}H4v
zyqT~Mt9j)71mqRn$C=sryu8+?pnj`Obw|Oi+{oKsS*U7)F$WCV88B&4Uu76PCZD0M
zrZBjITk-gW`0&^2UW<#5=;jGuVoq&+#|RUqIu!~HLpxc3>ygOawP^bnzpml6n^8zF
z`@vOkD;|^2(1rmkq!XkSjA8+U$Ap>8*EMu|FcQFU6*h7npAb{F<oBZbJg#^A{2k)3
zz6-G}j2Mu2F(`}0h+_)s0Ih4kZ_&j2IkB+zrcO+wuvR=KAtr6fuS<im6AZfdVbj`1
z`Q$TrOtdy=qKVN646fi-JU$`b*>F&%A5cj5I#=7{zN7uYXWFA_hWa%NKMN2?MSNEO
ze17l9=P|l)yC1=@(YjVPCaeTD98~GRcpePupG;cR#tj20rN-nlv|(_CYsGWq^DODT
zQ)LV2v)-dv30vP26Izh}R0mKlA^fZYvlrqh+BThctwKJ%ZeDpu!L8hwPG?E4KLg`D
z7*xYeT3{fifZ@gjh9<@aFkFR=+>LMLoRZ#(IVE-1NA6)p#(mF#y7U&9OW?ae9Gh@G
z5!XLtDC(7u6wpIP=0+S=!L4{qD08GwKhJ_eJ*P=46v}{n3K%>lA<kwfsw<{2xPn{p
z_~6xs4v}Ynka}sMN{F2MPV2QLV9rN8)E{p_-ci4`sZHicqXPfa)qbwvC|oNZd1OO}
za3?U9g8?h_mL{zU=+grRj|n|Po6O-RU{nEvD_kp^BSL(>tA#E!JFC7qaVhf3`i{}3
zKE?~Qi57^XJo>E7k)X|2`JjTn_+mBv8t(LR1-EizI=!ofR~U>dV9<zS(gI_jVIZZ{
zn6SqApn`V^jEP`y1-IhyVGp2Zl>TT#F1>d{F0?`GJ4zG(El%;Bu4$e{tMNk5=F`^)
z7uP>Oo%=r&ffbKQh>Sg>yaQnD0fSPqY0-7SaAPX)Y(DQQ7?;5y8>8S>ZhVQ$j>#R9
zI_UM@QOpx*PhpwgFG1ngE3EX=)T?%Xm-A*8tf(7YIN&IF8Mtw*SawY924jWKu=NTI
zzYGG#6x6qXEd&_aGOPj~rdqjiWPQ4)oLl8Nz3b~67;(7ov>&z@?V~Y%^V;6>ejoXp
z>Z&uhz3C{p6^}`Xf7|p_bHGqwz$&$!(~Q+Hcua`vejoJ*7&*Y;3U1}rE3aC0_2KR|
zy494tm?6i6(XPRvewO#OJdVMg$~4H*y@wvK<~|r!;l{ypAylopJOIX1KEuZ0_Bb)%
z25ceXI+aPv3P!BYunO=}t#~eAw{~S6l`}((zL2jbW_=@(_tX4wn~wl)O#ie9hdXb7
zS`T}?kfU&|czj?;!{EO25rF0@hAqVH7U2jnUIv3JTq_$BdWJq<$;>$_>#D^|LNO6z
z-y}r*2j)?BMCLIGu_*kFYBsdJeyQ~V>|a?0x8gY>M1?+IDZ}8tQ=Kw%6k0c1c0%|Y
zX&79=t=yP~U->~k<8{_&ZZAbGnc&BPzA(+)(8eUh;C9<0JF|?}JI1y^v{u0*=kZ}j
z_{tBe3m7MThV|W@Ax-e(Ahr;L+HDKJ2gYL)h+!4rrCRZrgpkL!${Uvk>(6_f@y7~U
z8=y7S=QCFD&}W(X?u~0d$Re+F*5UncW1iR|Y!%#!$LAf}s;-0aB^cayK4Ybk+N6ZN
zzH2|IN5GI^aD{7SV-g~2(Po)^tG6!HU?sew`RqK3qaI&D@R)=+@x}((`tgCfc#m$5
zf?M(UgebXavm!<TFu3o01pzkNN5J+O>M1aafx#8r%8luR&fm%pO243+W-W@5lhzl(
z`6Rxg<1t~T-(gbCn7c3N%6EUp_+b^?isy(ByE}iYh|wMl?mJ)6Q7>xPLR6_YeWzh?
z1-IfcVTXQQy2$!kuj!;QjU2CNOr$vj)|!Zoc8X};o`Ch>m+Q-_bR}QZ3%~iqiJV*U
zToB^&x^&^SV0;S(x1v$aFt~?4LmGywu+j2d5aPM$@8s&Avg^sZ4th~Lp1UJ5rDf(N
zjXQ0USth@v$NUvW<W@m8#){&g6O@SwcaaNVD8t}Zd_<<0?0DX^(<WsYuEJVT9F|e9
z%@p}hgIN7r{vD`^oNm3MD^g&72d~Z@jSD~Z$vEBnyXE-ARTwMm9{97GKmmp_3^y&y
zaJ>Q}LX7rca0Ryl!=Kdzj2WdTMAqaUtADzgGi{lt)+^Vm)9D6;x4kn;Upe}cqj0Tw
z<Y0t}@dX&1&g)g+t*N`m(+!9i23NRNoKY~UM^cWijdZWK%407hxqh$*w=*RdXsx!}
zzoy<g*xxNO=6E=7$H#TWuWLP|FjiFGXy+i<9gd6c(ZDds6Jr&46)?cwXW3vx41+7U
zRgepYarp6(vRS34bk)amVtz$;G2$_vZ=$|8H73`qtevvRpF}IYv0)KM;ac&XR4|lb
za60!2Y_$1+jWb@IvZza7tOA28Tr0afjI+);Z_93NyX$Tx^3q*0%-d<5IUi+UW8yov
zLX3SkBn#9Xpr0$UH;uyj&i6>M_nPyz>I%klVDQ~q?iJW*=Y~Du-H<8-MjJ4=f?K)!
zrH>AqC#TOFq2CTop>>*H9>E<4fotk_=^RBvKR%l2;QmMV^|IGnFYAxn74Ta=<ziYZ
z<TUy#vvUiEG7R#a{0;spPzC`5Y`;8$lvZ|MuZZyx7+k@vz<|ew@yhIVa%71(y?aT1
zCwF;{x?VjpagBVp2N)CApyX@`x>h{$;j`DN(O|@b!Rb6lc~0Xz+{86%3mAjI;0o7@
z=g5Kux0C*EJ6=Cud0O~ba!&NCi=n)lM6D~Bq0w3gr%F=GhW^!H%Xt0POgdfWDwq{3
zO)wf5#=~jREHE>eIl<&?Vq5@&E7Dk{mL}N<k#*E~{m(ZOsRUX5;q{8gvG3{leWAhQ
z_4azx(kh&2gW5jR&jrrlY1w+kYmX3M)VB<-;AJ3$%fM_{=MfESQ%phKecP!qY2qU@
zDEwdepHZZ>dYF-2OaQ}r;?@$LQjoS`SOtA<dBJrkM0xd3=O?}O^D@c!)5S3wwDxNQ
z-GvQCwN)51=(;HC+X`5pL)llY-`QJGxOSv!eAYAyYvtCrbX}g*o4}X`2GT+rL0Z8G
z7cgj~rdk=?0j^QMvpxsLV_<Lvw{q*)``^Coy^&*@RBtrFtgk%$rM-n=81X~C@5JIU
zVQ&5SFz<NjGV<DI(;Wr3;yEG&7<wugYr){YQ+cGE13@1~cS-`b&(J5qxC{nYa4Q}Y
zR`jcmga@9#C^Mev?FFSt?JgE+PQcuD1=?S0Cnh02D-|DJarq-z;nYA!!6WB6;xi(K
z!F>k<_k{w6%RW;oKC&5%N5OCvWEXhIW5QD@?|0SL?raI`dUT34Xi-QTanSsM$~?h|
z3C{u)uB*Q;SY2l+ayaZNY~=L0;rR^DGF;y+gR^rNuIqgRMpmEUDya4u4=v-F-#?WP
zw#-n&pY4^>a#Q=k24qEju-iU5_We;dYFAU$=o{P(57Nqvh;91_;#h$ingPtNzkjOI
zZ=Iorf#KHnw0&*B76J@q7+evgYpm#BhS4r+zT8x?iEa{AASI6AFKN8O=)4*+C8KO4
z6icA!o<@O0nH9}NNkJn~o2dC}Efj461xDc@t%p4{UV)()BUKT^VXQb~-^NOM{l<+d
zabqRFUb*GG0c~Y1^&^-wuJzl$5Wn3jt$%HjM~AX!a}=%>uVY|%h5;+gYfV~ImjVWP
zXxX=KmG-WIQ3Z^&3TwsdTl)vuWZUKWb<Be{@XFG;S5!8{#@~^<>1S^X$3KYDuPkep
zMq#aZn-`+<gKVl37*)WaGB9bm9&)e1h!_S}@SeeK|2sdEUa7T3^!YW5u?N8WTGuPe
zx1euNs@TM<j~Y@O^@}U$+F0>E82b#LNnH$#p92QUH%N<n=z0Z)u4fsk3U0;wV<CP|
zIwjxBwM->WdJb)nR-?hV#JzHB{udXv$dw;8(eIDU?kKnwl_0gjVC1}#bV{uRV>uYG
zO0DhWm5qs3sR3Jv<O^HWVlX}dgDbf2lna)TT(fyND*b4^;>8yw6sh%<$3b41x_~pn
zapl7OD~!S^q*0E7Tk%>c#POQVBhg??1B2)$EglDF`;5qd6b4stE4Nne#(ZJ%_rvw6
zd6ThXv2oC~?QwWdlr%4sDu|iJAj~q-C{p_?o(rf~P%uVnd8A>a<{_TDgICdD^b8n|
zf=UoML8EhOX$rBU$?K7JgYxN5^0fE8G9wEgk!hAec^HhKGbg6gx$<<=8+X1LCQppQ
zwW3@wy9Qu{4TI(fCM|bFwkhE}!^Cu+VQ__yOBv7U+A^jL@2Q6m|3SSmyeD>}?EH$n
zLdp4U+x76dI|aSLwd?6#wfFivMaC*vpVQj#5b}>l4uv*~V6~lUg&a*~q{b)2<l#NN
zSK-y`@QUIvRy=b0+~kN5FLoF$yDS*2%g>$ec*SR|Sg#`{x^pe?({W!tFS<-6ed(Wa
zlKeFcZpC8~Vqk~S>e&<qtl(9U7N4<lZ-wZvubz$qBi+4n=o7hu$3(l4L5^V0viCfB
zd-w=lYiaf{_Z>d_9@2W*UAHIp+bG+;IYdW4zrs;)D;|^2P{hdUGd8EtY29zvQ9LHJ
z(taCNTQJi346EQ)JU*P1#WMisGo4g_XF5r}PuQG+3WZLi5ANfaf$LSPRV}^c4+`qD
z@5?j_TWUNeA+Ba>r_X_L6AVrV1Fbb+P+1$c5Mbz~VEh6GS8yx7=ODy}#yz~>c4&3{
zy|qwS-)XOh7&NDHYsriSy}TI<&Z%xInmY<^<wm})aS#0m7{|ci?Spo748x5H3{8xV
zU~mPua^pMHe}p${`)su$^RzHpO#^e5i)y}UMchLnMn}4MVYyeW81W$(R^eLNT)-33
z{YU7TU`+HGwpR0&ij<IF!5KCoUWs(k^}tx|Gpxe3vbi9{(njyg1INBlTaP_McNF2f
z*$Z=5!iXL{(K6Jt=eA}G|MmM2?UmT9(khG<jfvDJQlIQIlwqJ|U_NNlvJ4sxEIW2<
zwn&yghUg!QZ}vwuqu^FFK3c}Pwjp(U&PW{-Y6nF_wC!MTl6&ZSHF<wVmFe;*{Yi9l
zN5QS=I_Q<?Zo}=iA^8^=)xqF&?iKAn7&iLd{TZcUa0R!b&n;tL(%r}>Nu%}CZR(@n
zwR^feCfBRSa36~96(vj@yqhby75ZY_VWjr~Y)2fCq`ToGVBC1Bz8{CBTi=lq+ox`H
z_h(rNjD=uuf4LPHIB{qg&96qOJ3q$h=2w?f?}HICb^kkcpW5|`tR6fVpECMeMOrIL
z2`3s62N=>YIGuaN*>r6gTtS~356yMJIGMGKdguHMH96-nkN<^xMc2VyfGzN<Iu!Gu
znEM|JYeh3PI^VkuKVV41Ag_$oe|Z>q1%@;XuHYWhwPkF1s;+L6<LyY^+0|*~m0BCz
zT2d5!@I<t8T1)L1Sx$emu9M1ey8c57V?`+$Z!vp)s;<{0l~ELZZIBkPF>XwIMwauw
z1Vi`?uCR3}ZG01S!wMsJ1ku^nNpYCofUbix6SPyo>r~okjTL=vyrq;U_+_2Kz^Z}H
ze$c$i_BeEH+YKo;ZQL08FNLupg=K84|F(Se(r=NtOD|(pWPPU;Om9cm7#$D;t&CoT
z;zKB2`45G$qLhrcLhQsFTgpN)2nshX%OI~zzTwOS6vP;jM!~J<+A==6(Ky`r>TsRu
z)|-A$Z9E~KseNuO2|vHRL93sK>83T8I0|m%#sNmeFw)ir!*JtU^!)auC&73QjI;{l
zp&Lib8QEp#@q=`H)O*w?rsiT=uWSkSn~_~*gknyhNG)5=a~bjDbLts*a;qku+6var
zOZ>SfJi;gw+@r^DjKMU{(ld&C-@Tz4eA!i(Z`9XOa4Ym%u%tbp9r#U8g-r*emd{AQ
zm>Q%72I{L}(Aqg*V>Ed8hO7@pQ=efK!P+^n0wc*X;;Y<O6PI_>14mti|MpHbkID6F
z=D`{2e7ljlUiR<NO09xhq1LCw@dtj-sd8UV0%N|<uyofee#adQX&F{Qvn6w19*mSY
zh_Pko3^ld=NWJ9EUy$ea<N){3^=kXB`;qLs$7*q>C1!nA!L4XcO?ic~591W;&KZ&z
zi^1S@?iH1dJ!=3)*f6+)ThW}_GM+eAK+}(hAM(nSGtDw-21fN6drH1nG#|nl7SzO<
zs3mPsOSpnt@j6CZnFT!j=$iNm_NS=E(R|UK<RA|X8*69OM0!r~F~25$;QJdWtQD+N
zYbefN9ljcw+qtn$I#LF4aNq6uq99jkKRFru{Iw64RU=#dq6UYa{|`lA#bd&Wt;1Kt
z@nGx)18Je187q5^h{q&Ejl*T-h!h4_a4Vi8LNu%Pwal^eF?~K=6fsZ>g7ZoIbQ7KD
zMCoFzr9FZh8$XfvAE~9Ey<FH)a4R?Rrq#Yy*}?c340@`~q{UA+1>TyoQD7*;a1}Ok
z9v@Eq=Da1d9D7PHi`tFe$DTYx-xQ{^d{jg5bQ|KJJ7g;m$I3+&ylw}J>ScGYI0|mX
zW5UUSoVQdqFqVKpPs-V}>}fL|6Hcoys-TIn8Vs)BRy;mDSJbMomno`^-u&?<_-TDd
zU+%{doQE8OYkC&pD&nZOIh&W|i)VEDPFEZS>S(GJj|qDKtqSX`V662SLy%687L6D7
z+@p<2h$l8@(>cKS>KS5K1w8Z>DY?sXq~yN*UfoVl>u0JzK%QIQ`Dsk71<^CjL5g%{
z`(jK!@9XPr_0y})r%`Y#8xzj;?aQz0fYAXAT34F1_-RZxrpqz;^cP@k0D~)BD<0p)
zx4xIR+UC`J4~IPNJ3sw<EV&9&TZZV%B93A;vUrnQx7Q<{?uNB9KaFdx+{mxJ^}V_S
z#&$4}7UD2z+0(H+CLzky$f93QVQ>Yv;_)GOFK?11iuKZqa=nK&jy>6n`~UDcII~Sp
zv(i{gf5$tWetAO9*<M&5JJbR*Osn8lZscGnV$25vR>+SaE%&stjR`ySzdWI4fN>HG
zt^g0};6yZ!5AUE`S5Q7EJwSif@Bnhx`i?#pV;1om!uNCN>zAQsR>5`64Gm@f^e^eP
zFTCq0xD}5H>zs83)n8!T27`9_O<G|1a}i>=G1c18P!$AY1sGhxt=#zf5B#e^o_|K^
zxN<LH2h#dZBOLMQc_nvrE*pL?(zd_|eX!VUN8wuW$iYam3_A+b*k>6wCcNJ#{9f2H
ztb#|*<HLwk?icysU?bf&kxt!Py6Y9~6wxy+^nRx^hc|@x<Q=Xv%v<0nf?c)1s$c8{
z^wxCpf?8Dda=)lQ!5Hc@Xh$fpa%UE{obc@H;SG@_Fn*p(46ES2gW=D$0!GW%r^qZZ
zLv+!Fu^0__JFqdiUR~*#C==)Q)5UuxItp$@DFw5P@`xk+`V^G|j2{=oVkX1s)_1p-
ze2cV~f>9E`xq@5KwPk!WVz&D5#s0eAk|h{%Y>z{pnD$85R&nH3J@v({z3Qj!{c-lg
zRTwL(jdUHntqBZi7@SVmhT*o_t+(pQ17KVPgDbcd7=9c9qiliekzRA&Qw<9Y#%#%s
z0IrAJt2<5Wh5Ma&Out^Vp`+lwV@8f#m<bNO<;E*;Jsbyy2L`8euiRcVu35dv5HLoA
z!4=#J4CI1gWd3TCx?8Kio;79!YN8#zcucNWrw&I{*(ZDJS@+@{1-HV^Fv?>(Mv4oV
z0i^$GlQaxY=U%y^_W8pR83o2-Ft~zS(eATlTsePEEjiRz%V8zZukv}6jmh=u$zKPm
z%b7>(S-Va;GcVVwmmf#aTYPi=oLmORHZWYe9g+Dg3j4di4wRR`_}}i6=wYnCu2nBT
zj(`#O=+UIh`}*tVubjXRy`9&(F>$Y!)}E`TeE*b=YMdG8WUPXo#0;KF*o^oZpgzq*
z`=Gbn40!ZtgKJ<^0|Qp6bncZq>r1RXS1te}I~ZKSt-wI~hB0K^hDh-(QjZ!}5hEwf
z+k>?M_t5of=9uFRKEKgX5Bf8gqu^GQ678@Acb{Gzw;@~si~(S9I`_(*h3_15JjpP)
zf?LtGW#o(OlNBmA)8CiC+l|<|k32Ck(X~~)&~k_Ta$IGdBcZ}W3S)))kp756t79Pw
zNA{_5V4U+AmhO6GSADp5*K&tC1V*&a;0kLMycdb~9Eh>3*8|z%&FVUKQx<q-_eFS2
zu2&17=nqA!GzxA-D@*EkgT051y&kCNpy&Vvr}GsZto^Y)*przEMQ<p2JfyHzv^KPi
zzlyc=RwrfF`M17<@x$&8U^a#*X&p_w5i3&0bGn{+KD(FxKoi|}$-9oiwc;}XFtlO7
zDrG$X18LeF4|i;zcOGXYz!(SySGZPuoS(h0jaU3oA^ma_I`M0L=es!MAw?dPQTD`+
z-uZpG^w)P@a}=(X6Zyh6x+EBJV31cPEijVsZ=|xriAjiDi5>M1V0-}vSMbQ`^I(QU
zxjSlL3$Mb>VmkB1+h_x`DDVAr!Sd&cRL*WK$(8*{Z(@~_`j4eQI||o|M-GNI3|<?+
zNUcjeCai?BKdGmJaS#lyaII{P;GAWojP7#(139;QwUqtp)Msk3D^F$6B1}6ji3y`o
z+o?D0RqT89=$1!h^T~Z2g=<B-&h+#d&98Pv%6P<>2L`NA9zj}|z2ZJaz@WWX>Q{m_
zPS0ZB%W_~m2?kfVR-EzJp)UG^O$X%mi+gDnp6WZ5Lr`k-;mHb&2=oq$X}Iq9Zx{XG
z>ONU|t4^b^R&Goc4|Vb02V*lBR2yts6H;o%e3UGY2@KCLxPn``@qKZ6ntJ+!OLEZU
zF34T$JAdAtW=Zs}29zq|`1aFoYWS?r<otnD9#-L6*_iOAkkiv-J1_==L9;%SmixrI
zjR~vNPrJ#HV7v(iSAdrqIgbyk?X^X`f@>Gc%{a5>`flGQK<|N|*#XTwgxEf=u($mF
zQduU>a}?aljj1HgspSP@6Bw|fbo{hv7YBKR+DGqau<sub;=OT&^(rt*gTWQt%8l=0
z{KxX?rHf?yUwR^P>pSkqz~|sy7MN9H#?b<8r7k{e+kS`q?AChOw0|#0!L4{qIF}Ls
zv1$WG2QY#eYLHfN*C$}`m@p2u-=WTc(FhDzVI$}9!K+TQWwXW$Ws#8s;Fa|qt7_y?
z@ZJxcg+u*qkkV?GPF^5K=ZlvO)4K|8#bXlU*-o=nQ!pL@gU;32w6>vK0tSysh}Dx9
zs1Yd)uHaTYKHOog+{%0TYCdo9M_tect?xJyh90jraj5@Ac@#w)Q_wGOz1r6s^6K~U
z-;bxGbgY6~@tA}-Rk@WO2*z+Qg7tWiR&eSpU?8Q`n0$sF07k}x)8Pf}5*WI*;_+d%
z{ZtO`=WI&WUPdbx>pSj6p$vj|?@;}v)lAUqP5Cgrw|q(t@9&zG90j-HF$wX_Q#tgH
zU_1c^^~NSGy5D6OZcLLtOs`jfaTg3%LFesKbC<=pJ9JzwTa?qgJiI=<vc98NgYR9S
zqIV(D+lL#H3gd5~su3xl+atHMrqiFU!dUT`aCR_sTp0$^LVg5k(Y0aln0$sJ#=Br}
z1-Ihy3DM`hm~g`{U-VwybQ&`my0@7DY4`W@o>urlYnqR-<TS=FDw90%N%Hw`%6Qe{
zt7@*GS&gxxIVg5lQgR!Nh+)vo!dQ{oFnCJXe~nL4y}_soMp}jOknRFmM!5$o<-em}
z_J&oX_sh|Vgq)z$dp>PUu2=UiRF}P@$9P%Z*@x1x3T}l`M5`){8c#cU<sYn6hT+mJ
zgQtYIMqH?_h|wL4v<l-P)fCIPK60779^Jw#RwkRDyTPjEuoIK(RmM`2BkxWa?Dfy~
zzF&ey!L6uA2x<-G&fSsA)KxHwg5lC_E(CGV3Xj+sN=*)L24fHy4=VyI+QqYs>O+=?
z2cDc7?!A5-l_2PS&?XM0#9`j*M?AxL_ihO}XIfWTB;70jp-5%0IC2bG9(f6j7r=1+
zrE~*EP<sq}>)jG+E*M3@;0kW##_>z}bK&3qdPeR!Gzxi@8NFQaUOpa^o4c2CrIUSo
zjg)PAjdT>;ic+Fy)9G!0LKLWQE^-Tu4}6BrMc1p$$iIM%`EKrXst*{geTG%g{;sj2
zYs=WXwyk&Impl?b@XEg7kH_SC)pqI-FT;){a#Ft17!6#7Ntf>U97`^N9)nJwzQ4Au
z-V4S-pJC~)SNxrTU}$2D^BG)Wt?0QA%h)$R&KtRTjjVhlH^#xdVe@u6?xE{dx5<ON
zTQO_pgoX_q1-HVy6eDIddW_bHBQifu$AM7=3{Iz38uYvLyfc3@UiZm^^ffRhg25Hs
z3Jml!hVc>Jx|gHTIPXU1bBNC7B5$?4Clcb#*LumGVzjsU*SEu5!L8glj-I+0$pyy0
zKEw8wyal`7E_kh%BE}}4VHMoUjpOJuoz(qBqrC41{EFP;ao8HpYrYUa-e@IzUwg^R
z`p0VJDr_Ica{+G)eWsHn#%Eyg*2!x)_Ym*?ywOVa0V4w#T*0k)F5vD`hIaBs!YHq3
z*ZX1G`Ns;*FJqpgJg0>ymE6NSu<BO$r&YJm?{bA5`+{D=jGSjPv{S!>kp~Q<h2An4
zA>AC^kI^7tqqZdX&>w+u9t^IadkDrV=p_teQOAK^#feGstL4-_gx@}bnSgC8JSOxd
z^9Oo4GbG8@2eUg0*UH8xL_)`bx-1xHz;J!%Z9<@}n0HO)nm<ry0i!t>u7X?Hn6TQO
zJ=m*Wq?gQIV}WyWIXI~kgL**sD`-E6=0vm$_T0{wyhD9TOF4DBqj0VE`y)}{!}GHT
z>snxB1tZwG3DN=s=>-gairHsqmBMfp{FL{8lx%PsTZkn``+N23&XpP0E^>00*BBm?
z5V3<^^akB*B=^T?N5QS&l^+L67_~I<Xn)-RjO<`=-+3(|OIsc<4ti1d0pkD|uEJWm
zk#D&4yw|1u^>Ea4moXZwMf))4rfrV8IlcA5SnvEV_md7>xF6;UZbhG?UZdxr9V#&N
zOJEcSgXb>AVRDpqcPtwW-TK1)q{Cov1-GJW%lNdwT-k2qVEVy0Xyc%3V}<sDK7i^1
z&D+PeTNUnGqNsPgTx&<ct?1g!$b|qyJ(<D?+Fp<r_k^x(9AHEYBdx+%(dU-YI-Dej
zcWv!0X-d5hofo9F)CKJS(4C}dm|g6^In|uVm2S8mhB!jmv>ddjwfEZ@|9xbs3fGEK
zqV*y@McXQzq=td90*qj`8l*)#1%|=dK0_G>SGZQ35jwU&R?2Z%{xITMe@15L+$-AS
z4c?_dwS9j74YEL?TXMq~de^;8%eA7v<VEx)pm_Y)0#y-=b6~&<>zJ<_;LoUav_>%b
zV^i`O$}qTsQZ*jZwPi%2r^v}Mcf)fFkMO-Rf8kzH&8N|bW&t=O+$}-wiOwls-V>ZK
zH44{?cnPS#H2>WnJw;7PVNh*1X{Ghhq$I?iZVBprFyg@A3fFhexcOp&+&wafT$L1f
zW$4^1cT{`5>3TVQ?g07H+yN=&5%}v`@s-~77ZcP@FxG&<N3XOV8nzHqo32-e!4(vT
zwc?DSyJEccO<P5Z<69)%zLu|2d3g-}XSlb$6P}noI1&A>9q-)8ccA73BZClNXv5&X
zyB_jUZO}i%^?EScfx#8j!`boNjy^*4I8)ub&}647@*$n;v->-IrvT4>P}`?HWjtG-
zylQ~=<Cl5W<To_tK3rj~@ODq?Az-~9@$@`XU7rV|sLyzd##TQqzEj}t?tHpxfIb1n
zM4!PGb`OZ&AZi&0hE0@Fxrcd!bJg|xEAjw68(OG;rxM;!4{G}^A=a$>L0-!g>wWaU
z>Ztovwt>R6qVf-VKD@<z*hEE)0buYxDri0Pd<IX+XDGwq3fGD=qHAQ7Eo+TY6)%qP
z$cko^XlL-9_CTo=xA@o87vs%F_-sX)i{W3s>!)sPdgUR7wW74>sk$Rj6t9s{83xj&
z^6=B5Yr`OO!xmy=nTrv_;0o7@{$&_TM%@nI?$TTijT!BGW&T2OFdp@wh-W*zN>@6I
z9GCqGc`4yxg|#9Cy%Bg1e%6h;9We~@%2=iK(6Dj-DmsfY46dM5@n1^*(4}GY{CI`D
zRI!n_Hk3alj^Hm8hvD%$hEv6}56KIMt9gl2N;wMG%GNg_x_!JtT~1-}y7aJz#w#$C
zVQ>X`ZhhyBE3s?j$pZDH_;`@VcbEANE$${$8$o@byGvh=y)UbD_%Xcdoo<eTThTrv
zN)sa<)z0g&Yt#ua{{3hW?w`|>y+K-hht}P7216N!tDy5FX1AW+uVom0>eLNi-cd~N
zTGh(qbnaCU6Fo5-3$Lz}ohT<gUq_$FQO;3tD>|_m^d<A4=v}97<X13uf)V&0q{Tg?
z9Z&1kg|ZXX6fiymgDbcdow>A(F|A5^h5o3avSw@wg{8Y*t%5fVF>f4$-wE#*@g@y_
zM6JKs;vt2xqLk1hpdKgS_iL?6>fB&t^BJom){5>S1Tnc@fuSvfE3EINu#5~-YIwa~
z?=3gZsP21Z{z7pu9&)z;+DkWl*1k_|FaNAY^6BeU9#R-9iUa%NP|bt_3~d>!kc&ZD
zl+%{My=vR1wl0>+;0kL+aahLYm!I%5Zm#IfnbQO7k7Xd!c=2^g-VcE{wBNwJs{hy{
z-oiaCytj5g?kKnw<%0262rzUeFlK<k>B0TZfI;J+VdLHEk3FJ^u?P&V;8v6imQm#3
zN?Gft|9ML?<fk(%$h}}(;{6r(5O0FsRLz?|u7<az{~*j6tb$twH8AiH=SvQ*RK!RG
zgL}pME4NnyLla{=7+k@vcrKt%OkdjTQM!a&U$HS-t?g^++KeCMp{Z{|<ZJbqH~LCj
zd1*&oN5QS=+Lk#O+Azo~lNQBc7(6B+3buMoj|Jlb7-<z|G@#EdW8a8c-jIGF)#_J_
z2+Vi7He)S$WqXEJOX_*EW74U`4XZc`ZbjF|TfF&eL@jL?L^o-X?}kCwCMJ9f#*%t^
z1{e##NUJbb^toj$ZXDyCTvt+8D)_PFz6UjiW~`miUifJ6)bQrss_TEKYzg=dO%^S;
zqBxAV*vn`fqfewVZ2ub662q`D2?2&)m&&jT9yyN>GyS>W%a1-k7{2{PUyt7{=3^rF
z5O0<LqO|OPW|~~zd6=W%Ry-GchB6FTQNH^lBp>_SyU1WD!{7>T#d85~-)oHT<t*Gz
zC(QM~7XY)={unX)qmQK)oSIj-4(5zlvmJ<*oB#B5-1JBNd5KZ*vZ2|7o%I!MoS=x2
z2nNql*Atq(25g-7IS{Q3gDWUqYsDF}>(o=nGQR39m~aI(fuD!5Co3?Iz~9Wdj2A0c
zQO{?8&3o#LV~&Db@sh=ftvdB2G2R4&<|H;Pd)5SOlu>YgMu`5EtH?fJlmde*xD_uO
ztcnUxRm&E5vQBs?T9l20d&0{`h_??dlpAt1)t4IOa}=%>&o?k+VhV$o0r!M^h*Nrp
z7OKr)`~(J9xK^A|byzvIsmdswTqsPtgQ=&Dcua0-J~phJ<O);HX&JQI3uczzwSL80
z-l?@3(%>;gBP4l7f1z>L+~KEnPS7*Z3bLGdAWICHEAJI*kG$dvYsF`PLV%&?f-wvX
zq=hmMR--g>8U~G=hV3&HG46xG72Jx?0P#kL<D)&<wT`Np?_c<CJ)zSOn1R*Don*v7
z-kP~+rcr~u!S(j3tB=0zD7cjy2N*gQj9Op>o|v?@A&!8-y%hosZ5Uj^t=u@K6dvhC
z%^oZ_?kkKkx4z>I&G3@;Flp{%=W{~bS~S|r^yZV1E_r%53T{Q$HgYg@MKCsjL0*}8
z8@&bEFzD=uiOFZ^590%dtFTt|xn+E($9Z1!J4u}f?njz-4o9At84_Jv_O%)#y>ZzG
z$wlo7r<ucXE1L^A6{g4O)nGge2KS1t4TI0luqvuCQV#`VF&JFoTJc;syQrd<aputa
zBR}$JR}J|e?CJ5nK041wf5Qlk{ekKCq}M32FLLwDV~&DbQ46D;+F&mn44oc~5n#}G
zVbbDzeeMp@jC)dtQy5&qt-wGo7{=X&O1@C}-N-9TU&7uJ-yPzf@HqCzD!HZLmPpIh
zu7byb^ieCPU}oMLk%OW7r!crz+!K0t3C7@*{i}~+l{%Qha23{yMhMHu63XKB`l+}Y
zdzQXs%$^-W4@Y&0`V*`*c}({UB+JHkW~y~1dN~Si#d8GjdJSdKv8fE3i(pV&Nnwn{
zZ*y+xp90CMQ7XeK++8&r6VBq)SSs@kYNe+Z$gH{VLD>`I1KbDUJCFnaE$v0Oc@PP$
zZtN(y6^{vf4>gvm++fV|8Fr_UYDo|WF>FkD)6~DEH8E=V46EQ)yfhKVr25|L!$zvO
z%w^$~^_Su>ZN>Gf_oVuI92A3~;A!!?i<ID@*?avh%T&2%a990N=L;C4csbkqI6U9*
zoN$(@Y8MorLcycucV&1?LQMQLoA*RiE3e=y@ff3ce?_x<Y8S2d()@vTbWq3eO!ew~
z`MnX}ORx8n+Z+YAa@T5L=sI8&@EOfemx8o{*}Y-#HRqaq`86^6_zbJy{gt~`OTX<q
zd8%Yr@2jZAh}^CqxhLF1oJlR3*Sj<IN$(!=j4QYm&FAc0ATX3+z$!Hk?g@?MX1(h(
z^ldP1gW)Qy70vA|qxn~t<$%yxxw#;HHz4n2Y#+rv#60%TGvP)f4$2lQ2RI6DMQa>W
znnJYv>arrnyI^pycu(Z^KBxXX6KR^l;0kWVOB2ty?~-2nnXk&3KhpOP+Iw64o)`BJ
z@85mvaqrH&rE=HGa*o2avbo?hFn+u$XMn-I;`hAxSb{g{y!E&?46bmk>_~+D-S~fH
z^>&_D=-nS_za6<4+;`+&@tDv);{R2o$PWegg!9}oKeB3)9DS*a{^-5SSmT@ogTDW$
zF-Dl(xc2YTpdBXRvmLl2UI3rvs`{7wB)OCRqr)4H!nHa9)m7}nRfOWys!3`T7ze<B
z6-LM)t>AkN0tVO-!xkcQ)xQ)m(sg{pQMgtoFlrmd@9D40$KUU)?;W1xc!lp>N%aId
z4R4|m3-xE$cK<97Z|<Y7C4Pe5#YWDpjwRoNnw~0di=XT1uPb7RBa<+b`6FV}vYxme
zf}sq9E4URHh{G`EE&p8Jct2Lp>_m6mEuDLnn2-*$weo&kSVs*hB<oZgqqipSbrjsH
zUo4#qse;HeAdbZ4pR222ECvHs=pBNzxQDJ+%Z3zEwZV7~46fi-V4%(!Ms)KQa{b0v
z^`3ogF_W-#?v)UCP|s+uT{OU5{pfg^wcv0)sK{wY!L8sw=G!q;8!;M`Z{9*}07HQR
zE2x6BxQDJ+N223Zb}$Bj!4=#J3}qRo#wgjnO+Vd#M;DYir(54~k^{MN7ctO#cu$W_
z=Pg^(K_A;V1|z3caI0g<pCfWQ?N}aT?y)gS^#Egn&o~{iY1x=uuTG3jr&ob-)Mr=)
zw*mt>Vi+;+l=qHbsIO;CZ-!E`bnX@1>B9*K%pooyj*R1rc<EoMiKpleItp$@qc%oq
zC}L^u@J@M6j5%Py3ULH!fsq=M>lGN<Ft~zS(YR|FD^{M6{T|Jv|Ebu}<Gu$m6{fm`
z9h{x;YBPGDqKISWo!j!6303q{-{elCuvR=KtSDBVP<_C-4+hc#JxD9?Dq!%K@crj^
zZmUjUv<8DKxD}5NZ?n2KRrP)4A9b&HN9;!Ny7UKXMQxM=y?2YsDd3@7zB^5Ya@<v~
zU3V4SisuOKpI@6QTZ56IPe*t~wZZuQ2d)DKZv~j2-km1vrZBjITk)81kEMKf{aX36
zYWFtkcWu8!eHSt4lpFP3mKQqSTKA92t4qDm(NVZoHb;DhXBgB+nY5_)F$~`3@mBfc
zt-aA;6aa%OTr1nU@Xmp%S@fwVo9aEAcB9quapI;wUIc4f8x!6$@nI(2xOp=@cZRFr
zk@Fn!8Qw`SJ_3XLF2v0UjY-5HZ|%4e`Y@B%6pU$La0Rz=W4iR&=W@`{N_zkDQdrm8
zv5!W$AcmZ1BQ_>nU%MlV-3jR*f5_-4xRskDV5lKrM0|$zoklgwuwx=d&TDs6DKNhE
z8CC%g(Yk^$oW~?Yr)Ofk6TjuxXLg@QZ*1e>vkZI}66zPJsqfA4O)c-q64`Y0XO|rX
zw{mj<3~d<ny-Fr6K1bmm`V38s(qM1}w{ml#yBFnszV+8|kH*a~Pqcdge147dEQn)P
zN`Ez{?s)HV?U~_iTZ*A}bA`2{Ju<paLA{+2U}(cYT8JY^i_ehVJpeHD2ViUmgDbcd
z?U7l=f<9sO!L!43-0<;eQFd*>M*z1Mefx_Rs@Q=+divoNj)Ggck<afFmWIK5OFjbd
zm@xPGqJ=aJuHaTYJ|XJF_w(L({Rw$1-yzI<?4BjhX*YLwjOypT`o&;*x;8zVVHK_w
z-!-fs-w*d|pOCk~;JL_im+wa6tlFr4+Av&&-MMvl2SdYZ%c<YLs-GW5XXWh<9mXou
zC3-^vofYTvc6q35I5cyJ{xJO(%%Ql#TJc#wP8$xZtqg;=38arY7BJlX%jSp5Mq<GD
z6b!E5R(#fvv*)Ga)cvbk#ct_~yM1>4!N+jyAfV4Whj^0rqHn)9Q~r5*wc42FMMuG{
z+<JAaRGj=Lg#jz%SCAGTsd?@38R~WlgDbd|Td(rYEa**Xis#&$wnXHcjlcNL$@v(|
z6KHOT{cziYg&5JPxVLyn2K`be`qmy-VOlVc33sq&7Sx8p>2z%vR5mH`)e~YQ(tZn!
zLHNxT+=|DC^Kj|1dUZ0F)FqY_!fc7Yzxy2ZOBlm#9NgQW2Qu$5<qFqoeM*kxL{4;1
zmsLNJ!r-3pID#j_0=CO=74+Pgv09%JIWab0@w_(@is`0HW?($ultQ0{IA~X%_Q}>d
zF$uADM#$SdKfPQ%BZR!-3L80(4|V#Ar>B6?$Y<Cxpj~dup!d2_eVYZZz|dcS@s7{n
z3Tx#?{`kZg@9>CHy3E}D=u2$dcH^+#;+)KwD{}70rn+0B^Jx^;irP7y?x1&1;M<@k
zVjKjc1Q_&vea2rN2jx1&MEwfhnmy)<nhC~BU~mPuqIPZ>CI7iD+YM~0f8KBwUfCXp
z$K-l-xbhR;girG5+q*w^6x@oB?f5?7f3B-mVB7$M)45k}uXMEX6M8%te}dsE>@I~n
z&KFJU<=xJ6cVF8|e`1zldwpIWJSHK2(jB}_<Lbz!&9Y*qZxybU?T>{3LmLM7otK9@
z8vLj`=yy{XT;W>rdn~_h@9#~;TblObElsXhc4oobJXUv8W4(iKXAQq{|E#0nRy-H*
zy{~xtlVR|-%||%bLooCqFh+sF72L|rh2>9<_u5Qd5Po&)f+Rj7lUJq}<sJ$Fg*FP#
zplhRWJ&cQL;+-kCH?s3bdM~G)bMl%<*I0d{pP~D;XWoeMrmdK-Rz5<{RJ#ge6$*U>
z59tX4YEgrun&^(@_eOSrK{e4>@mlHD#4q27(M!Qt4hC0nD=-j~VbosO%$wdPqncTo
z_AG4=z<WEYmBbGEt46t6d$AX`L~dm(>L|DsrDVLts%T*|Jr#^OU{FmoY4Kiy$0Wr6
za<$giQy5&qt?1e^MjQ%xQ+C{l3_X;#r?%r2-X`aJMPnCM!ce^a9|~^8WAYW+D1tZB
zWk8Qf<DeaJcuZCS?+`h?%g!ilkILgiPu=gIaD{dG<=lR+Ib$u&&VoKTC&u0lejFGB
z5y$ftM#(ut%0=+oQE)3B6W&PL@1IB&Fyg_W*-Ma%l$ISWc}zI*P+^pMGljtw+=|C1
z#JwNmyz6+U{)1oXefrjS%F!T><a)u(nd((C<|U)z2YboK4n@jbqqkF91-Ifc`3y~r
z^mu1~Fn12pq8v309uww1@q_iJV3Y%cE4USp4|^OLYoUk6YBeK$t1kE5&Od2J7Sw5)
zi(bkT<y9*5dF1@*W)CTh6_3ehV3i#(Xy#?D_`H<IbU9Cyt_;RGFt~z8?#4G_*My`;
zlk=+feaCv_Z%Y2`<#TGf4$gN{95b>F^4|}SS?WUyW93E;Mgzm3d9<<Ob87O|u!(W)
zJ&K$dT*0mA+T;k{bJs80TiJe*YPhrxdQrQ7!FyD<7mZn4${RCZtBP;OI0|k>`(<=r
zg7!x727%aUy*-7&drRJnx+{7x^c*nCfZ;0a?g#CiS;pbNXUN)HHmQU^Vo~;X?}f+Y
zde!UdEP1WvK~?ir`u;(y;8wIJLwACxf5&O{zh|gAV7vter@OxMotLgxXQ^Mns0#*H
zfQK@$yHS>LdQ^AaWpz6F_-l7CBjbBv)<f5;QTJQw(N&hookKH&VHMo!ShB!AU<<@T
zbJ5R7b@vLTFla}}(ydo~Ck$^eyx-DW2gY_VxPn`Of&Ci8$n;xhy>R7qS-sL5GVsLg
zw^|QfuX2=brx&l!>b<kPXBq|fo%SsK@<1H-Z+7;!f>9X^PUl_)-kN>be5KoYv%pvf
z23NRNcTA3eas16PI?Lz>GT&c?(W2~LF^>s74)f~eH*Txn<7S6HjP2<txK+Q{FHxte
zpia?$-@IAI`)kw#nF|b9rP8@qRC~<cvJhZMVjKX2E4URH5zF{&WQ@A7=}3bY9vK6#
z?5;eI$@Qvxk8<HT@8wl-b;ddhZbd1XUI}+!N5;rYVDtrp)45l?@4_mzN4bb$xC(wp
z!1jDX%w1eWZb=%WtN!u=$adUzM}YnKTQ(X8v3pnUWKP+6)L32MsUMTL0<*|eE1F*g
zC(uGDb1)P!cK`eWN@#z?q{T-7cT_8OGN<YSMmjLKf?Lu2$}+NJjLmR9PM5}a*tm4-
z75s-+g%L+<c-0~Q7I|RB5dB-^&m^wkR>zV*hR1oKpp`Hf$}nK%*PkFQ9uxNp3}qNx
z!F>lKVi`r6t&tDX4b>xmYJl2i>D(*K)!`96gGGBZFBD3YQ~w^S53G*hQ>$>TPQbb-
z<vtYUnypdyz?cXItiTD<;vTwQfuRh;RdC-=pkxiB;M-BM#`|M*k#o7g;B@PIDD)FN
ziiX#l;nnn`b!3~+YkK*j8TiyHxK+Q{?|lW0&^X!uc9b#<Sb<~GvN6#vxAkhw(K?D4
z3&C&|)(Q+irGT-zS3d9lwbnXMl_u!nK1VJFqc9(xxwkl%F``V;t$Z)(MgJaj6x_;<
zV@<Do`T-b!S8jrtrS*i5)YRq;8$H8_G7WBnF&zx9;8t!N-_|}Rlco;PKhL>>@1e4*
zAGgNXTt_eXNwO@TZGf)5Z!&7SRq#06TmVBEhU=BBB~<fiwiMJvFqC0%1-Ih4fbSU5
z8^TX7AE$RLALn>QbKhVVHVZpMy!WZz=mR-EX0ZMzX0W5+R&FkU5n&9dG9&E}_L1z`
zkb8)`WQ{&hjFGC~R&I-Ws(p63@U5|W(x5#~FY0<_V+uw#%tJA^qO;$&r{<Bn>nJdk
zVWjol#Ke14m%$a>%3Vi|=y6y5-6mRR{kFJ23pX=iK9ad(Nu{-mRSvwZkiNl}TcJjv
zbp@l0tEh_i8s>&#I~060ps~-4mV7M1K6Q_~(s<=s*}1x_h*_H3J5i~ft{qhbyJ~he
z;r6@MLp=N1xw2PpN*&#ON*#<iR>9+N<Ey<iw>}C+7z{TKJC^WK8>@-Vm33V(I)K3y
z+=|DCyE1z(%J8n9dilv(VDNdA?RVXn=GFK~?)tEY9vk<Zqu^F<eEarZRQ15v2L>NA
z_{hL#Q8?dM<0nOonP9jIJ3Hes;XK2LQ*!Y0TU6oEoi(4=^3lK@;f7ARC!Z+LT@Oj1
zc^aSN+Q{7;d3VGqH4KdWU~u30$iQc{*e{uKPZ47p7+m36*_g0Tom9Zvy10d|mDoLH
z79OmA_z2*}^leVz)f&}K=P3~HD7cjy9~jy)>^R6rfMD)xR$kxb6xuSZf?K&UeKlvX
zyqdj{{%b}-v|2k3x^eK;-RU`tl~M3?xmSF=!Z)VxP~nWnXNOxf9fvZfwLx$a$h}|t
zj{iC4T}gOOxQz-|>3ASKNsV<Bi3!#!@Rr_IniGnfJ5(e?3IkStPfcm@`?chiVf&0o
z?Gy%Aa4Wi|wOYWqk9T-~+;(mFhZbG2y0fRL$P<$;U0X%Zj5lSqodwjPM|wV_Fji1v
z%}L)4K<CKrmB^^irZ702t_=fhwCI==MemF^RSXyh!QcvR1tr#;hVk33m%PTC;#E}Q
z>*y2hc{m=E>(zJBabBKUlhxjCuR03XDktVet>M)ln8SggiBZO9Si0*KKc)9$bez6b
zbF$j!GpvH2hr=7J(9ak~s{&oTuNG8I%C|WS>aIPX$YX+nd37}OjQ8D(Gs3U0$>}Iu
zD}g?lp6+jmpEd=$=%w?jCglNx)45l)jt*9kbTacu=owwO|BUcNFu20C5-35#sQu|;
zH8l4s`Af%G?2FpdpFAdv;mj+!q=|~W-$Qn}KipApD@uv>0D=>yU`WG&RVtl(Mf)@O
z8x)NZ2N=>YxPn{JwPjRHmqV9&Q_4?Pw8NZXH_}KwPmAjyUDvChZ|_oNOD+hn#2#!~
zg-v%G+FuF(M~KSla(H>bI0y!(bFXN2X4pbpxV=k00>(RFaD{8N4JBw8+dAjexr=u4
zz8pFZ&#Bm4<ZYYxL>LEO&!i{z?CN#M@D65uR^eLlIKc37fN^jbF>HUuTQK+5XLy!j
z6|NPJ<A*bEsQd>;dJD$U`^T+UD2vp(<hI}!Div10J~7ri@>nJ8_FDzF;<<n~{GEA2
z<^$s`Fkr=c8C%C(5BpRqEU$u*wPGdA=&gcV@m#>^&&r?4I(S3#$pc@bXR*HXR?9u~
z6^axkKcxH0mciR2uhaMr(1o7<bJ5ubdDd4%O|;K{&>aEn1XC*bZR<4N7yd$fUFFfu
zk@j9*N8wuWGT*V#^DKkcQF}L(#w%*EH2=l7rM=MJD-1?kpJ5fQ6)*DxHL82@!)kdS
z)ToZ0n#W;1ackv|S-z6FkN5JbEg0Y^xRv|dM5IP_y%3DuVDP$2cScPsay>ko<tvp3
zj5=U&1-Ejan`km|j~cfATUjVaM~s~Ij2HKWdniPejk6+h#JX_P>GT#_tKe3YBN#tW
knsi1O3~3nLEA9zDy(L7&jkCgr;VOoq_6N_wQB0Qcf8ltuu>b%7

literal 0
HcmV?d00001

diff --git a/examples/ex_collision_object.py b/examples/ex_collision_object.py
new file mode 100755
index 0000000..6802213
--- /dev/null
+++ b/examples/ex_collision_object.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+"""
+Example of adding and removing a collision object with a mesh geometry.
+Note: Python module `trimesh` is required for this example (`pip install trimesh`).
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]"`
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p filepath:="./my_favourity_mesh.stl"`
+`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="remove"`
+"""
+
+from os import path
+from threading import Thread
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+from pymoveit2 import MoveIt2
+from pymoveit2.robots import panda
+
+DEFAULT_EXAMPLE_MESH = path.join(
+    path.dirname(path.realpath(__file__)), "assets", "suzanne.stl"
+)
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    # Create node for this example
+    node = Node("ex_collision_object")
+
+    # Declare parameter for joint positions
+    node.declare_parameter(
+        "filepath",
+        "",
+    )
+    node.declare_parameter(
+        "action",
+        "add",
+    )
+    node.declare_parameter("position", [0.5, 0.0, 0.5])
+    node.declare_parameter("quat_xyzw", [0.0, 0.0, -0.707, 0.707])
+
+    # Create callback group that allows execution of callbacks in parallel without restrictions
+    callback_group = ReentrantCallbackGroup()
+
+    # Create MoveIt 2 interface
+    moveit2 = MoveIt2(
+        node=node,
+        joint_names=panda.joint_names(),
+        base_link_name=panda.base_link_name(),
+        end_effector_name=panda.end_effector_name(),
+        group_name=panda.MOVE_GROUP_ARM,
+        callback_group=callback_group,
+    )
+
+    # Spin the node in background thread(s)
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+
+    # Get parameters
+    filepath = node.get_parameter("filepath").get_parameter_value().string_value
+    action = node.get_parameter("action").get_parameter_value().string_value
+    position = node.get_parameter("position").get_parameter_value().double_array_value
+    quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
+
+    # Use the default example mesh if invalid
+    if not filepath:
+        node.get_logger().info(f"Using the default example mesh file")
+        filepath = DEFAULT_EXAMPLE_MESH
+
+    # Make sure the mesh file exists
+    if not path.exists(filepath):
+        node.get_logger().error(f"File '{filepath}' does not exist")
+        rclpy.shutdown()
+        exit(1)
+
+    # Determine ID of the collision mesh
+    mesh_id = path.basename(filepath).split(".")[0]
+
+    if "add" == action:
+        # Add collision mesh
+        node.get_logger().info(
+            f"Adding collision mesh '{filepath}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+        )
+        moveit2.add_collision_mesh(
+            filepath=filepath, id=mesh_id, position=position, quat_xyzw=quat_xyzw
+        )
+    else:
+        # Remove collision mesh
+        node.get_logger().info(f"Removing collision mesh with ID '{mesh_id}'")
+        moveit2.remove_collision_mesh(id=mesh_id)
+
+    rclpy.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py
index 13075be..d35c6ea 100755
--- a/examples/ex_pose_goal.py
+++ b/examples/ex_pose_goal.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """
 Example of moving to a pose goal.
-`ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"`
+`ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False`
 """
 
 from threading import Thread
@@ -24,6 +24,7 @@ def main(args=None):
     # Declare parameters for position and orientation
     node.declare_parameter("position", [0.5, 0.0, 0.25])
     node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0])
+    node.declare_parameter("cartesian", False)
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -47,12 +48,13 @@ def main(args=None):
     # Get parameters
     position = node.get_parameter("position").get_parameter_value().double_array_value
     quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
+    cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value
 
     # Move to pose
     node.get_logger().info(
         f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
     )
-    moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw)
+    moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian)
     moveit2.wait_until_executed()
 
     rclpy.shutdown()
diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index 88f64c1..c5019b0 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -6,13 +6,19 @@
 from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
 from moveit_msgs.action import MoveGroup
 from moveit_msgs.msg import (
+    CollisionObject,
     Constraints,
     JointConstraint,
     MoveItErrorCodes,
     OrientationConstraint,
     PositionConstraint,
 )
-from moveit_msgs.srv import GetMotionPlan, GetPositionFK, GetPositionIK
+from moveit_msgs.srv import (
+    GetCartesianPath,
+    GetMotionPlan,
+    GetPositionFK,
+    GetPositionIK,
+)
 from rclpy.action import ActionClient
 from rclpy.callback_groups import CallbackGroup
 from rclpy.node import Node
@@ -23,7 +29,7 @@
     QoSReliabilityPolicy,
 )
 from sensor_msgs.msg import JointState
-from shape_msgs.msg import SolidPrimitive
+from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 
@@ -131,6 +137,20 @@ def __init__(
             )
             self.__kinematic_path_request = GetMotionPlan.Request()
 
+        # Create a separate service client for Cartesian planning
+        self._plan_cartesian_path_service = self._node.create_client(
+            srv_type=GetCartesianPath,
+            srv_name="compute_cartesian_path",
+            qos_profile=QoSProfile(
+                durability=QoSDurabilityPolicy.VOLATILE,
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                history=QoSHistoryPolicy.KEEP_LAST,
+                depth=1,
+            ),
+            callback_group=callback_group,
+        )
+        self.__cartesian_path_request = GetCartesianPath.Request()
+
         # Create action client for trajectory execution
         self.__follow_joint_trajectory_action_client = ActionClient(
             node=self._node,
@@ -169,6 +189,10 @@ def __init__(
             callback_group=self._callback_group,
         )
 
+        self.__collision_object_publisher = self._node.create_publisher(
+            CollisionObject, "/collision_object", 10
+        )
+
         self.__joint_state_mutex = threading.Lock()
         self.__joint_state = None
         self.__new_joint_state_available = False
@@ -208,6 +232,7 @@ def move_to_pose(
         tolerance_position: float = 0.001,
         tolerance_orientation: float = 0.001,
         weight_position: float = 1.0,
+        cartesian: bool = False,
         weight_orientation: float = 1.0,
     ):
         """
@@ -255,6 +280,7 @@ def move_to_pose(
                     tolerance_orientation=tolerance_orientation,
                     weight_position=weight_position,
                     weight_orientation=weight_orientation,
+                    cartesian=cartesian,
                 )
             )
 
@@ -263,6 +289,7 @@ def move_to_configuration(
         joint_positions: List[float],
         joint_names: Optional[List[str]] = None,
         tolerance: float = 0.001,
+        cartesian: bool = False,
         weight: float = 1.0,
     ):
         """
@@ -303,6 +330,7 @@ def move_to_configuration(
                     joint_names=joint_names,
                     tolerance_joint_position=tolerance,
                     weight_joint_position=weight,
+                    cartesian=cartesian,
                 )
             )
 
@@ -322,6 +350,7 @@ def plan(
         weight_orientation: float = 1.0,
         weight_joint_position: float = 1.0,
         start_joint_state: Optional[Union[JointState, List[float]]] = None,
+        cartesian: bool = False,
     ) -> Optional[JointTrajectory]:
         """
         Plan motion based on previously set goals. Optional arguments can be passed in to
@@ -372,12 +401,15 @@ def plan(
             self.__move_action_goal.request.start_state.joint_state = self.joint_state
 
         # Plan trajectory by sending a goal (blocking)
-        if self.__execute_via_moveit:
-            # Use action client
-            joint_trajectory = self._send_goal_move_action_plan_only()
+        if cartesian:
+            joint_trajectory = self._plan_cartesian_path()
         else:
-            # Use service
-            joint_trajectory = self._plan_kinematic_path()
+            if self.__execute_via_moveit:
+                # Use action client
+                joint_trajectory = self._send_goal_move_action_plan_only()
+            else:
+                # Use service
+                joint_trajectory = self._plan_kinematic_path()
 
         # Clear all previous goal constrains
         self.clear_goal_constraints()
@@ -788,6 +820,77 @@ def force_reset_executing_state(self):
         self.__is_motion_requested = False
         self.__is_executing = False
 
+    def add_collision_mesh(
+        self,
+        filepath: str,
+        id: str,
+        position: Union[Point, Tuple[float, float, float]],
+        quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
+        operation: int = CollisionObject.ADD,
+        frame_id: Optional[str] = None,
+    ):
+        """
+        Add collision object with a mesh geometry specified by `filepath`.
+        Note: This function required 'trimesh' Python module to be installed.
+        """
+
+        try:
+            import trimesh
+        except ImportError as err:
+            raise ImportError(
+                "Python module 'trimesh' not found! Please install it manually in order "
+                "to add collision objects into the MoveIt 2 planning scene."
+            ) from err
+
+        mesh = trimesh.load(filepath)
+        msg = CollisionObject()
+
+        if not isinstance(position, Point):
+            position = Point(
+                x=float(position[0]), y=float(position[1]), z=float(position[2])
+            )
+        if not isinstance(quat_xyzw, Quaternion):
+            quat_xyzw = Quaternion(
+                x=float(quat_xyzw[0]),
+                y=float(quat_xyzw[1]),
+                z=float(quat_xyzw[2]),
+                w=float(quat_xyzw[3]),
+            )
+
+        pose = Pose()
+        pose.position = position
+        pose.orientation = quat_xyzw
+        msg.pose = pose
+
+        msg.meshes.append(
+            Mesh(
+                triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],
+                vertices=[
+                    Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices
+                ],
+            )
+        )
+
+        msg.id = id
+        msg.operation = operation
+        msg.header.frame_id = (
+            frame_id if frame_id is not None else self.__base_link_name
+        )
+        msg.header.stamp = self._node.get_clock().now().to_msg()
+
+        self.__collision_object_publisher.publish(msg)
+
+    def remove_collision_mesh(self, id: str):
+        """
+        Remove collision object specified by its `id`.
+        """
+
+        msg = CollisionObject()
+        msg.id = id
+        msg.operation = CollisionObject.REMOVE
+        msg.header.stamp = self._node.get_clock().now().to_msg()
+        self.__collision_object_publisher.publish(msg)
+
     def __joint_state_callback(self, msg: JointState):
 
         # Update only if all relevant joints are included in the message
@@ -873,6 +976,71 @@ def _plan_kinematic_path(
             )
             return None
 
+    def _plan_cartesian_path(
+        self,
+        max_step: float = 0.0025,
+        wait_for_server_timeout_sec: Optional[float] = 1.0,
+    ) -> Optional[JointTrajectory]:
+
+        # Re-use request from move action goal
+        self.__cartesian_path_request.start_state = (
+            self.__move_action_goal.request.start_state
+        )
+        self.__cartesian_path_request.group_name = (
+            self.__move_action_goal.request.group_name
+        )
+        self.__cartesian_path_request.link_name = self.__end_effector_name
+        self.__cartesian_path_request.max_step = max_step
+
+        stamp = self._node.get_clock().now().to_msg()
+        self.__cartesian_path_request.header.stamp = stamp
+
+        self.__cartesian_path_request.path_constraints = (
+            self.__move_action_goal.request.path_constraints
+        )
+        for (
+            position_constraint
+        ) in self.__cartesian_path_request.path_constraints.position_constraints:
+            position_constraint.header.stamp = stamp
+        for (
+            orientation_constraint
+        ) in self.__cartesian_path_request.path_constraints.orientation_constraints:
+            orientation_constraint.header.stamp = stamp
+        # no header in joint_constraint message type
+
+        target_pose = Pose()
+        target_pose.position = (
+            self.__move_action_goal.request.goal_constraints[-1]
+            .position_constraints[-1]
+            .constraint_region.primitive_poses[0]
+            .position
+        )
+        target_pose.orientation = (
+            self.__move_action_goal.request.goal_constraints[-1]
+            .orientation_constraints[-1]
+            .orientation
+        )
+
+        self.__cartesian_path_request.waypoints = [target_pose]
+
+        if not self._plan_cartesian_path_service.wait_for_service(
+            timeout_sec=wait_for_server_timeout_sec
+        ):
+            self._node.get_logger().warn(
+                f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!"
+            )
+            return None
+
+        res = self._plan_cartesian_path_service.call(self.__cartesian_path_request)
+
+        if MoveItErrorCodes.SUCCESS == res.error_code.val:
+            return res.solution.joint_trajectory
+        else:
+            self._node.get_logger().warn(
+                f"Planning failed! Error code: {res.error_code.val}."
+            )
+            return None
+
     def _send_goal_async_move_action(
         self, wait_for_server_timeout_sec: Optional[float] = 1.0
     ):