From 9003ea942892ab8cf0375d87c82c4b53da1ceb37 Mon Sep 17 00:00:00 2001 From: nothingstopsme <50317951+nothingstopsme@users.noreply.github.com> Date: Mon, 12 Feb 2024 02:34:43 +0800 Subject: [PATCH] Addressing discrepancies in parameter definition between IKFast solvers and the plugin template (#3489) * Correct the inconsistent angle definition between IKFast solvers and the plugin template When determining the angle parameter to a IKFast solver of a type IKP_Translation*AxisAngle4D/IKP_Translation*AxisAngle*Norm4D, the current implementation takes the roll, pitch, and yaw value from the pose frame's orientation directly in each respective case, which does not match how angles are calculated in IKFast. In the solver-generating script of IKFast (ikfast.py), these angles are defined as (in the base link's frame): 1. the angle between the given axis (x/y/z) and the manipulator's direction vector, or 2. when the corresponding normal vector (z/x/y) is set, the angle between the given axis (x/y/z) and the projection of the manipulator's direction vector onto the plane of which the normal is the aforementioned one. Therefore, for example, if the orientation of a pose frame is described by (roll = PI, pitch = 0, yaw = 0), a direction vector (0, 0, 1) in the manipulator's frame will be mapped to (0, 0, -1); in that case, a TranslationXAxisAngle4D solver will view it as PI/2 from the x-axis, while the current implementation only comes up with 0 angle. Solution: This patch has implemented exactly the same way of angle calculation as done in IKFast, based on the assumption of where the manipulator's direction vector points to in its own frame. Note that this assumption is identical to the ones made in other cases, like IKP_Ray4D or IKP_TranslationDirection5D. * Allow users to change the direction vector defined in the plugin template through "create_ikfast_moveit_plugin.py" for customization There are several IK types (e.g. TranslationDirection5D) of which IKFast solvers require callers to pass in parameters calculated based on the manipulator's direction vector defined in its own frame; however, that vector is currently fixed to (0, 0, 1) in the plugin template, and this assumption might become incorrect if users have a different robot setup and manually generate their solver cpp with a customised wrapper XML specifying a different direction. Solution: Instead of letting users edit the plugin cpp every time after "create_ikfast_moveit_plugin.py" is run, this patch enhances the functionality of "create_ikfast_moveit_plugin.py" by allowing users to provide the direction vector of their choice via an input argument (--eef_drection); thus not only does the resulting plugin source file contain the desired setting, but the generated helping script "update_ikfast_plugin.sh" also keeps track of that argument, so that later updates will automatically stay consistent. Note that when not given in the input arguments, "--eef_drection" is set to (0, 0, 1) by default, and the generation falls back to using the original assumption. * Add a unit test for IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4D To run this unit test, given the MoveIt source tree root being the current working directory, execute the following commands ./moveit_kinematics/test/test_4dof/test_4dof.sh catkin run_tests --no-deps -i moveit_kinematics Based on the current testing implementation for kinematics plugin (test_kinematics_plugin.cpp), the setup of this unit test involves 3 components: 1. Dedicated model description/config packages that are created for this unit test, which are packed into test_4dof/packages.tgz 2. A script (test_4dof/test_4dof-ikfast.test) to generate necessary packages, including IKFast plugins, that allow catkin to run a predefined test set 3. A test launch XML template (test_4dof/test_4dof-ikfast.test) describing test parameters and what tests to perform Note that for being able to reuse those existing test functions in test_kinematics_plugin.cpp, slight changes have been made to that cpp for dealing with the following two phenomena emerging when testing IKFast plugins targeting robots whose dof < 6: 1. Since an IKFast solver in this case can only produce part of pose information in the FK pass, the current implementation of getPositionFK() in the plugin template will always return false to indicate an error, causing the failure of every test run. To circumvent that behaviour and allow the IK functionality of plugins to be tested without being disrupted by getPositionFK(), a launch parameter "plugin_fk_support" with a default value "true" is introduced for indicating whether plugin's getPositionFK() is well supported; if it is set to false, poses from the FK pass will be obtained by updating the robot state with new angles and then calling moveit::core::RobotState::getGlobalLinkTransform(). 2. Since an IKFast solver in this case does not consume all pose information given for IK calculation, an IK solution can only guarantee a pose that matches the given one up to a certain extent; that also implies there might be no solution (approximately) equal to the joint states leading to the given pose. So evaluating solutions based on full pose/joint-states difference will inevitably leads to test failure. In order to have meaningful comparison, it is desired to only verify the information that is available and expected to stay unchanged between the FK/IK passes; and for cases where position checks remain valid (such as IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4), that can be done via setting the newly added launch parameter "position_only_check" (false by default) to true to bypass full pose/joint-states checks and only measure the closeness between the derived position from solutions and the one from the original poses. --- .../scripts/create_ikfast_moveit_plugin.py | 16 ++ .../ikfast61_moveit_plugin_template.cpp | 79 ++++++---- moveit_kinematics/test/CMakeLists.txt | 18 +++ moveit_kinematics/test/test_4dof/packages.tgz | Bin 0 -> 46995 bytes .../test/test_4dof/test_4dof-ikfast.test | 45 ++++++ moveit_kinematics/test/test_4dof/test_4dof.sh | 149 ++++++++++++++++++ .../test/test_kinematics_plugin.cpp | 132 ++++++++++++---- 7 files changed, 378 insertions(+), 61 deletions(-) create mode 100644 moveit_kinematics/test/test_4dof/packages.tgz create mode 100644 moveit_kinematics/test/test_4dof/test_4dof-ikfast.test create mode 100755 moveit_kinematics/test/test_4dof/test_4dof.sh diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index 733b78d8e7c..c8ed06c6d09 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -112,6 +112,14 @@ def create_parser(): "--moveit_config_pkg", help="The robot moveit_config package. Defaults to _moveit_config", ) + parser.add_argument( + "--eef_direction", + type=float, + nargs=3, + metavar=("X", "Y", "Z"), + default=[0, 0, 1], + help="The end effector's direction vector defined in its own frame, which is used to generate necessary parameters to a IKFast solver of one of the following types: Direction3D, Ray4D, TranslationDirection5D, Translation*AxisAngle4D, and Translation*AxisAngle*Norm4D. When not specified, a unit-z vector, i.e. 0 0 1, is adopted as default", + ) return parser @@ -136,6 +144,9 @@ def print_args(args): print(f" srdf_filename: {args.srdf_filename}") print(f" robot_name_in_srdf: {args.robot_name_in_srdf}") print(f" moveit_config_pkg: {args.moveit_config_pkg}") + print( + f" eef_direction: {args.eef_direction[0]:g} {args.eef_direction[1]:g} {args.eef_direction[2]:g}" + ) print("") @@ -262,6 +273,7 @@ def update_ikfast_package(args): _BASE_LINK_=args.base_link_name, _PACKAGE_NAME_=args.ikfast_plugin_pkg, _NAMESPACE_=args.namespace, + _EEF_DIRECTION_=f"{args.eef_direction[0]:g}, {args.eef_direction[1]:g}, {args.eef_direction[2]:g}", ) # Copy ikfast header file @@ -392,12 +404,16 @@ def update_ikfast_package(args): + "\n" + "ikfast_output_path=" + args.ikfast_output_path + + "\n" + + "eef_direction=" + + f'"{args.eef_direction[0]:g} {args.eef_direction[1]:g} {args.eef_direction[2]:g}"' + "\n\n" + "rosrun moveit_kinematics create_ikfast_moveit_plugin.py\\\n" + " --search_mode=$search_mode\\\n" + " --srdf_filename=$srdf_filename\\\n" + " --robot_name_in_srdf=$robot_name_in_srdf\\\n" + " --moveit_config_pkg=$moveit_config_pkg\\\n" + + " --eef_direction $eef_direction\\\n" + " $robot_name\\\n" + " $planning_group_name\\\n" + " $ikfast_plugin_pkg\\\n" diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index d1f03ef405b..51c5623d1d4 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -560,7 +560,9 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector(GetIkType()); + + switch (ik_type) { case IKP_Transform6D: case IKP_Translation3D: @@ -589,7 +591,10 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); @@ -610,35 +615,53 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); - case IKP_TranslationYAxisAngle4D: - case IKP_TranslationYAxisAngleXNorm4D: - // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator - // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined - // in the manipulator base link’s coordinate system) - pose_frame.M.GetRPY(roll, pitch, yaw); - ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); - case IKP_TranslationZAxisAngle4D: + // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin + // reaches desired 3D translation; manipulator direction makes a specific angle with x/y/z-axis (defined in the + // manipulator base link's coordinate system) + case IKP_TranslationXAxisAngleZNorm4D: + case IKP_TranslationYAxisAngleXNorm4D: case IKP_TranslationZAxisAngleYNorm4D: - // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator - // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined - // in the manipulator base link’s coordinate system) - pose_frame.M.GetRPY(roll, pitch, yaw); - ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); + // For **TranslationXAxisAngleZNorm4D**, **TranslationYAxisAngleXNorm4D**, **TranslationZAxisAngleYNorm4D** - + // end effector origin reaches desired 3D translation; + // manipulator direction needs to be orthogonal to z/x/y-axis and be rotated at a certain angle + // starting from the x/y/z-axis (defined in the manipulator base link’s coordinate system) + { + double angle = 0; + direction = pose_frame.M * KDL::Vector(_EEF_DIRECTION_); + // Making sure the resulting "direction" has a unit length, as users might pass in an unnormalized input through + // the input argument "--eef_direction" + direction.Normalize(); + + switch (ik_type) // inner switch case + { + case IKP_TranslationXAxisAngle4D: + angle = std::acos(direction.x()); + break; + case IKP_TranslationYAxisAngle4D: + angle = std::acos(direction.y()); + break; + case IKP_TranslationZAxisAngle4D: + angle = std::acos(direction.z()); + break; + case IKP_TranslationXAxisAngleZNorm4D: + angle = std::atan2(direction.y(), direction.x()); + break; + case IKP_TranslationYAxisAngleXNorm4D: + angle = std::atan2(direction.z(), direction.y()); + break; + case IKP_TranslationZAxisAngleYNorm4D: + angle = std::atan2(direction.x(), direction.z()); + break; + default: + ROS_ERROR_STREAM_NAMED(name_, "An impossible case was reached with ik_type = " << ik_type); + return 0; + } + + ComputeIk(trans, &angle, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + } default: ROS_ERROR_NAMED(name_, "Unknown IkParameterizationType! " diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index e99ce099203..24c115db249 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -33,6 +33,24 @@ if(CATKIN_ENABLE_TESTING) add_rostest(panda-ikfast.test ${DEPS}) endif() + set(IK_TYPES translationxaxisangle4d + translationyaxisangle4d + translationzaxisangle4d + translationxaxisangleznorm4d + translationyaxisanglexnorm4d + translationzaxisangleynorm4d) + foreach(IK_TYPE IN LISTS IK_TYPES) + find_package(test_4dof_${IK_TYPE}_ikfast_plugin QUIET) + if (test_4dof_${IK_TYPE}_ikfast_plugin_FOUND) + # Setting a default value to IKFAST_PLUGIN_PATH for passing the catkin_lint test + set(IKFAST_PLUGIN_PATH "test_4dof") + exec_program(rospack ARGS find "test_4dof_${IK_TYPE}_ikfast_plugin" OUTPUT_VARIABLE IKFAST_PLUGIN_PATH RETURN_VALUE ROSPACK_RESULT) + if (NOT ROSPACK_RESULT) + add_rostest("${IKFAST_PLUGIN_PATH}/test_4dof-ikfast.test" ${DEPS}) + endif() + endif() + endforeach() + # Benchmarking program for cached_ik_kinematics add_executable(benchmark_ik benchmark_ik.cpp) target_link_libraries(benchmark_ik diff --git a/moveit_kinematics/test/test_4dof/packages.tgz b/moveit_kinematics/test/test_4dof/packages.tgz new file mode 100644 index 0000000000000000000000000000000000000000..5b89c295d4bca2a47e3d92d723e94ef5383d7151 GIT binary patch literal 46995 zcmV)TK(W6ciwFP!000001MEF(bK5wQ`8xg;SY@hW&pLX`uJbzHb8IE!oA`0Evol*) zS1gKzB#!x7g0!ufn*YAt4S+9Eq7^&dWMWq3K>}zr8vO(sP_y;ovp;Qkb_WCex3fEF z=l|8ui%!44yW8&V^mm|Mr{5XuzF>pTkJ^hDiHOaJu@`<2&m4a$;vf_rU#wSG{*^w> zR?J0g^zC3`%*{JT7&aGHaUJYs~*(r`Lbc?sj%|p6CBbp4IbzbZTDn4~~dM zGro&IwGW)&oqm7G{DY_upi z{;$M|xV`z0;pp<@{EYRQ?PhmtBaDJyxfO4&;FYb7jh8RkQQ(D+%b6+I(O>`C>1;ER zgkccH+-8o?M(3C8C&%ZpV=>dWna^)|wDFRewryClsSyx3vH4oW_8SXdY0QzkZz*wqK76c#q!p=VWKt&H=Fkn`JLr6vviEv8D0bJ5!=s%i)+EusW8fX5p->VlLKc{hvZc&k9; zDq<4J_zjQdOQ^%*KqGfw6^T<{*{?=?oldo&t8R}DuYNi?!+OKB<6>7yPP1{c7e5$C z6ann7n0CxjY+%lO&TXlV@R3FWw5P&e znRh22hL`leQ(4%-(dR#O)rV4dsm{{f0liAysVddoUCP1^(mDU3t3H&v(`rZeCf`dP z%8%9k9a33G&1mjZH~UoX(*_>N=!#sDj+(5p>CcN+8<_~2py>l}i&IG5=I4<0LuyC zeXx{50PR}GbALqT9mHwzb60)7AHO5^inb!;mNNn5b?0R3?#b1}$1t>_;ZaVo9w3M% zMn$RvCd~};6M0RlOvm%Z&tb|VY3@>I}fshPv&|p&ZR=5%dI;bulV=2gpNt;=vd#H?dI}XXIcjLR73+axq998@i)w;!|BF2VnqtH;PP( zLw!ryugG*V-Xr#$qv~+5QwX2jz%r$Q0~(Yk@}K$X&T5KTkU$5(Ps;3!IdC9X_9Lyg zp=e;(5Vl)!fNX$-%1HD5?`m{;()}Svlrpf1(Yj)!xw5zFZS(uzFGlD8Gd#L7&JIt9 ze@LSiS*_CdRqIGiD-2T$dWv)BsRw`sh6M84N=XBU=U_!t^>A0I~Z#t3!y!zAyEBf%}90dyI5;H92+Vc z^m@XGPnKI0g3`H^Z4A-n8 z21Zp-j6hK)_$t7kxOjno5pQ9f+yG)h^ut8paR~baDiVgnYtZ5d2s}2ctd)Nuihz59 z0lgt{GC(Va5K~rgpCmGTx)_eGQ1P9Ayt?=(3z;e#zB~DaH5zNkU6BC=PKhaPUrs9J z1l&VHB#w_ECR(dzwhkuBb^L4KdW{uMQN}uIkmeN4CrE+%U|=I71yhMJskjuQ)U^Cj z;jNHCmkVGE9>@k#2)?TYGKD~@+HpBXxirVa%d3;K!z)~Iy#c&%02mA0b2ZJhZ;afE zS_(lfry+``W^|n=j$Y8)yr3mE#$&6wMboU1HKn~oLqduqGQq_JlIz98ZRu-(Vzy=h zE;k{2fON->ubOJbr2wkMyiK6?`_bX4ij5>1EpMK#_0bmI($Dg z-jWSZE2L9X9;v7tRF1AdPtnxG^}w^XokarAg4;M~0VL0iJ1q)6K#k-ypwKIqs;5oG zy=*CajXs`Toq(i1V3e6Ge8w@Ro!oPTGWH%F{dn?^n!$lwl?SK&xPN{^%asIu|Ka@Y z;fES}f-Nd|XBA{kM-NUE4EY=|Ri4&@J5K<@wdprD8Qzk?mj|9Ia2{ru1V-|swE zySZ-~oo2i7_k)f7C>RIQd4AIb`uVzOg;M$FM#5S zrepI>qup#X`Dehe6V^IFC@l?UDbP7buYEb#0h52)>w%p{ocpVh{RF5Hw^w7@UImjH z?vo0WAW_fd;oUeQ!I~`h>_f~y1uyMx{#%~P&_NW=OJ7>0rc(~#M;y(IpQ&gZ+(9_` z8z%@|M7=}!?+zBhw5s9UYc?f-j0o8`J=pKy*{9GGSyaA%c>^abTRAl4WVUcN^~^ zvXpFCsk!TT4zvz#dGx07y4URV8!QP!e3LJH0zO9!2vE;A;B#6|JeT@KL)vc<5e^pl zUUj)9=l6E2sMBclq!M*U6zT?x-!xJ(m3IM)>yWDEOGea zh_Jpmbk?vEm7{$ef#}o-ME9u$qWe_~MB)2W8;LK95;al*g^M!L{W4^tTO$*_&ngpD zLecwE3&pdo&{DDAqA>kn>Eg~kp4IC=xeS~K^ii|^(;oENrS+fQ?#}c2&yze$)_-(D zg4_>y(~x#{AEoeM1;gyOs0LP5AP<1cn{ALq^@>Hg1_!vGy8@uzg5GcC8=)M4YH%nw z1_X@5|J^r@;uQV&)QM-wxM>AmOwb`z+A`TzTgLMFON~YqWi(@9*|Y`oG`q^`G_sQ#@X9!=2bDO}1i!Jy3ujN5}8>$nQ|PZI6{X zCZA%56(f$@j5pi}&3Lv@oi>rE-W{eH8D=yUd#urbPnT(5B@l`y%aDYGbaa09?&Q5} zXeRM2hzvACd#pHT`Es%t{=wPP=f6SqqyN_%^q%GaDW2N%pPgo}>ilP(SGG3l&Ow$kdEv`qB8AFEST~U% zmkm);VoeD#&5zis=v1Y6ui5GH?ewqiSt|dMc#q-+uH*mh^ilrN0etrVp5m#S|8i;S zFZcq?sAu{h&ykX}Wy=J0`XBtgqWqNnvHicie$CELFFvpf zH^D`5%o{xgg?|#W*RN^m4uUnc6rb?~)orjF(@o$zF^uEKJersmZy~q~tjZ%=>ek)Z z99%|BfDg^F<2tcEhJvSB+`z;`9f8kqZeq46z;VPw$)OC#widBT;W%705V)DR@tZ~? z*Q4XloUs!Zy1WNu+Yy-IZQP>=dCcZt31)Sgwi(h{%1uchu|+qEu-h5lC!lC24;mwh zntBi5d~`ef?@F)tRl=SUpCv8fnXlq2|`%UbiRJx^hf(j7I=r04I%PWE0YYY#FKJ&HaQ}F=?s?)tOhl z@}rU^vffcY7_@1gU$jn@P8=n?Ar)0i;gMlVW<#EHGh-8TW*>s`EjT)}09=BlxT;7y zt!U_OX-z!ESydxFmI-Rws%AKEbqhAkP6w)C(cw*r3ut2>+%eTx+6}1U@Oql!0Qxv9 z6FNIb7v>d71365jmSQqR$15#;t*keIh&ZVDTmMaCV!D{593*j=fZGLgmt}!`Mniz4 zLpuCvO+uaix49RruknjdjsD+h?{u^Huf2==f8FQ!?`fU|{hwF%X`G_t{)&J^mE=Mq zl%#@=e>>NO+w2z9XMp=|Dz-B-ZMx~Cm#VpVe6A`O8|V5_DnUvq_H#&g3eoX=JgTe) zlET-QW>kSyIyHq|qce1vktt^{@TY-+_$?ne#D6jQ^&V|BpVA(LQJmJplZ%>vnXIy4Ru8iv3N zePUfBt!tp2$aP4I2Vvks5RxA?{F5K|496%FL1nxWCS!0;W;}W&*!zzs*i5cnz{KHf#*s5^S_0IIGj!COg0 z>7p`uwRmBXx^Y1`UgFZ-C4>fca1zT)aKNj&fl=b6oiHHeX|$zPt$?-YI?)DLzr=Li zc|#)V;0cMRm#k~!JAZMMWD9Ntr#I9E#FO*gOnkfsQhPg!Y+DP?3;hkQO^`$uH_*{;9Q+^7xG{y< z3R@&KnOqsU-o*c7E~tc)cUm4eE;+p`#wIH`lmEon^z^?6v6c6gX|(LZ1aGHvulOic zjFhEa#Q;(rRugJTnaV`3E=;{x$@V2DHZKOFL=FX&ie7rHxDO|e%wOrcr+H50Zr&CJ zA+LhddyQ;7ImdP5HhD`U7!&QWk>Q%D{ECQcg@IVFwlXJ$b);w5BX{YDp+2UfUbhPu!ZC#j3 zsH)tNM+*DcJJ*~-*S3SvJF40lr>l*WcZ93e=x4~wAW(dp%@*uN`Qv}M7vq}rJ6qMYIYIugQOmJ^9yV_}X zVW8>GKRF%nO?QrgktHF#Cum$q#&UY&jG1F3uF4S15}S zgu)?3G@c3EjQ10(bp;v5NM~)`VadJT6p`GB+MS9awL3EE&hOh~kyQRtX2sh~B?taZ z@~6P754AVx#Q9)PRiwMi(_5&Q%IRXmyqp$|S$=i!XSt?<*E1Vq zMXacwwn5!Epg>>%IyV{vsENK{Rf+L7{UQWLUQmb&DzDQu2wf%3@HiCJHA=~pYI99q z7Kyj7j(LUxT`o-tG;w~rj#dK`bsHl{hjlG~7(@t?5pY6r(9MUqq6K}ni2RnZv?%8_ zG)l9OTMl~mGoSXU^Z%?_;DrxN0IbXZ!(>10|MjK+x7&Np|9z4t_5YNXJo@qc^x`4; zf0ALc1V94#Eo1;(#-MKa(8R&wbqBRzMqC8$4bSrJ-XT1(E|`iG#PuO8 zWFbBTM3#v75Gdtb#1gRQ^un()y|8FLWm*Zs|6<&nI{QyC`+a=66@Ap@|FsA0()pim zclY`H=aW1O>pwF7(IG?{Q-e3~Dz`%OO75kDfADMKg|xN+Q8GlSf`0Ubs=KC+TVE~7 zbojIdp-Z)jA%(LPFz3MOuoCx`Y42MK>?|L-F{vG^g69?CW-;}yymzUpwWR0t4oZ*G z!czre;naZ4s!WF^f8HaFs_9oL@V9dc*rp0fBx8`_{I=CvN~$yf@eQUyf3r`W{$G+> zyH+2y=f8R-{omc$=|1cKCwa>8UzTE(OVz`ivfIc}RsplMo9t+2`cq!|VI9;{5iM^r zD=XIk$9!hqI6=kIUTqSZ%q%KtP5Q;4yw;?3!-Vc!lgoA*U0lCYG37$vtGZn*E2pj} zK4y)HEQ>6cYbP(yVw;FTY1uj3v|YB7pJiS{rK)n2#+6z=Uwz@~6ug9`Oi#hVbYa3( zeh_3|OG#uYSu4!uGK38c{T`T?>{HdXFN=o=lF+7vIGOuH*DOK=ou)%#Y6ngSO$`1N zSYcLFc7s@fzt)70nRG7GoFuNfob$&DJ|NoS!ZH2D9`;)_;t~n9pcXdwt{tBv-7N7p z{d=9d7=08)V4KsAQlv%ndt{g`@=D0x;G$)g`%^pX^)cz4LCt)eJTbmKf2Yp=uXWMG zlBf3kSGQ9>|J5Emum3*D^CF z(w-QOQa2x?W>k4Pavy#)?u9v%Y`a^f<3c4`jC@wj zhg}u2c9zvPKY=wC+dL_l(^{MVXtt&H-Zsg6htFpQtof6h9cR&h%X{+9eN3nSTisT( zNdLFmJKIb8zli6=9m+7l*ohlrKXj91SKwT49s~(6yc4#Jd@sWh{3p1$On1e%TlDTD zMOe&LJK8|MJOMgtHlbQLP zJ@m_Vj((`>=V#-x{e6@m2HHCqOS{pkt9BB_P_5JXw*4YEZr;t82V9^j)3z@9}R*> z8-D2EZ8#Y_p>usHW%vXjiJuN6QOb^)j|NKoWTq#6GSd@3ndxDP$U$Ia^k`7@pUfWa zKbbw;e>!`(|77-X|HfaVE+Fp8rjz&j|9 z3(UxUUO@&W*%R zgByvT4mVN(%3~fY@ss36;-|rl#7~DCsQ~41m(A1QM&hTzjl@rY8;PF^HxfSuZX|vR z+=%u7dQ?sXsCp7T<}Q67MI*5{4A>L8P9& zjIN&oyrze)F9m4e?+oL5MDK9cc10**7XX1Y75*`nFuxgSg$2aL9JgMTS7B6N4zE~c z`ol6l>_RmJhW@*piwc2%=E~O99i* z!jJPP3QZgjJ4T0gnNYBOkD&*ThRTQHxQ9+GNmX15`WPcR>_a!PTTyrhr!BRcA1}ov zVMtWy5ofo>bb4&`J z>RH)T(_*avHAt~WIV4rtukQ7(=r*>yFJACC&)h3q9{+FU=V0i=va;PQ@)24*pa||L z!Pd-T-PG@I@1LM@!#kqek`>8mHyibMqr{wpOFW@*G#?hp<;eUK^gQ0?6Il^!# zrG8(9)NA&A6I|VX+HRX;F7>>=*KDh|-F`2tytUPAZogGs^-pi>01|o+GaTL`<)$zhridA?7s`& zLw{QxhI`vrZM69}<3VAnwVlTgwxn?(p$+^NYVIhU;J>%wJr1C2;RfH)pBL)y9fhY3 zeu<5J`-1<*-wZ0FamUs3(affRAT0jw&#Hkq+fp$070x@^{3{HZ6{4R!38`3ZDnwB0 z+cwfhAO~Ar3@-yTV57a&`Lo4K`=dQ`$N#=l<|lq|nN5rT+1_p!<9~Lymia#y^4ul< zryukw1XpS@#F&KE$#V zVI`x%DEvdGnYw#`Op|ncYC5-a2I;wz@Kj{izH1JiW&Y^}J@dr>WdG*7WPadh2L0aw zrl>&wyIW9wN&gq{l<5DvgE!(p_Qn@VWUe4oSX#=>pyKypwec{Ni3#pb)lEJF?#>y- zK$dR|DX_oB$I59x{O*-s!pI?m;b3M-N!xBMdhPWyEjha_9fj7f_0Fj)|ahPPrnBj7a{pbc?HPuUQ)UF z(YO}|$)!_w1*8HIbF&qbXYDVn1^O_4|6g=4Q zr*bqUxUAbjrsp3_4vI4wUj<%yG}c%9p>*S8juT&3y)gJ&WYwXyC<tc>oi+X*X{2k?N|l+hGSPFd3tQQ78vP%9W}X#{@M9he>OcY&w`2#%{c<> zgm?xw)`pokHNF0_nC>9Y1LTjksyUn81-4<2eRi9b2WoB^RMeQVt;^a&i2Vk$ce&R?!>mW+bToz&0EnfOKu(cmG}OalBMrvv#GC5AH6SSt zkVvd!<`d6NkS~_QckiJEOt_VB)@tI*U@3%pB|vL(c%{aDaCCO^e(zUn*+ruii&cTx zZJLa{h2$sG_|DuSp0*(LrYL4rDKd+>2Fn$y8!EApxmF6LYM2?iPnfCX^mh8dB6iXw zy}O(wfoF2hB@wH((EO!Sg?jE#d>TgAdR=l&GZB^fq^S?h%Wi7kw#d=uPpTOVuyumn zqRR_qnsYd}TPZ*F7wMGKQ!lbdSee5Fv(qK_?Dpj*Qj}Y1%Vz5TgYdHhlzEJ4kK7P_ zzwjW?InC!X56sa2P_pNI{^#u$6fgBZ3wTQUpA(pHvCl_ffi?-s8AQw zWTT(HfOdCPSE{F+nu0SUQ}hd=Q_zbwZa;a>r_1%F92)A_(6Z6->A~Tt_^@|=et3E| z1=dpH`#-elAp3w|R+J78vmW}wB#w;ZD7LbWwG28csaavB6|KsbKCen!rY5zF{z4PF zyt0wG#=s3utNJ`Q(g131^w30|)^-SMLnSS9+ z!M5jF5k(%h={ikB#ZxtNcNRaft)(I8L7!RYKZ+K1YWTYtfo7cl&F$89G5>eBxy=8z zkmu#!fKwBg!Wwq`tJPMcx%xNQWJRuk5=U}o zGmh)xS_(fBLsTsglj{nQPF-}o6>G$$4n+@GVf4$b;!1mUMq=z|50x^=!^cP{I1}w8 zYd?&8cGbp1L=Sy5HlHp*ov%!MxH&5VZVTn6>vPeMi}U6Y&l zl*70=Z!DXW(tIVsV2qYtpTWQmj?lp@fiowLu3(hVFb)V87b-%lzJfWXI{>Rd-^Zw+ zTxCE5G<3yC+#?VZR12tlQ}qidz_A0>r`NE$5JW~{Ls{i=Gl=W&VjN6eGVh4qX6czD zzPwOwj0tn0y-HL2$VG9v#>8G+rZ@^3P3-rxov0A&^EprYM>f({3=%$^bC=o%yW&~Y zz;^62UAY2c-xmr;Jf*Oea2?TcY$dq9hlMhC=Bbr_n^DwkmD=DkiIvDkwgyri@vwn!>#COl>J+t1#Rp{TEZTk7Ex*Bqr(V z>wj6+b+T?*e4f=?^4rTz-OgOiZch1F>t*kW6ka`35W^KROW0R~7(@8rFyZ~H$K>ah z*1B1BY6WLo9dE2vEj>OgtFc^D#u%l_&pF*X8!%2In*sT(_X!A zfc2?@z3N0(V#(L$x;{((FP7H2p-j0KG)w>2?Q{$FzpZYwvy}fA@fi7E?a%|ZdFAzH zLd~&uDgeTUA_5)2Ri{^lDP*?$lJ>+22BT0`RNUkv^o+{)P4Nfaj4dVrX-RDOkrhFx zS!hA1zLY-lYqL}qzW+LkLw~9qsh|d?&d82( zFiIy5eOfDAQ>lahw#Gm&6aKzIKPtrM+bWP}YmO@f?gYJ+SV2b0d~K62@V`0dtg@;E zZ|G=b6|~RxfjAl`mlZ}V=(F##h>+>%877b=lpfz@`$-fBAYiNu@6sItPjRz8S^tdZ z-gqE~X~LF5{2V@X?KlL=v|geAqx#B%?Vu?uRLZ|>!gF?gT}K_ZgtZOEPlbJsDr;lI zjcAIE3I6A^X!5Z-%kTakm-?$cb8hQUFpK_Y$`5l>_SOMFX2$>6>J;ODfGn`2|BHBv z^1r_0VLW#dqh~iufwpn1wcAQp#{{L3yMuXd=|m6Sz%GqbYK<9Pv4-mH>}DpkqbtEa zcdw$rhhHh|@4yX7=m-NZO)5MMN{Tvc#G=QY&bRFsCD&pLvdbvRK{mUG%ls8$G?Iq= z&gun`ITeu=1fokes-O6Rf^_8l@Om78sJC>#fr@YFu(*i8-8=MvJT_tE> zcbBJJ=~VaGxJc*CQt(*VGu!{GR)0QsAG7Fxr`0Xe|E=~i{?kI9oQlS>8A&+qr+AB? za{;=Av4;v;Zm#9CFeJ6GYbfg+-dwY6ED!AD^*&Q`WmMQWFIkq=TF~#ps+q28#fV#( znx%`w|8)%WY_R1cDzVUqrx>CWgoqdS9lL%@!LNl$~{b9A+w z<|`Odr9qTIU%tv}70tsxFHeelJhSNk@6jYl1Mia-HSVGbnnnLx-6H+p-tI2>|Ajnd z{$Gl0D{fv7@m zQy=9NHObtcCJq*8DuhK97jLQWf`h^p5lHTGA7(!q1fL}nJ@oW+Q*}NV!as?|vCzZD zYwgI}UeL{U5e+M^Qv1^NKS$Or>1Lx{eKpt}@MYt_ zeLEYzP@Hu3Ri;U|bk~Zvrf~MR2@69Gd)kea{#|ubEi)-s#eFH#_B+OxSUyktnOpyV z4-@cA|DV$R-!A-bN&gq|RK)*A*e<9CH zHP4Fo1k&=ldO-lSz(TC0yoFvj8=Xe;<);3LMV4~}(=4$!6sY5%h_@63@Inqbkdg5m zCH*@lhp=_QPuof~Zaqg(=DE>s`uK?^zfb$ZX zmjgElU#;dR{C_T>>2a?CO!DgMe9_BI{*c<`lj-M#L*nGJORbS^n0ck6Z0rxWIuM+qy0}CBXHgpIk z)!b%~Bx9LWH5N;9zKP<=yp5?=u$kUC2>mpQLdTaFSn0Lrrk{f0%gs^&RR`Hm|E`RD z`Et{$QT;*jW}~`XFvpjlOx)%!p!mB@HVkd2SdL z%w=v+S+q|ZJ@x*Td5j=?XwHboKCPC|LqE0rpZe<|`k*8)-EQsuzhaWl>Hg1yjF2#`WS$y<&r^i2@ob8HJJwg+)$i*}}_zw#N>b{|( z4RIhXIKunmkVc~7VIhtVj^C2oK5&#+w9cS$IHCb?9O&UM`+d5Ei$7D(oc93K`=gIP z08Bq)m|T}S#Tl{uwv@AP7H^4I6_*2Qh_#?k82}N*$#a^_oG_=fbiUo?-;a+zo@ae> ztL09;OJt6y7u_XSUAw*vfT6k$Kq7R*Yj=`x|8sV~|NY+4$EfgCbJ?D>Ac=?=n4*0`E$v%nb1iIqsv*8s$lLs#kd zYFP}VJ4`IhltI;NxjcbVu=g6Gw-$68ZD&8e(F>Sjz4EFguzGaWd%*EV^gbaGJTkXF z)qW8wtOab9#I&Bhqd*3atw&Z!Y_uLA~ElxY<^ zDGWYI@vI+(J`tip@+^-=7LkmVmMomB-0d95ZD!sgbD(hEs_pr!^6Zvst4ZL&Wm9o* zbhh{A{h`=De*gaH?C2P`brz=#2~#P>=x}0@6lnrijYI1$0T2!kq%ZV?i74Z7KxY@L zp`ZcEK&5D<6-`V{Uh10g#EJE_cPYJ3xG8!vy_Q6JGE2$G4PpdI5q&eICd{#JCZQ^= zSF4OD{Ly~3ihtqU!B$J~l-~1yhog9yX85~JL#?c#kMc@|PPla|?K`xh$58{#ZH86V z)I)*PxobPET6k_fwAKTG)UItO2NH(mf+fC`@c(SjEcuTUBPCA9r}!p!>SI>?pVoFu z+kcv^P6zG(&1L-W#XR4T!5@!kJ~DOk@8lBCObk`qlj=4uFwSTl2~}{=QhX2!Z0pMu zBoR28SJcYc1v0fE{{0{Q*JBTO`Q)Bi`ycHWCd)D(4yTn_dfp1r+BwT^Fy9*_Js~yR~u4p!zLVP1WxHp0`h6vY3u?0PcVksNq z)vMQ-EA4hx>;$L(|0z!LSg$ zDE49Ab@n;@;05;eR=uF(AQ+&G>rtj9c-4zs{K)R)1vNGZh7JgIfzKK7>_!QLd!8pp zDGSOZe^F)fQ%X?ZcLm+3Z*q7;v`xp_SB$j23kR+5$eWQ%%2OxtqF6c{E5o3U79S~C zNh763gx@9rqUYjSjo+behCeM~@XOB6)ahC1?0mVc`*1Ta))4wuHl4{vuVguNtfl%T^jHg5qmxG~) zdLdHk%fZOn;jJd)2i`g?2O9F!W|Uk;*J3dCE|p~i+rdCi(1iMTp+)%2P$VouZ@_UZ zu`9X3z=hE@Xl(44gWpHuXiztT;)cRj^;Ob9s?Lm9gT>dbj=@0VLZ+iY1 z`SKKvhFF!>cx$uOB&oY4hQOe!Cdur8TEGd;Wo1V#YME|rlXPx|LaKL8or%Lhqicxw zU*QOX0VPS1yjMVca{8fW&p?S;bP7X`#!#Xc9{NmuDj_f_E;(px3YAl(Q26#k-42El zDj1FjPU52H8^O>dDBxvJl1^S3UdVi4QY~Y~GSYwu{xSz$v#AJ!AB+=c0B?g)2r~d> zJZ;%4#0JjH;{@xv;YAb!oC6+$T3)89en8O;@I| z;YEYd7`=bd!b(!3{ygCzm#SSm_ZcxEurILHTZNn#;Hrmt%CFQ$S8vw^BO&q+L7fQFkE4DQ-?BF9t$Z$eQO*=dsm_HywJjnxg zc{7XFlnGO&XN;r}nv(qLnI)LF?<1Jwz~2N@BpfbW(Dtj69~6^&TlqnAw^dAka~_tz z`BWHw3Sr~K56SZ5iDM6_B|D51K2h*E(uEDNH=O8+Rz+^A-8y@cay8QN$iEd=Ang+{ z81_lCJ!P43;U9p?%TW@9yfL@1VT&ViNAT@Upd45$bZ}d=8SCvDRJITtoxpig4~HuK z>Syb{`*~6?r>fG0j8gSAMV7T{AjfpFcY0RGFku-L0beXr@6wRc4ug+*9tZ9S&l;9o zxSCm!S{%X1R&Z{N@us@yQBqUN%PiiRjXvMw?WFvlYr^d-m0`lJG8W%<1a5qVj= zLjWa1O-G$^m|X|2q02CJ^Cw)FSrB+{PBEf&o(TtQSRCbl#~KXdM$R1W6)?)Y7I;EQ zoTE-Pi<>_iwT5w6301S%ssSqtoyCq0C3Q)&W%v!$x53e4iJF--1pQ*JBs{yh7Aw2T z!X-_4dL2z$5m3Bb6kpw<=-y#b;5RF7JiygZVYx>a8*QWC5Sor}h@5G8L*#$ue&~~W zLlle}8^Sg$Y>4x$rlzDDBBNsXOKV*=#GCA&N-iWwGx2XjSj^~#uvmr-ksXa2!aklh zgiXdbM4pUN2Zmg32y3Hc5|x9oo5n6}7WS}T%iNULmCG~|adf*!fAp+~4 zju!%(p}KVuNmS)?{@hAWp@0(tt|eN_W$47YA(Eup^h4X_HKRnXBPeUE~CNdo)PqJS?qiq%B1<@S>1C|*ag6J+n zk+~##@DV|iL%Qv&6ogJJJv?$Jsubr(zjE9Z4WLNWrl_W7?f)$5-Iu%n*=}!@?td=x zzbxia%H(XCog@mc(1Y!hA6hXFRYJeb9}eFCU|yckyQ(P0Pw|<_|GP0%2;AX4bU-uXKa}!6!~S2|e-`o-=NES@a*=854oRqHi8*`aqMve` zjpf-}U&7lB{VtAD^=YHKv)p!1`LXx^#NPi8*vH)QUormwHtzrCcB`|D|GJ2$cK#cM z@Z$IAV;242+9~pXTg&*b3wo+!$*Vk?WO9C zaqRc09>rg$bYr_>6?2lEnZ625l!6gYOfX`ozJL7j?a@2_kW(=@%)#d8%r2+g48;mo zZU(ArGF6a(3Wy;~==O`Pb{CfGw||I(t#<$Ke*o70J%)X4q&N53{d4U9?Hv>UdwU1o zw>zEgGXLX39uNICodF0`gYm$L<^PWHGSJ#jho=~CQ*;`wM(6p8a+zDJUMtU6Rxmr4 zqX@(rC-D%chyVET==AUaiusMH0B-`kmmG#`2T)`G{P^@&ok!u|@a+8P^yRUePHJ z;I^GU|3hqjdTRH-N~!h`ecXcoYqz$#9RI(&UPTmVv?2{j)%A{4w^_Z;*s%wHe(}FQhk(=1u345yN z5-_T$*Pc7#J3P{glpaQ|&v#)tILR8{W=-UX*|FzkXiPhSOzhoLbR=xJ==q>yb!^*q z(y?uI(6Q5T(y?vZHafO#+qPX(-?#V7>@(-gnlo$7T61$Q>ZUI1re6Gi&wDY^tsc{R z!*T*)Q9aJB&2j5&_Us$ScKdD6!@v4EnkwrMXP?r23KZldymbhlH;X77NBW#PS^)`B zVGxREZmQTox7qHdnrJSq1P>y#tflq%{kUf+`(z!PW_DrBr?-G7XLV?O=g4eAEKt)U zfKxPn-P=2uRoXlM`O{k;SMhpl00tWK9gCQJXuwJhrpN-6#!h3wdiHN>Ab)5B*HKjR z>w^5ORPNkN{YZR6`kuPFhA`TvSgk?Cspg)8fG;Xj)WIB|0~?p~82!+@s~V6zH})O>Ax}%KH4YsQyU@w{ zK*ewy{J>D}k)^UB-hwt&jXmFJRh(Ua5^Db%YLLU6Apga?`9NEt86@6x|D!J#zn=@8Lhz%%(m z>tQ=3;N%HS05FjYSf9LXiUSBfNqqWPQ3L3YsAqjLR&cq+#y`=)!f5J5Nj9XoadL_- zv@n+ie~`BQn5xgpP?~2)nHpgf2t+haVp?i=QzH8{PbRk6?*T_;s^o;h|9##Y%iVLb zE?-MbMQ)wdJQ#zOWlz>Z13$3FdxvU$Z-n}fl3ip9?I&!aveqG&)mjl2z2xo97{r_R zkRM}1huD*n##PmoB-`e>$SfWn%6%_&3?Ts+!TSk}acA?-p)oIYHv}#G_&Wo%T@wY5 zg_{)qD;Yc^lk#1&Ft_J@WbdIixR=gwW9uY8wJ`WsZyCBM%s4_Q4YV8aP3|&l+(Rd{ zdy$m2k1`#p*&6IkAtftPuMLU6fcc_0_efQCj2J5ir7l$a;v#$25otm2nnJPA>D3X!2h#%XA++TS#QCP*LF`0L!p$!4GOWCO-=Lf~%Bc zi{_Wwa;vBTB$0D6wi)s~*=%hy`Jpx&vC==|VJciF*4YQ;0p9=Ms&?o^Vhjc016nXRk2b??07 zADNnDuE0w}WNVa4DWv1ys_QqeUf)u!?<~o&+9XblYfZ_zh4<1O1UIUic;p@a6w+`m z(gkTRp%mw3EoSg!-~XZUeD2717$P`CcRh1nq+~T#wFPpFdu!Z;Fxb911^bPu$((A$ZV4MK0D;OW90p5r;!d>@Qq zEJ1M*$MVUpbtG0n|I9=-y6prPeqM@lR)2=gBZ9Xgai&AY$z9#SxpTGF@LWU6O*5qo`+!Y zgI>qBVU|7$o|Oh&qBtlXAzC$6+)V+yr|?tc*P$B)*Q+n+u~xp}tV*KgN_=fPvwZLz zZ7o*+#F|aWV3QtrD0{wXg`wmmhRkNuCPhLa3jQ3a&bq&b#|M6m0uff&E=9ls#*?ZT zynqQrOyzpXA=0H+y1<0><<4~FJXWjJ9th&d^$U6t3W%jJFe2f0 zS)ZH$)U0lbf7z8Z0RL<6u)Y+OMnA7(1Tr^<0Wp}eM<9|2U)X#g*C%W)8r!|?9`I*K z*XLYR4$~8+^5kM3BOjMrh909f8D+nPI=tY>83*3k;dG13^ zPCH26gh=8PY9Z0Xl8Yi5<5km^eE3(z-+5HD;kPm&{8laHBa+NDDAqacR%CP>P-XEe z0*|TpOt&7Z&D&KMIno^ez!hm(?;(|k%~gRum9zEl9T2!4?PX990t@f_s$naWTs- zWt|Ljv!sjZ9{H=*9kgbhz4%0H>d5X%8ayxGQgyKdH3D;UU8W+NoRdE$?r~_;lJ;#g zr4KL9ie_N20`o$rMf+xh5H+5NPCV(k;;7$1?tSeaQmu z){dUTZK*Q)kD8CAg591ATP1Hl#2STtthEg~F9i=k+P$q%h!8pHe=zl9&!QaJ0ymt;7KU|XOjL{lq> zrchKejD5<^s%J+HM6GQ|0yxionNm%gRde=AwfqMSmOm~y(hmdb`z$kZdT=6ave70i zswREd)|EZ%GK{-KjAAN_jC>p#_UYd2+!eN3V@@A@D%LGAP6Z?b$6o+&?(!RX@TiFI z?SySo^|ZgMIg$LoVQs)NqyAo3OX;-hoP&%ajmVU`$mh>(s1tBco znEh8l__17OQw?cLeDVK>f>;czb8TAFmG6JvB&Po+~IZWyVnEi54a!W zbl@on&%8SgkLOf{3*hUM*AG5TY{jbxtJ1zC10Icy*+mJ8rY|L~Oj20-NQkM?)Gsj8 z(YiFt!OY@*ivWqlB4J23!iF?QLk!R8_fN76S7Ptc@W7YFuiNPhsA|xY-4y-H?baim zwzIMenXv+*5#JVCy)Fb>!K(uO8Vv^{CMqVl9tXi?;>-Jr->?{3dFLG>KDCaQoJmna z=;y(BZ0UwqK|fDp!kk!Ga~dSLzs3Bw0s0R_`Pdz7sFw+U$k$)R}P#KmJE z#r_2k39J2|B@H3(3Z_WU|B8q#eoty$!v6^YA)Pj+W%FXrR4DY~W<}y;yaFDVM>v)t z@eQm7<`-naa0rO56#;T&1Q%A+iQ9PqG!B@}{y00Qh^ z03qrD!cRj9rpg~i;F!`ClOMN&3wZ&fOT|ZCQyZ`2e-$M7!y; zxP|OLc{C~BKwIz8db}#vcyEs8)~>GDCYRH#ifQ8o`>VKXLEP?E$dC_yywPhGs+tstBW=k2bzesB6ZUKi@U8kT1e!Y1y$2WrfcVcqfrd7c!+-2n_mo#xBr;(e)M4enn!RM(BIL7I7@5GAW5w)LUiL z#?Km{edW!|Zt@EjOY!_*&(h%zc-W9~qp7#rW`~+)0X+C;yYKnb``qiY2;8s+d|iX6 zsOADT>8bSQfXfQ*H%mlRBH|vo1oUrWGZCmTo*ZQG&n=`s-S8n^A#C~>-I22f3 zHR{xIRP*ISpK|GE`xqysSM!CdsDu_TDc1l4e6q4flZu7;>)t^z$sfvkpe!9JsmQbq9v!59otGd%Uf-3VwY7ulLtNp_e zq8>RLaILMbQOI-m#FEd>xCwV5l4ep8(>dJ{o5qFRyh<|l4@v3ByjLAZeOM4k93N$P zaxe_%F0rStPzeEhvO^kv8U#^~i{4b5FRA+j%W919Ul>}>=Jerx>Af9P=X>z;?I6s^ zF5xKXP5Ne0-4Zyb>5a-JM-8^{+ywO;A@X5S3oxLKXm=QY2HO}m=V{@gQ4@_Fnaz0o zxet!+s93c#a8q_y7A3O@bgF}fF8&n z?v48U>qt(nrkk#qcjO-AwV=v!%GA}*X2mFH)AZ0UT?Z1Kz1X^fHEDA{V)7%A4)HnT z-Y`A9O{)T*`xJ>u3-UL-5oedoMc#$#8Ho zl=IZr0Y}t>h{Kyr4|7mdTk_1m=`Spruz7kgGwFDGW%h)4Sy>(1?~B?lvs8nIYD6v> z6r^^b+Ny23~romUJrZn80SwCt+v)H7r z87QT8(l#8W2FmH#d`?yKvU$;Rf@lBgrTmcZa1KfYDF!DCIGf zj*1I-fnHvfSF0*;vqPAP7oT7Xm*iqvZFt{hDx{ptaI-Dc<(U-h&j<9Y-2S@$__%qn zhOI0bc=j=$7kUEit)#*yFy(&;lET9tR{tTjU zayKD$jPk?HRDX209kY4>w-80`V%R|b&GkbT5*QH9q-rMsE8Y8WbMm;Zv$U=_me0O4 zf1BwBl5jrjdFS-d@Dj_TD>`pO9T}0CfFBK0K4|n83pcLJV3S;cjP?;wt2BTO z@$`5%XbA;V5c|c8Mn#plki|z>W%k*A%TXNNDhVZ& z@4A1PTHaPhDa|f@^btvp`#$MBX0s4_Sf2nc&FhDs+ZIhallYAQ7v9aoFZ}_PwNpEa zbo-x}%gQftSoxNIsXsM*ozC+2t29X&oT>iwz!vD+qHC;Gu%Z|Vc$6jz5&RGr?oBhN z)O!DQa2N^i(opC!nx<6}kEAx;$`N)a7~K_$7iibuvU8C1jTEXb1isYJmu(+o=qBBz zreL=rk)EDNF74rgg1W}0m&%oMzR-xSB<*nwatGzzFafclU-~c6{=Y|wvt!{?w9wre z_V+b+Jw`Aaxs=?*zv-eVMq|JW&zFV9!pp~+lo&*QF*s4%nX-uRstNJ-Lmc9e z1{^0hk5+t-z%MmdTktTWTwf408o%^MXYgVW5eqEt7)?rs&`%%CeS@=jcZmlMnQ6S+ z!ypGE*;Keowe&R!hVXHAil;r6L8TdU!|t3*(ICy7Y4;*qhzV_63d=^yz%a zCZn9wG4${6U5Z?L)In8&=b_O?w7rk~bcwC3c4~=oF0gs-qe*(|Y1$WQV_xy-s$QnV zEYC^q`gRGtXAyacO#0{fnBc2DZc>uNDw>KGIZoV>WF2#TOGclk)4^BzZMDun)sUG2 ziGymzHq4~?qbNxJs#&aSFl!~L3f*e4n@m_Pq8&wEL$Pj7!|MIENI4E+WkWWKIq{4{>1H+1GX%G*&YSuD187hV*1u{LgU zG)h6ST7IQBA7^?K{I>A`KkMyHsIB2}N4G9@%+zFZJf%v>XjH{d+)kb zdTiI}hp_PrqkNhyqTiJ|^zm;7^k@pla#E(yN9#7KqC*sQ&$VAJ%l-B9M4oMH+S8dA zc%=gRg&Ji&icA;B@}t(B56S%PhFksozWnKHr?y}KU60rPp47hDiO&>*w zXi%3}{7;r3){%PxQ5poo`7LcE$9x>?YBk=$4}UzycR=7*^Iaz6D_YFW{4LY4-3kGB zkd0CMfoc+lXWU)U&_rMPKor7IcfhBb-PT2d`_C}hLmIs!Db$)n$7|pfZg>59+MGV!Ju^<&Y3=z!SXAx6P+sWst>YPacZTjgHcM zX!&ObLjHlaxsVGsIroU^^Y657;ZX7T*ln#&B9;=8djAg$s5n=?8l2MNuf?jL>Mb=I z2&R8%OS@UCZW1-zU9@};y}%p5x+*{rF0SqpX|u+D;9{P1Xu%S|d6>TYESyarT>)=n zuN&LM-ntjHhJ3U}5>#e2d5mC&Khdn1aWH4;1L+q^TRW4d`(o*atMe)y-5e+8>VDKF z48O}xCMo^e?=a9(w-oAzt1xXVM<=x{vdq?VAN%A>Wq%M9vLVQKo}OKFNUt%FOoCK`^m?VqP-8;^bnsDM-2%@TyCo9H z5IPjJ7g{1H%rQRgoFg85JlcWxotSrtuRhfERW0sfX`TVoqF|h#Bdl=w$h67Wdht%# zN~8IDfH9JN!O|LEre2Dz4}J^vS<{nTS+7Ku0WwFQR4&cjvhNsbKRXJ#;2_6nNn$q+ ze@u9@LI04U_;Yf4SbXNm6zGEzWt0~ZwX7Z2+?ktEMS`QQSM*SH?h{?96F({5{$`oW zI|S!NdJ5J>?GW~6P@$!}9kHQEk?nM%O|RP2X5Q337~$)_5KVDv!ghe$>eZpDYStx4 zlbP`bh7D!SCZ52;J%w+|MJe<4F_5G6(5`KZukY_Bhd~FM7lK@!x=z0y%#=^rhE>axC58sPl0&Is|P-A`y)a^=Ws0OZ?H+8RgHP&1PpYh!a z+#?19QAF{_GTk6x!`3zV9f%^LzOE}AhRW@oC-(?dpXh3CAQhf^D>SCFCkcaDaRHa> zt2aS&g(mtY$xh=gRZdokQ+pi0@hQGWV}&gXide*HVkYj=0*jO=&5|4f#W_9G(9?k zUfpKqjoGNd{ zQ7**AGDmt3m~5;X#(Ag4c8jZv-dof`%nU}(ytDHUN+O#g<{iDE4>hnH6N5ZGBasjT zl}Q^&DVAg@HKRJR<8`X+iS<-Vh>X*yfEk4XDksk{zePO{+c#>gt_?bLMd#ShhwHJC zW=75`oJB%3I}X~zIXr!D7W?(%(n{eHa>QcU)B^9(9gbnnAP?btPo)x_j3=8zxqBTS zU)!_!ALSi3{Ey(VRwYR#pB#qH+DW*}5e3Q+3EyNAC|@Wgmru;E>Sj5w`Hd4woM&-M z`eKZQuap|Ym137TeLQBPo}36p&Drb}OXXfd7C1wZ5l<{ghWgo{R~e$vXe~6kVI2BP z4Hz5)#_~VcPRhFVR={gDFj*L$aVTYiuG>;$0#-)34>l;EEA%^;Mn z#n1y^mL|B{sF$5B=x8kZpM)X;xkp^d^~8Fef1+AhoT6PNG7e7Wosl12?VLnPHmC#@ zmw@Ht?y5V&H^sY~@0$E@_QsCcMnc}18Pmp+uVq;Fo?njeoVAa{qQI3=;_)j$8^H`2L5n&LpArK z<4laf+t~Ha9|!lF$T6(3MhUiVD_EUAti(*f{=`Z>QRZO8HE7~3E&Vm$IBo#sKs8HW6%l~}@el{VH^gR_V1+S4kXsk?` z%JV(X7D@yL9ex;`(th?%h=~mb)@`+4@xTkeF_nckP1Pf{&Yz9Qzk5Px*_x(r2Wo!+ zyJy`1tq)!hTS4b3oVf~IAQ2Kl)7fZ+dn0i(EL|vSP$B$;_|AJ~l8kyyiP73SsQd0` zZgBLm7NOdA>2kHVC*|mUkeW7P_P(0iiS9m=71R+*?dS>v;Z4OCsT+S3Ol{NFF2}OJJKC;>(>I$5zQlW6 z9ptJj1&Ec(@Cfkpl%gWG-q0G=mj{Z#x}i#RtNivH*61nwgp7b))*DP7MkCc@!-|@H zi@0(pV?UclMRX`$`|GIx9Tn^yOlNX&N}AiuAhyr#jU45298QozmY7+xOBO8LO~|ee zHRW2r0&@El29@yFH{5UD;dIjkRdDvv`c<9pEv{c1dIamis7#w)GHA$0O-;1s1GWV= zFQW6}8Ed(Pbj_CFJ6J_w0oiGJPVeo8I^e9rt`Mn)#$0)Y#rOn>6NKN1vk&~p;x_5D zRVZZBfX|f(n@itSm?ax*b;TG&gZrS$#{F*jEwo-E?X4(#WpfBMxv9?lIq9zXb)@p^ zg*wm=Re)?fhBBHzH~E4|`9-vMUlNmJVC#22r8YX~F)GzIx>W0*6SsryaN5`>7%cer z!qx2hiIy8A!NWr;7~!o{ot4_bxE9ivC&*?2t0QTi7W%cwM;8Pd9q4HJ9lo*zOob*wNqj7vbS`Sac%ZkBc4@W{tBbi+Q@KU#^f?r3rUNR-eEogaMwUH1Q~{( zla-S6(FVq-T%1F0Dng1JKZW8pK$}HpzZXd)ldgj}e&h=2J47Zb&=^Q3Z`=F6ndK7x z?r5wjb6(k|vkd?Bv!8u2%ahsk^&b{NLo@egC5zd{5Wn|Nk7^ z)pZz{gWqY(FRi$z`S0EeWJnI|u5=6jM-eWVve|cb;+<42*YM#}?vJ+tWQkusCm^af zfOT8?YafQASMf~y*bm=Uj5cS~~- zqQU#JO3g`jGeu12+uH8(jIg=uN+Xu$f1o^AaH?iZ@_R&xoOC_AWjr`=xKna=JMsb+ zGTicY^Tb>o&R!Cc%>;O>uyHh>{QX^QyYE@lmZoA!}EFI;OqsOb|d2j5L%E*ekX2qLmDr z(LP;9Sr6GuPI#)O*nQ#a=j^W2vBAx02S*yN@8143+~n`grCYh`_SPwriphHXZ+dWu z;*vv-dW;uNcsdm_knvphI^l0i0{D#II7GS?v$mT3mb`Gyx2ybyDOAUgBlv(PdU~t_ zAzPO>fbokLB%Rey{Yyai-@lQ7&e&sIrSxAytv_BzZYV`uzq=LMfYvb~?Cgwy2~w%O z#*{$WDSf+;$|J=p{Ldzwp}93|Og!?qa>{D*pCWvWss>nuFS`r;&lKVN#M-)VURUS( zDYk~`N?cl)FLSnX=pRjn1q?=BXkj(xmMO9+;us4>L$R8h)780D-;Y?^VKbZ@DEs@~ zZ3L9NSM57$s|h`te|!Lk9C)P68aPeE5j9--Tv}hEodV>|uz{zPG1*$lk>;30IxFVY zJ8ztbgte?|lY6|wmnDV1a|G#L^|LM-@j~BLaQqE+WBJe0t%s}&!~gjHj;dO)YK%(t6WiTia{PQwD4|kqOts7*jMe@il=AA!>`5nY0cg@D!clQmI!b z<*9qP8ObVD&LsRCvsYpBB~i-APfz~~Xefo!zxp!M8TM(q@^MMI)yKG_GI8LTlt&5D zIO%;FMXE~0q{*8sQnQ4NpGIi!LbdpX*P|MgZ50OT&-n%OiD<)_*+wfe&UX$QQucI@T!a_9^U; zN)!`Q^{HCrr}!b&S$X^D+AcON+sQ)_USG{hrJ^8XU%O{B}*!bx(*ms8WsnEy_gNrpX1;0AsJ03o~cvGiD#(0 zHJCmM8Qya(mSJy$1-uN|KF}kxp5ikOTH1pU^7>!Ey0)?i0B#OVwDOP8x|e^i>vasa zA^qAkwq&Gn5&V68@a7~nhkpp+;o~JZ-~-VX5G2paExW~5!cE*Tyn9La`2j`pfCqT) z7NOS%r~yf{^uHka&ZRsN%S|YG@%EtMs`#fX{Yivi9+$g(LK@srSw7plONx)ck2kmM z$I+@}-bV&zE`rDW_JeY(%iv5^A1xdD_SA^xgi2T_X)vAyK9W=_X}j_ynVi z9-S*pr|Xmd4A(59L(W`fcCA;{m#(H09FAp})vpi%P4TGvFH64ELf6)E>HNRA=yQm+E{HIH6s>?;DLy}B^Dz`+xD z5~aRvt#OlcbeZ1CBsFR2*C|p__fR6TmbpYhcrxJXkTBq^b!i{$djZiIxd-CtuqFFr zT~plFo{hR=mCm!AAztpTrVN^Nk+Ropinc2tw;9?y!EmFzT?;sws9_YrSaM|RwD${> zj}gJOzdL8E2WCtTPL_Om$NG%Wc`sJJ&_1a6ZX<9RzAmUoH9Ed0SCy*OP8))(LlqW0 zP@eVK#lARWl*_n1lhjcuv09@7IG@7GD(M=bOZ$+p9x2}*_8|~tUXNG0+dS}xaNqe| zn}+U|{h_PNSuN`|&J6_!sJQKcIp4zfe7?l~hzI4WNRP_^u(E?`LI+Gf2(k?u1va~G zV(t6uV08$JyY@WG&Xgw+t2J-Iij-BPkA0VS&|HSA;3ZvGk^3;Zz}M(sUw0h1yHs%a`}2$)=M zfkG^UqaT+Nvk{wIJ>yr~257~f`$&|-gh&F}IWSJmc3RlgwLt>X(FV#%NSt)k6iOEmMX9Se1z;G@F& zX&V*$PCzMJi?>Cwa;jht0@p&iY8wbc2ue_mHBxhHI>5Q&c8fAP6gmL0vYo?<0kfn9RY!cq3|X?L3sg8znC9?0$Q678nye_Iey%k|?&{_wkaMZ)4uBETL@}5;^gx$nWFKP$(L~m13 zD{trB@59(z{g3Z0>I*#fw}TL0O`Lr^RF{-lEjumEYQnvg-6R4uev+Fm453oFTJDqh zE8Ls}9E|}VCtSOiWMo4gT&EqbGbDwt8meGt@Nfi@de)1> z^@fT<6X|;sxmd#iB3A=v73E&~0_Rh)(Wqj$XUBN2`SKpW8@im1w4eWsL<)V^5sCjZSV^^Ds%U&3kS9oGh zFTaypd{jq6#3LMV=>YwHBb1nT)D5T-H!HXj;3H}9)lFPl6izR_SO3%odbpfS0>I~Q zz{YFK^X`p+XX)UU>Fy^Rzj8lr)C5ci6kF1>#@P^+V6sNogy1VAHcy+OFtX0_FNv2H zeac4Bh2(kMWdrIGv{=yHNMi<*U9;_$aFwfH`@7NtBn)VVpOCmcTFWHzX%z9wAWYLTW&8vNC6fdrXDG!7T zxcyB-4dqM#!^YA9KLB!`_j@iN9Mqj*q$O-e24%@@364dN&OP-8V_j9vgGoF5*-PrQ z#wgOD60aaJF-;Nrm#W^5kGV}=W(VvD*r^2Btf2o&3K4N$CR#-yo*zO?j@nYa&v$WO zej&fDU#XdN9O~@xyNjc{r`#xA`-Ia9-Uq^YV~E=XpCsxUc6HZxa7?|__NWPeB`>A3 zeYstr`(M7Vi>fSMR0f3YjnhNYWae9+mI}3@4I&3b&_u@f4zQ`fFvoF$$8l?asq|LO z_)WpqqCnOAWBETz3l)Pf6=u~tFv~dpc60H5u)oG*uhsy;+@#k`)5$n`(}*roVx_O; zK@j=?PU^NkFb~>VgDG3NnbX{F4L6bNY4gDtv5yRx3l?7svfX z&;Yv61x0VR2`Y6Y(Nk9u%)%HMj3Zt0<4WMK6r66M)rA}YqYgZx_-hvP@Q)pxa!u(t))Y~lo+bSRzbW@mb7xB#}K*txr)W8xDK|b)XxW`!S zWqHn>XH?xE{LauZaLqjH=tIxjr1fnXjDiKZGt3|yMuLCF*t~>E^ZrkiUwH#St{I?> zlj^a8Wi|*naS8tdWMy5Nd^`v7v3~)yi4N~~7HQsfIz^ja9}^+&9TF@J)D{oUO~JpO z9aI1VB~AvNmvDh~pfM0}AR2(c*Y`o|G7 zUZOq|Q9*to76N*}yDz~UxGy1cCzN6(G#dG5cDwqNj3TgyImI1s{^yg6Przz4;MUh` z!V6FVe(iemrQz8A_g^mq6vx)7t_guGmm90E$BYf9fF*h6uXXi?hC<+~*@-0P!x(6f zdvlpqU_&sguyi8|l~Mp`8uw_TF)-zET>4R%|t=HM#16Qmqv5?6>udk)_J#qe0-CnUf9UwFk2b|b829# zTsO3Ofy!;@a^a4RGcQk+poUL$ws=;)XvEY3|3@hO+#xzy&NFM3zc%gBXSj6H(aTk| zi_I!B=YZQi(TJ-rZ>Ggaf>l&WL?52}2R|1dI^l^VZ-&3~W>;^!|H!O`5IRxu-cj~- z4mhqy{-&q@bmhWQvr*u8)^#`BjSsxR9B3=M89)Bk%VRUHf<(%zuI=mdb{1E=y>EdL zpU|oPSc^hoTDjO=7NF(Uu2`$db9)rPRds~Xl6jiKXoTeu#_r5&(A@FaAI0h*k{4&W zle;nTBOGu}t}08D^;g|Et8s?%kETCnhij$fcN;rAKq`!c54w>DutcXGrom>Zg=lM5 z>aRb0X==TX^4FP<_HHdZ-7`WxVf>ij0qoz-oJzec_e2SBhtfMgZRf{eANyEYc~lSq zE*sbWJxrNtSxQ-g7jsv?WH)tG#Aj?~)7@AG2R34j5#DZ+lzVW3cT=61K0C8s;+=yEu~lA^rWJ8w3YnroEti9kfTYI9y*S<} zW)Rcpgc?cZePRJi&c#qKN@GRG-=sesfS)MGnXR)fNUt%4KaTnm#xva_w$wc6=&uD~ zCjjFueq-4Lp(gNZV`4HG`>B$A(!SOKgDD5kT(-YLbwE!%N6uZw?Kxil#PXIHFh z%>idukiXfXdXScww{{B?DUl~j2J%u%>^H=Ap6}s|n2Fl&Yc)BtTDD=N$~Y6ZDX792 z@mCOvp}3_O3V1k`1a7d-s>vY?G}dGlSEQ@4EjIE8n3^+_l1>GfyCHK!>woqrH4VlE z7+;QWVI@?&qf-^tkJ&FZ%EZmZ?d8Fd9eGE9>zmp~I6gfn+c`_*pXw2YAMM1JF=6|- zLAzZTQ4pPXCb5)nn29YBRfs4PM3Trp*~Z{})en1x88s%vRjBT>cqfOt3J~m?Rkr(X zS4a-2V{0aj_};G%%^4%#?Sm+$9A;MiCoh(*h8iKrXZ(|UWu6NjUp+eMvA+U4k=J2s-LzhxNXx=Uov|0mOaf1H) zpj)Uo(Qp|CmovyBW3FJk&i*+wFE1-Bb*r*HhW)C3WI!8!S`C&yL{3D+nBJU(m$Qu{ za5}MOJONK8$sM>tq1P0i5Yv#-z#7#(u+J;}*`{$KRTJP1j5E9;^ zcjQ82rdP)U?DNhR(4s@!5HuZmVQr>mCg46XH8afdJmTx^tGd20r&>gMXCum1@G6=Q zIN`tdU9QNE^A8Ws^O{)yBt8)s`sdlV0GsjWMK(=uTmR+BjSA<)!Jy*@xI>7ZgIXLV zb`u%~CFlR%8UNk;A&S%9J@1?oCg!kQeUIBq(R-u^clcZJzcAx5(yRZT8JDv;Ync~S zlFN#)R&YA=9Y3nRT)B%KM)Hr*UM-xLE0~UyZU{OT9sivpsIE{U>b)QXGGpEgM-m7V z2y%e8bC+T=%)ibUQ|9V_(i!KcT45T2?hp*_2XpD{L4RgXi8>KhCaubKYxw;DepwF`T`iji2)Nr-5C)Dg&VbN9>X!bG71o^@RF&;J`TdEID(bP>^kTomQ9*xO_v<%(c-jJA=>}xj;BS7{||Q@u4|MZ zyKpss7xMIDlF+ot<;HBu!oUp?im@;r0_^$k4tyLkdKjNq-$z_(>&tc&G`W6D80Awi_Ynld}yhH`7^bBoS7c^b`I?h}w5)5niUZK3gViSex^p zzmoymF0A(eE5ol6dB>7@VDA~>1`jAHJ@|SC{%>FH1BT%!96<^p0q_LAu1+}Z71ukP z{E&|!&(3>!AdIB`jfmv}v#For6#D>l`Y({%-FW(t4rK79@_M9SXE!`UAp7GS7WA zbDc~+p7lB~$LpRX|5t--db_;}sF}BA`~+a$1B-Y%fH`vC$U+YSUjrN4^hz?~NALB1 zZ>SR-5qUsjy5bCPIYFDYt(W@#>2J=UnY1~>nn zs%*syVZkN80&Qn})_~b*jeS+88X@bok2CB;cW38+G;?CFjE;5nKObjGE_e_Z%6zxh z5bAtR5gCn%Yf0wHQWrA%#>bok_}x$SAe4eE?TS%xJTTBpDa0Zh%P39}h7&_W7F}?D zE^Je$)h|3Z+K!uuEkE9V?;MJsh?uzhyT8M2h(JtLn#EdRlSM9Y-w`aWcG!YjqU_%O zhuR4Y@)y+}YusuIS4pSK9^kO5Ud7s0Eu}4lQM4C!a!<(;M;Av!P5PYs5R@N+Yw=P$h~xYn8->W2 zilv;EQ!UTbSkeO-DV`5S-K<)Z0}t%z9f96KlQ!C}|I;NyRpf-;Yf9fb?&yy=X{<6M+KDp+ zpDlOSbx$H8qjaJUYBsCRu2R9DxJ*(zoVVw%uB=+RHkT!8FU4Bh*(L3WE7HF#OnYYK zfVoe8$IJsueM+)IP`A{gHz7>gJ+J-{Ei+3pk)@(1Y$|^%x^cnL+Q8+3jAGFJ%5LA5 z5ZG> z@6F8lWvFsSFBHxV;+Wcr2$%>}E-*3)TU9HOj3OY)fYH=4fruC0Y((8wY-&^X9a&$l zKlCzc#-esC3%;eicCv%}w%~jVx@#P1mah;o zy$Y~#)W!p&5|Hoz0@^~eMsKP`cS)F7JlLq8{+ld$)raLTsO{*)aC8{LpYOsiALmY) zDkKY)=~C>R=HoTgM?jwv`X*t*0JO=jz?BwZ&J27R5;0N`+dE=gxo=U3o+K2r_RL$9bqanc z?mEcqAZJbTO*ziDCotk3d6^UGz_>lg(00nwk2`E|q zT{nYxH#3xMi9J!@QmQGVv*~*!HgwM(U;Z3~z@l9&bq5*6SG$ttQ>~M!(w=qH!2Vx` z1yu#H>ZA)e@VlIsLe667_M3E4-3-Hp7N7u>6xfKeeqqE7&hYSLe5y6Hj4GODp zJh;n;l<$C`Z<_<92n{2;frllUayNLy$Jo0L@v-L;{B*CjFus|J@Y_?#-18rF1d$J zJ>{)B0;N?E$u9@^;UHIdGt1q9nl35Ffhy4(`vZAb>9OZJ9&Aedv(3`EuG${ay407K zZ7GiHmv%(HpXvrXYz@2@SvIwbIZa*5?!8z%V>@p}hvGr2&=HmzFOEj%l#=j$Md(%Q zijOr1krDvUQ0chUrgvyd*>}^kw@hZg(gw2t1BD+VH+f$q#9Kk?8pvSdCULg{jduB) zX8Xfl!v-HRQkVc;BQnbbd-n(42LIb@gMb1(Xes{rMqb}P8MfKZ>OVNP#q;EG8t8YR zC+o98jvMH|E7+gjTZy0HAMV@KL>_OE>^+FK8Mt$OObYYEUk_B8KUxZ#R!t^-66fVCHlW;jQ+u%knqh%4&=WP4qf3ctwPAGFm6vms!*!N} zA)}0y>@UzCg8V!wccPA&*<~(sx<4ReF9kryY7w1Cd%a}rAR74jQN$VSSB8;B9X_L; zl_g(CBwy#OR6?zh(L}X9QoAej{7Kgmd3qL( z!#B=)51%{$)PCd!JXM!lafW~WhK)k)&nsQspx423BU?7Jq`B3}b?a<@wlU&Oi%fQ| zvh_;y__~Y0B_C|hR$~1?OuTAPoUIYXL?B(Vp+S0^>a_u-(U{e+vj1I%zv`TQ<=DD$ zV}#b$nmuiWF?B2hba&G!NA%+7Pv~8=ykb!@>s3m;F|dB5WX;|YsC8l5I%0id2H?>y z-N-C$uGM{-ht6)mMSk%%4c^*1cYgnT-hDqw926tm)aW%Qp60_!gjuMy;{IYFgNK8@ z+-aM$AJq-_=83<)1J;6Vb3yp(p7XY@4q_W(EJQd;9#Y z951An0oq7^t1W)4qdP{E^l2zQCTy-y7^=SAn%o-7p?a?Gg5@J5$Fg>YpmtlG;4-W0 z%Q>&H_cm8T6zo2jei@8dLR6S~_zPeNJ3b_J9o5mIIuUyHaQ5>r{F`XOaM zNu*?SS8pSPkwq|*dtHt&cFY#yVNjNuskvBVsI%ZjbPGDAmbAhSvPlgT6*|(H#$@us zDTuk8w(f6g?q}&uvMRhN+}XCv?8ADSOaciF!az4@t@ZUkf7>0>H^8+ne0vhl9fbER z8mP4N40|Sum!37tZh+xlDA%H6$}Di#gNV;0Bb-5)i~2i8dv#e?^%V+=NV*UFHTLBD zv1IkdY!xA3ce@I+P*D%`2)I`gL_GuJ4Tuze{JRS;Etc)!YC z6=$0H9J*5AA;*cY<;p9NSYespH`M&{IRu$*hvc^#&S4SpbQ0{U&tYCnC4 zHjl@J<1*YXqt}K>`ebJoaZ%0jeT1ED+{DWeSv>0l;*EU*mkVS%La%qgQtRrk9K-oM z&_YuW99XZiG)d7Q3*NC|O)QL&%eI{J23`y%tR%%e^L6L}%v#@=3h;1&=j;$JybAjOEFXW zbOQF^=a3Q?2X*KImDxuT*QiES9Xxow4A6MZ3OZmYd(c1<;h2(!r3Wiz!G$TG&AzEY8p^%U+jd{lz7L|#5> z=)ZIn=jrn7fI%}{QD@A03lw@-0w3nO@%GSk~zcO7ph{LpMD!1;Ama7Z}mmcV(iMo?W=!+tE zH)`_QT}k`L1}uues4{;{wMQ^=lWSd=y3ia>!j37F_3(&KSZFXvXiYPpW=@(J^)QCC z3-zjz3SKfS(2BXN6>WRIG&2@E!1}gP*7T_1zF7lhGZ~+b%9SJ@zdHavefx{VSWL}$ zg*rKh38y82lNB%55qCB~5DquRNzP_n1$k$`t;R}T`Z^IRfLY7K3kH&xOBl8mYCN5* z#a~Idhl@lmR`@X zFe+{!l5yh57)fPExIL5eP#d{NT7hZQ!?Du@oq(x)Rg`pPXOLukKAzMaHXI@A4R_;BcisYYE z%5Jx^LWmlJ^oIIKpAokLdcNIP5yfX|YyC2rE5S;)c5bUtufKVyF7J1c!!>FuoV<9k zLiq6G8M}su@LKVrc*a7dfiK5`oR@=TrMX5XwEou%cAYW6iv+^PBgjm!1EqqF+db(t z-x09#!_l-fKDkCc?YgKt(8eP^>}c3BVsWHW=TVP{!nqgg;0|-l%I(-pgKo#T_waqY zI_-D>83NsYfdPK|`DlsLe7%Z9bLAti-h9xAyJXNbMWR9zho0^7IVI;HR>r%VQA;#NY7cM=(oQ5H$}6g6 zmLSiDd>w;h%vt2&gM%l@5J{#G@SSQ!kSdz8u5?F|?{_F7cB=IZ? zE4%P_P3ex}nySJmUXkVgcS8cG5=N+_BhpsC^)%nKqeB2CYD6N@gz>96CM-^uovvH* z8LC@Y+U?MjDc^lRB)2R^1Lo$zMULf$h4_p7aOvB@iAPHmj}@FH zenb}hE1%l@a6kSOCO*c?gz40-k#o|?(15HG9ZD<*L|EfQzvI|6c7?)YMRWIhA zPloq%BUz*HZW#8^?V2BNZ!fRk_$$t}O6JPD@=|#b;xuZ76RXTxyvU=25VodBW89B^ zsb5nBo$>aGrj){S%es*~&xOnE1J<{Qz1f0dXpnPwm9Phjq4V6X!&uy5vqGVlZ^$R; zL&VEPP28lGnd%mt=NC^?6brxJZxU-o%Vp z^C^t6b$f;Fo&Df{_-uqDQyw6n&F8K+i}%W>-jA#F>3biLeMKAktqRma_XD*D%y)--Xrl^fRfSmX+K1ZFw5(kR*SSWFWCrzLRi z_~rnN0#G*Dg|1~JBPr>R%u|Myd6>8A~sf6kPXL7DQ4_Sx(J{jMYKXrX*dW(B(m=hPjaslTF2!nEQn1W zbW9;-IOKr|Y4#W_2AdwRqkbIf5iz)>?-kn(l4rETD4p)x_vr^*>xC=*gF^Zd^H0^-@DOmEU2 z6>)N8Ay4x8q7A)J5ru3~6vY`q0S#x`yAIaQLIxD*^Up)K!L7kDQ5pg!*DpR4+{1BnR++{%U?{2Gx*{DS!Eao;CT)dM2j)Emz))`j;W_T_{%qM--ID;~J2VtcLwU9 z)ALy1GkvhQcnj4%N>FTwJ#_IX_!g{>vx5EQ+Y8|9_C@p+|A#9KuZgn5>4SG7XwDa}X(0uW49FDi~? zH?lbpNt+;)*WcFG?zz^aGCyD?ckGkxLiUfq%|od1cn>bsBMm|A^RmbF>H>l6NTARp{>2v54JTg2&!YMq!I59YxGNG6IShcq4u~x|v z5S*yUo>qZZ{bLgDGCWB+hhZJK4xk1~3l>4c(%sX8q1 zz7es@Ifk$^LTqU7+U%t)pI5CLmLMG2x_5meimvN8t|oR%2Mqs}yrmf1#t zMF0!_(APKX=kHs<1?z_Jx2%=1ioI9Ry%*Lc5SkL~K@cBb^b=CO2HeNK-#zWZkzN!! zrjBviJOrb<*<@uN41{d&k_0bga5xJao|go3p7R7Fu*61Ot+wX^x@wh7z3O5I%O?uuwe%?~>YS=*bB zbP4@IX}g}upkT4e{vU!xxqjv3rS5hl2XGq2KXn{8{CN%9+u$Y7U+g zyugHp@iZK*u>s`VN?ORjV?`Nx-Lj1`W9FDMrWz&{7`3XsD=6u%DND4U_1PWmcw4=j}>Vh-$`d>WB@XFP{47hcHE%44=a%)j+9y3l@Yhpoi*7HB1O1Bkb|@E zk8f+{cYLhH9vwZvIXwvmbk6b!1w$~an_6$oC(#g?p;|t?rSG=6b*TDH!)xPyt0veC zHprOc+V%WCYME=w*6eV6O;)tXQR8>(ZGxE31LqsW8H9v84dYjs>;4 zOfj{$-mHVQ8OqnrjYNoagGf^yyFCaVNi<*6RLk5LLp?sZ1&7+ppu0PNSbQe_sGedv zUQ8O$46X^ln&MO`T^>r;K`)*5s2>jLCO*|Dd%zhdEET!@CqA`Ux$WPPDWVtd1-@@@ z+qyUPG#Tq_ ziGV;%RQx)r@|nIpu_OGeteRg>dLqxbMtc{Xu_kbS*wF(`(({t~3nzsKRW~(Wg(6ev z-w8iYn%S<7_c|7#a=r%!N-;%?881szUhZ5Kdx59@QU4peec{K-AHPKV{!5^enq|$$;e{QAJ6LXB|l*BR%eo< zwgsa{K78Q0N9;SgTnh8@6il-xK&Q(p`&)IxP9=1|AX1K4IVh-}~317xh-0a_bhX5GA=S5(|xy?`bXMN*G35s^=^6XU)? z<==ObTRuzno0Bkzr6xs55vRpvN=j5iO5uBbTD=T}x7nt&o0ZgmavOJm%;YI6FH9s; zUE`eS=Jtw<7kvp&ZyH4l@m85rXF{LcSW@dN*@;@|bnwmrNZl4xv=LnP~*M}5>oIW3_c?k-<52@8%milRkWZG z_OVg^Z6#@SQCXtPZ8uLRe=?+}zLIhKf#Nxkb6H(T4|R={$snbRG-LsqnkBgxUq^Iu zH>S>OSg1F&AXwr|G;#aoP$McW7m^;mo~9BGRJ3E?o{jVY&40ZRtG{F!zZLcI4X042 znNWd_+qHGw{O-p1q2t##?WTwU*=`wMN@>YYL;H^wP9(1D1wbE0s<)p}+*uA< zl*GOVx_Elx0!>L30tIXW=o|if+bv4!a^i&}aO-ec!e=dA?1pmc`M+J7RsGkahLeNCqw_d9$$6=VQ&TT@BP;Spy{?3u1gT z2R=Z^^z-wn``SNiRvyr_zRL}ZH=&Dw``zG;n2e%%%hHi!&&LJpEysQ2heWk7;=08?Q5COV*a94ecO&Dx$x*6fF|nGgM3W zby8!!i(v3=T5b``i_ap0a(M`#M6=7(r7gACPjWSIxE%6F&br>2FC~8=?sC{qP%7{{ zXKh(UKZm=K%tbKKt9!9_p5eyx>d$PH- z=O~QOD;a&cV=VxTkZG{)3g+<0&QIxy`9Ni}MCj^$rsyPW;|X{xS2Ww_Vs7Lo{VAN~ zp1)I8XokO2);rE#W<6Dzy?5VPsDWi}D2s1)cjMe#eD>qW6QKQcG~x#vKfUd^(W>f_7n$fAPu<_$QP&UpV;RSKT zIeALF5*+(O1*qkDICXDgj8AP14R`uJ7X(+fOqT??uB?aj80`Lq-G#E~fzpq+8?mW0752y`NB-qdTTAYNiO?QsTkl7#8 zJh%aFM!C|BWEiGAYI`W_!SzEg!M0Y|Zm2;V`<=47DTEkU zbi%J&QC!ZMi0E&LuKsw$8|QWko+id=ohu0^`040tQ#eU^`#(%shhA|bwo|e#A^t;C z*6CaCKU3Cy%8zHW{bEpAn^Ul3iA6Cj^#`$fW@KT@Wjmj0(1O(SEXMX$bUsN1xOX_p zb6dZwgR*C-(DCns60G{K)HFq-L5O5w$x;r)wH){1#1OBWmH*uLckP4G)P!EE|kd z{)7ok&W+MS=ZJ*^2iWM({BXDJD;h|{h_tpeQx%h&TrmT732Pqu@ZO(E<3fD zwiBxT4>MMlf6G|CiqJOjof4FLwa94I>8k;vQG{z#R9ZzX5wKfe9vWBSn88m$#rob; zHEjc&t~7&0`tVqWyEQ#f+u2;P9m_KrbE?c#p320${7VC7c*i!Wm5sS}n-3%ndA!1% zuu4Ixz_&)cjK~(?MaU?Gi;|WGf_d(^_O0b<2hhvGPCS zxBQF+$?%;r=#6!e zKowwf2CrO2p{XyhcSo9xrJqL`P#*$-w-mShGMt(%-2{TNi{czm_hLVi_ZWa7RujR) z{wxE;9Qa41y=$kijx_yAveOqA3};&-r&bS}H>X);_}mBR5{DL7R5lSN{i5jZ2h6Dt zbhx4=LC7bL6@`TRtndHr+h_F3I0q;WK5uo+kvAa#_#G6qN3fv-%f_MwMNe6L&x|9* z*8$L!f|tQS;Y7lA{wJ~W%Jt71dpHLED%+Hl78T2heBh2Js6m&c4WWglOPa74eaS0G zs$61A(@FtON7%4>ACv(FmNA7X(jlR}xFMrcS6T2jfyhqkzKL8hSiPui8t^R8zNDv^ zi7A??0X?h||3mp#hk*Vf>bLQ{1VL9KvkW0|r%*#vm1qqSk zO{me6L(K18qobg=+wD1!Xjky*nCkWPc}^(hL(D$`ARLPY=Dx|W7bmz|`kuNLc@igB z^%__qQ;IK0*d|aUlJ0?O=<(C|pRdGs>x4YEhlre5qqCS2sjA4s-E^C*%tH3TC-h;mpTgE82aMDkO z`wqb;^Zrc(eJt3b$0q&ri#^qE?Vdg_Cg-n+U%NFEcE)KZEyPh|F%cgDBx>pEp@gYF zgOEhj3E+>|(@{JReEXu$dxzE^AGeMqYMvJQ_h{9aZ1INX?9>R}gpu|ndLlmGeoXMz zFs^)Wxc+kz*D4hIlgL;d{YFR^6dlN2Uapn!xO-hUH&0i~ze%Sa6vLv~gFJ!YhR34A zJqY)I`cL7@INeUf>>u*VBWZ~OlH zVNpjC;R?OKB{)V($hs&~-?uy>GnP6PK)L(R{*QA@z`_-u@Trb|*N`{%8K9|Yk!B09 z{^!p>T2ZpMg$)z&HmyGfsN_@{-E^tsnECGCbl24FO!&6>;*>CYoRz6XR1N#I`1?m# zxA&`aIJ4GA%2PO;-j2CI1I^bfH4%vf(veRHwIT}y38(k#IJmQ6W0`E?aw|W9W~e-X zW~e}=03n*{(uPd&M*s{ae+^NA2$lamM1?gGG(<&;;RN;2`o*@4Zurznos(_B$pI1< z8)TE$0ih>k9CiJD1cwKUkkhkQ>pE5aBJFr?qg&i0&GNkJ@kZ!w5+}yo`cSp4^(5JQ zD7fshOo;1gt1pxT5k)SX+=yGb!pD>P25UyRIWhLkJ?dMg%_VQLh*g}wy&<(SlN_3r!(oC3%iEh(DsMW|v>5%8ugfx{}L%JNkj(S?y9Z5V9GGM6sU$bEBFWPs_{n?~ad{h<#J zXs;UxTB~7yB0i1jyz|v4`#Rj-EBD|r?C;(z#a>-%am7x5C63XM`xGiR4_h^r$y*>~ z?eE=e?(=O0j9TPlJ9Bjh#S8D(`qZpkVDr1_5&Nnwv4FXSN3wkECnFnrL_XQ)%crp= z@Ust~KKTorR35cz()dF87q4bb+RoSFPThVbU4aqPNBXS2+OJ6yGhT0kW>VyxV2e;- F{{`j*OMCzT literal 0 HcmV?d00001 diff --git a/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test b/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test new file mode 100644 index 00000000000..a48776e1507 --- /dev/null +++ b/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test @@ -0,0 +1,45 @@ + + + + + + + + + + + + [_JOINT_NAMES_] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_kinematics/test/test_4dof/test_4dof.sh b/moveit_kinematics/test/test_4dof/test_4dof.sh new file mode 100755 index 00000000000..4c0cb99c1d8 --- /dev/null +++ b/moveit_kinematics/test/test_4dof/test_4dof.sh @@ -0,0 +1,149 @@ +#!/bin/bash + +set -e # fail script on error + +# Note: this script assumes that it is located at SCRIPT_FOLDER="[moveit_source_root]/moveit_kinematics/test/test_4dof", +# and all the model/testing packages generated by it will be placed at ${SCRIPT_FOLDER}/../../../test_4dof/src + +TMP_DIR=$(mktemp -d --tmpdir test_4dof.XXXXXX) +SCRIPT_FOLDER="$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" +LOG_FILE="${TMP_DIR}/test_4dof.log" + +OUTPUT_SRC_FOLDER="${SCRIPT_FOLDER}/../../../test_4dof/src" + +if [ -d "${OUTPUT_SRC_FOLDER}" ] +then + OLD_PACKAGES=$(ls -1 "${OUTPUT_SRC_FOLDER}") + if [ -n "${OLD_PACKAGES}" ] + then + echo "${OLD_PACKAGES}" | xargs -d '\n' catkin clean + fi +fi + +rm -rf ""$(dirname "${OUTPUT_SRC_FOLDER}")"" +mkdir -p "${OUTPUT_SRC_FOLDER}" +tar -zxf "${SCRIPT_FOLDER}/packages.tgz" -C "${OUTPUT_SRC_FOLDER}" + + +# Robot/IK configuration +PLANNING_GROUP="arm" +BASE_LINK="base" +EEF_LINK="link_3" +JOINT_NAMES="joint_base, joint_0, joint_1, joint_2" +IK_TYPES=("translationxaxisangle4d" + "translationyaxisangle4d" + "translationzaxisangle4d" + "translationxaxisangleznorm4d" + "translationyaxisanglexnorm4d" + "translationzaxisangleynorm4d") +ROBOT_NAMES=("test_4dof_xaxis" "test_4dof_yaxis" "test_4dof_zaxis" "test_4dof_xaxis" "test_4dof_yaxis" "test_4dof_zaxis") +EEF_DIRECTIONS=("1.0 0.0 0.0" "0.0 1.0 0.0" "0.0 0.0 1.0" "1.0 0.0 0.0" "0.0 1.0 0.0" "0.0 0.0 1.0") + +echo -e "\n\nNote: detailed messages to stdout are redirected to ${LOG_FILE}\n\n" + +echo "Building docker image" +cat <> "${LOG_FILE}" +FROM personalrobotics/ros-openrave +# Update ROS keys (https://discourse.ros.org/t/new-gpg-keys-deployed-for-packages-ros-org/9454) +RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && \ +apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && \ +apt-get update && \ +apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ +apt-get clean && rm -rf /var/lib/apt/lists/* +# enforce a specific version of sympy, which is known to work with OpenRave +#RUN python -v -m pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1 +RUN pip install https://github.com/sympy/sympy/archive/refs/tags/sympy-0.7.1.tar.gz +EOF + +CUR_DIR="${PWD}" +cd "${OUTPUT_SRC_FOLDER}" + +for INDEX in ${!IK_TYPES[@]} +do + IK_TYPE=${IK_TYPES[${INDEX}]} + ROBOT_NAME=${ROBOT_NAMES[${INDEX}]} + EEF_DIRECTION=${EEF_DIRECTIONS[${INDEX}]} + + + DOCKER_INPUT_BINDING="${OUTPUT_SRC_FOLDER}/${ROBOT_NAME}_description" + + if [ ! -f "${DOCKER_INPUT_BINDING}/${ROBOT_NAME}.dae" ] + then + # Running a container to convert the given urdf file to one in the dae format + CMD=(rosrun collada_urdf urdf_to_collada "/input/${ROBOT_NAME}.urdf" "/input/${ROBOT_NAME}.dae") + + docker run --rm --user $(id -u):$(id -g) -v ${TMP_DIR}:/workspace -v "${DOCKER_INPUT_BINDING}":/input \ + --workdir /workspace -e HOME=/workspace fixed-openrave:latest "${CMD[@]}" >> "${LOG_FILE}" + + fi + + # Producing a wrapper.xml describing the robot configuration + cat < "${DOCKER_INPUT_BINDING}/${IK_TYPE}_wrapper.xml" + + + ${BASE_LINK} + ${EEF_LINK} + ${EEF_DIRECTION} + + +EOF + + # Running a container to generate a IKFast solver cpp with the given ik type + CMD=(openrave0.9.py --database inversekinematics --robot "/input/${IK_TYPE}_wrapper.xml" --iktype "${IK_TYPE}" --iktests=1000) + echo "Running ${CMD[@]}" + + docker run --rm --user $(id -u):$(id -g) -v "${TMP_DIR}":/workspace -v "${DOCKER_INPUT_BINDING}":/input \ + --workdir /workspace -e HOME=/workspace fixed-openrave:latest "${CMD[@]}" >> "${LOG_FILE}" + + # the solver cpp, if having been generated successfully, will be located in $TMP_DIR/.openrave/*/ + CPP_FILE=$(ls -1 ${TMP_DIR}/.openrave/*/*.cpp 2> /dev/null) + if [ -z "${CPP_FILE}" ] ; then + echo "Failed to create an ikfast solver for iktype = ${IK_TYPE}" + continue + fi + + + # Note that the robot name given to create_ikfast_moveit_plugin.py is deliberately changed to $test_4dof_${IK_TYPE} + # for the purpose of creating a unique namespace for each plugin referencing the same robot definition but with a different ik solver; + # the real robot name is still available through the input argument --robot_name_in_srdf + + PACKAGE_NAME="test_4dof_${IK_TYPE}_ikfast_plugin" + CMD=$(cat <> "${LOG_FILE}" + + # Removing the current openrave data, so that it will not interfere with the next run of solver generation + rm -rf "${TMP_DIR}/.openrave" + + # Generating a test case file "test_4dof-ikfast.test" for this iktype + CMD=$(cat < "./${PACKAGE_NAME}/test_4dof-ikfast.test" +EOF + ) + eval "${CMD}" + +done + +cd "${CUR_DIR}" +# Building all generated packages, including model/config ones that are extracted from the prepared tar file "packages.tgz" +ls -1 "${OUTPUT_SRC_FOLDER}" | xargs -d '\n' catkin build --no-status --no-summary --no-deps diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 7f66340e71f..72719a43c2f 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -92,6 +92,8 @@ class SharedData int num_ik_tests_; int num_ik_multiple_tests_; int num_nearest_ik_tests_; + bool plugin_fk_support_; + bool position_only_check_; SharedData(SharedData const&) = delete; // this is a singleton SharedData() @@ -133,6 +135,12 @@ class SharedData ASSERT_TRUE(robot_model_->hasJointModelGroup(group_name_)); ASSERT_TRUE(robot_model_->hasLinkModel(root_link_)); ASSERT_TRUE(robot_model_->hasLinkModel(tip_link_)); + + if (!getParam("plugin_fk_support", plugin_fk_support_)) + plugin_fk_support_ = true; + + if (!getParam("position_only_check", position_only_check_)) + position_only_check_ = false; } public: @@ -173,6 +181,8 @@ class KinematicsTest : public ::testing::Test num_ik_tests_ = data.num_ik_tests_; num_ik_multiple_tests_ = data.num_ik_multiple_tests_; num_nearest_ik_tests_ = data.num_nearest_ik_tests_; + plugin_fk_support_ = data.plugin_fk_support_; + position_only_check_ = data.position_only_check_; } void SetUp() override @@ -252,20 +262,24 @@ class KinematicsTest : public ::testing::Test val2[i].position, abs_error), GTEST_NONFATAL_FAILURE_); - ss.str(""); - ss << "[" << i << "].orientation"; - GTEST_ASSERT_(isNear((expr1 + ss.str()).c_str(), (expr2 + ss.str()).c_str(), abs_error_expr, val1[i].orientation, - val2[i].orientation, abs_error), - GTEST_NONFATAL_FAILURE_); + if (!position_only_check_) + { + ss.str(""); + ss << "[" << i << "].orientation"; + GTEST_ASSERT_(isNear((expr1 + ss.str()).c_str(), (expr2 + ss.str()).c_str(), abs_error_expr, + val1[i].orientation, val2[i].orientation, abs_error), + GTEST_NONFATAL_FAILURE_); + } } return testing::AssertionSuccess(); } - void searchIKCallback(const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code) + void searchIKCallback(const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code, + moveit::core::RobotState& robot_state) { std::vector link_names = { tip_link_ }; std::vector poses; - if (!kinematics_solver_->getPositionFK(link_names, joint_state, poses)) + if (!getPositionFK(link_names, joint_state, poses, robot_state)) { error_code.val = error_code.PLANNING_FAILED; return; @@ -278,6 +292,38 @@ class KinematicsTest : public ::testing::Test error_code.val = error_code.PLANNING_FAILED; } + bool getPositionFK(const std::vector& link_names, const std::vector& joint_state, + std::vector& poses, moveit::core::RobotState& robot_state) + { + // There are some cases, e.g. testing a IKFast plugin targeting a robot of dof < 6, where + // kinematics_solver_->getPositionFK() will always return false, and that will render the entire test process + // useless as every test is doomed to fail due to calls to getPositionFK(). + // + // When plugin_fk_support_ is set to false, the FK pass will be done by updating robot_state with the given joints + // and calling moveit::core::RobotState::getGlobalLinkTransform(); therefore the lack of support in + // kinematics_solver_->getPositionFK() is circumvented, and tests for IK functionality can still be run to perform + // meaningful checks + + if (plugin_fk_support_) + return kinematics_solver_->getPositionFK(link_names, joint_state, poses); + else + { + std::vector joint_state_backup; + robot_state.copyJointGroupPositions(jmg_, joint_state_backup); + robot_state.setJointGroupPositions(jmg_, joint_state); + robot_state.updateLinkTransforms(); + + poses.clear(); + poses.reserve(link_names.size()); + for (const std::string& link_name : link_names) + poses.emplace_back(tf2::toMsg(robot_state.getGlobalLinkTransform(link_name))); + + robot_state.setJointGroupPositions(jmg_, joint_state_backup); + robot_state.updateLinkTransforms(); + return true; + } + } + public: moveit::core::RobotModelPtr robot_model_; moveit::core::JointModelGroup* jmg_; @@ -296,6 +342,8 @@ class KinematicsTest : public ::testing::Test unsigned int num_ik_tests_; unsigned int num_ik_multiple_tests_; unsigned int num_nearest_ik_tests_; + bool plugin_fk_support_; + bool position_only_check_; }; #define EXPECT_NEAR_POSES(lhs, rhs, near) \ @@ -314,7 +362,7 @@ TEST_F(KinematicsTest, getFK) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, joints); std::vector fk_poses; - EXPECT_TRUE(kinematics_solver_->getPositionFK(tip_frames, joints, fk_poses)); + EXPECT_TRUE(getPositionFK(tip_frames, joints, fk_poses, robot_state)); robot_state.updateLinkTransforms(); std::vector model_poses; @@ -357,7 +405,7 @@ TEST_F(KinematicsTest, randomWalkIK) robot_state.copyJointGroupPositions(jmg_, goal); // compute target tip_frames std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, goal, poses)); + ASSERT_TRUE(getPositionFK(tip_frames, goal, poses, robot_state)); // compute IK moveit_msgs::MoveItErrorCodes error_code; @@ -370,16 +418,21 @@ TEST_F(KinematicsTest, randomWalkIK) // on success: validate reached poses std::vector reached_poses; - kinematics_solver_->getPositionFK(tip_frames, solution, reached_poses); + getPositionFK(tip_frames, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); - // validate closeness of solution pose to goal - auto diff = Eigen::Map(solution.data(), solution.size()) - - Eigen::Map(goal.data(), goal.size()); - if (!diff.isZero(1.05 * NEAR_JOINT)) + // The following joint state check is skipped when position_only_check_ is true, + // because matching two sets of joint states would imply fully matching the two corresponding poses + if (!position_only_check_) { - ++failures; - ROS_WARN_STREAM("jump in [" << i << "]: " << diff.transpose()); + // validate closeness of solution pose to goal + auto diff = Eigen::Map(solution.data(), solution.size()) - + Eigen::Map(goal.data(), goal.size()); + if (!diff.isZero(1.05 * NEAR_JOINT)) + { + ++failures; + ROS_WARN_STREAM("jump in [" << i << "]: " << diff.transpose()); + } } // update robot state to found pose @@ -475,7 +528,7 @@ TEST_F(KinematicsTest, unitIK) // compute initial end-effector pose std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, seed, poses)); + ASSERT_TRUE(getPositionFK(tip_frames, seed, poses, robot_state)); Eigen::Isometry3d initial, goal; tf2::fromMsg(poses[0], initial); @@ -488,7 +541,7 @@ TEST_F(KinematicsTest, unitIK) // validate reached poses std::vector reached_poses; - kinematics_solver_->getPositionFK(tip_frames, sol, reached_poses); + getPositionFK(tip_frames, sol, reached_poses, robot_state); EXPECT_NEAR_POSES({ goal }, reached_poses, tolerance_); // validate ground truth @@ -549,7 +602,7 @@ TEST_F(KinematicsTest, searchIK) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); kinematics_solver_->searchPositionIK(poses[0], seed, timeout_, solution, error_code); if (error_code.val == error_code.SUCCESS) @@ -558,7 +611,7 @@ TEST_F(KinematicsTest, searchIK) continue; std::vector reached_poses; - kinematics_solver_->getPositionFK(fk_names, solution, reached_poses); + getPositionFK(fk_names, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } @@ -583,7 +636,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); if (poses[0].position.z <= 0.0f) { --i; // draw a new random state @@ -592,8 +645,10 @@ TEST_F(KinematicsTest, searchIKWithCallback) kinematics_solver_->searchPositionIK( poses[0], fk_values, timeout_, solution, - [this](const geometry_msgs::Pose& /*unused*/, const std::vector& joints, - moveit_msgs::MoveItErrorCodes& error_code) { searchIKCallback(joints, error_code); }, + [this, &robot_state](const geometry_msgs::Pose& /*unused*/, const std::vector& joints, + moveit_msgs::MoveItErrorCodes& error_code) { + searchIKCallback(joints, error_code, robot_state); + }, error_code); if (error_code.val == error_code.SUCCESS) success++; @@ -601,7 +656,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) continue; std::vector reached_poses; - kinematics_solver_->getPositionFK(fk_names, solution, reached_poses); + getPositionFK(fk_names, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } @@ -623,16 +678,27 @@ TEST_F(KinematicsTest, getIK) fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0); robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); - std::vector poses; + std::vector poses, poses_from_ik; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); kinematics_solver_->getPositionIK(poses[0], fk_values, solution, error_code); - // starting from the correct solution, should yield the same pose EXPECT_EQ(error_code.val, error_code.SUCCESS); - Eigen::Map sol(solution.data(), solution.size()); - Eigen::Map truth(fk_values.data(), fk_values.size()); - EXPECT_TRUE(sol.isApprox(truth, tolerance_)) << sol.transpose() << std::endl << truth.transpose() << std::endl; + if (position_only_check_) + { + // The joint state check is skipped when position_only_check_ is true, + // because matching two sets of joint states would imply fully matching the two corresponding poses. + // Instead we only check if the derived position from the solution is close enough to the original one. + ASSERT_TRUE(getPositionFK(fk_names, solution, poses_from_ik, robot_state)); + EXPECT_NEAR_POSES(poses, poses_from_ik, tolerance_); + } + else + { + // starting from the correct solution, should yield the same pose + Eigen::Map sol(solution.data(), solution.size()); + Eigen::Map truth(fk_values.data(), fk_values.size()); + EXPECT_TRUE(sol.isApprox(truth, tolerance_)) << sol.transpose() << std::endl << truth.transpose() << std::endl; + } } } @@ -655,7 +721,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); solutions.clear(); kinematics_solver_->getPositionIK(poses, fk_values, solutions, result, options); @@ -668,7 +734,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) std::vector reached_poses; for (const auto& s : solutions) { - kinematics_solver_->getPositionFK(fk_names, s, reached_poses); + getPositionFK(fk_names, s, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } } @@ -695,7 +761,7 @@ TEST_F(KinematicsTest, getNearestIKSolution) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); // sample seed vector robot_state.setToRandomPositions(jmg_, this->rng_);