From c43e9cd4703431657554f34bd341b37ad973b3b6 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Mon, 21 Oct 2019 10:39:21 +0200 Subject: [PATCH] =?UTF-8?q?Eliminato=20il=20supporto=20a=20bio-ik,=20si=20?= =?UTF-8?q?potr=C3=A0=20=20reinserire=20in=20futuro?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/bio_ik-kinetic-support/CMakeLists.txt | 129 -- src/bio_ik-kinetic-support/README.md | 393 ----- .../bio_ik_kinematics_description.xml | 4 - src/bio_ik-kinetic-support/doc/goals.pdf | Bin 20945 -> 0 bytes src/bio_ik-kinetic-support/doc/pr2_vt_0.png | Bin 117770 -> 0 bytes src/bio_ik-kinetic-support/doc/solvers.pdf | Bin 19457 -> 0 bytes .../include/bio_ik/bio_ik.h | 50 - .../include/bio_ik/frame.h | 258 --- .../include/bio_ik/goal.h | 130 -- .../include/bio_ik/goal_types.h | 661 -------- .../include/bio_ik/robot_info.h | 126 -- src/bio_ik-kinetic-support/package.xml | 47 - .../src/forward_kinematics.h | 1503 ----------------- src/bio_ik-kinetic-support/src/goal_types.cpp | 269 --- src/bio_ik-kinetic-support/src/ik_base.h | 214 --- .../src/ik_cppoptlib.cpp | 257 --- .../src/ik_evolution_1.cpp | 561 ------ .../src/ik_evolution_2.cpp | 659 -------- .../src/ik_gradient.cpp | 289 ---- src/bio_ik-kinetic-support/src/ik_neural.cpp | 690 -------- src/bio_ik-kinetic-support/src/ik_parallel.h | 278 --- src/bio_ik-kinetic-support/src/ik_test.cpp | 137 -- .../src/kinematics_plugin.cpp | 649 ------- src/bio_ik-kinetic-support/src/problem.cpp | 340 ---- src/bio_ik-kinetic-support/src/problem.h | 142 -- src/bio_ik-kinetic-support/src/utils.h | 521 ------ 26 files changed, 8307 deletions(-) delete mode 100644 src/bio_ik-kinetic-support/CMakeLists.txt delete mode 100644 src/bio_ik-kinetic-support/README.md delete mode 100644 src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml delete mode 100644 src/bio_ik-kinetic-support/doc/goals.pdf delete mode 100644 src/bio_ik-kinetic-support/doc/pr2_vt_0.png delete mode 100644 src/bio_ik-kinetic-support/doc/solvers.pdf delete mode 100644 src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h delete mode 100644 src/bio_ik-kinetic-support/include/bio_ik/frame.h delete mode 100644 src/bio_ik-kinetic-support/include/bio_ik/goal.h delete mode 100644 src/bio_ik-kinetic-support/include/bio_ik/goal_types.h delete mode 100644 src/bio_ik-kinetic-support/include/bio_ik/robot_info.h delete mode 100644 src/bio_ik-kinetic-support/package.xml delete mode 100644 src/bio_ik-kinetic-support/src/forward_kinematics.h delete mode 100644 src/bio_ik-kinetic-support/src/goal_types.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_base.h delete mode 100644 src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_evolution_1.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_evolution_2.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_gradient.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_neural.cpp delete mode 100644 src/bio_ik-kinetic-support/src/ik_parallel.h delete mode 100644 src/bio_ik-kinetic-support/src/ik_test.cpp delete mode 100644 src/bio_ik-kinetic-support/src/kinematics_plugin.cpp delete mode 100644 src/bio_ik-kinetic-support/src/problem.cpp delete mode 100644 src/bio_ik-kinetic-support/src/problem.h delete mode 100644 src/bio_ik-kinetic-support/src/utils.h diff --git a/src/bio_ik-kinetic-support/CMakeLists.txt b/src/bio_ik-kinetic-support/CMakeLists.txt deleted file mode 100644 index b46c04f..0000000 --- a/src/bio_ik-kinetic-support/CMakeLists.txt +++ /dev/null @@ -1,129 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) -project(bio_ik) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED COMPONENTS - moveit_core - moveit_ros_planning - pluginlib - roscpp - tf - tf_conversions - eigen_conversions -) - -find_package(OpenMP) -# the specific flag is not yet present in cmake 2.8.12 -if(OpenMP_CXX_FOUND OR OPENMP_FOUND) - message(STATUS "OPENMP FOUND") - add_compile_options(${OpenMP_CXX_FLAGS}) - if(NOT OpenMP_CXX_LIBRARIES) - # cmake 2.8.12 does not yet specify the library - assume we might need libgomp - set(OpenMP_LIBS gomp) - else() - set(OpenMP_LIBS ${OpenMP_CXX_LIBRARIES}) - endif() -else() - message(WARNING "OPENMP NOT FOUND. You will suffer performance loss.") - set(OpenMP_LIBS) -endif() - -option(USE_FANN "build the neural-network-based IK solver (experimental)" OFF) -if(USE_FANN) - find_library(FANN_LIBRARIES NAMES fann) - find_path(FANN_INCLUDE_DIRS NAMES fann.h) - if(NOT FANN_INCLUDE_DIRS OR NOT FANN_LIBRARIES) - message(FATAL_ERROR "Neural network solver requested, but libfann was not found.") - else() - message("Found libfann: ${FANN_LIBRARIES} / ${FANN_INCLUDE_DIRS}") - endif() -else() - set(FANN_LIBRARIES) - set(FANN_INCLUDE_DIRS) -endif() - -option(USE_CPPOPTLIB "Include gradient-based solvers from CppNumericalSolvers (bio_ik also provides its own solver)" OFF) -if(USE_CPPOPTLIB) - find_path(CPPOPTLIB_INCLUDE_DIRS - NAMES cppoptlib/solver/bfgssolver.h - HINTS ../../CppNumericalSolvers/include) - if(NOT CPPOPTLIB_INCLUDE_DIRS) - message(FATAL_ERROR "cppoptlib support requested, but the headers could not be found.") - else() - message("Found cppoptlib: ${CPPOPTLIB_INCLUDE_DIRS}") - endif() - add_definitions(-DENABLE_CPP_OPTLIB) -else() - set(CPPOPTLIB_INCLUDE_DIRS) -endif() - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - moveit_core - moveit_ros_planning - pluginlib - roscpp - tf - tf_conversions -) - -add_compile_options(-frecord-gcc-switches) - -add_compile_options(-std=c++11) - -add_compile_options($<$:-O3>) -add_compile_options($<$:-ftree-vectorize>) -add_compile_options($<$:-ffast-math>) - -include_directories( - include - ${FANN_INCLUDE_DIRS} - ${CPPOPTLIB_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -set(SOURCES - src/goal_types.cpp - src/kinematics_plugin.cpp - src/problem.cpp - - src/ik_test.cpp - src/ik_gradient.cpp - src/ik_evolution_1.cpp - src/ik_evolution_2.cpp -) - -if(USE_FANN) - list(APPEND SOURCES src/ik_neural.cpp) -endif() - -if(USE_CPPOPTLIB) - list(APPEND SOURCES src/ik_cppoptlib.cpp) -endif() - -add_library(${PROJECT_NAME} ${SOURCES}) - -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) - -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${FANN_LIBRARIES} - ${OpenMP_LIBS} - - -static-libgcc - -static-libstdc++ -) - -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(FILES bio_ik_kinematics_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/bio_ik-kinetic-support/README.md b/src/bio_ik-kinetic-support/README.md deleted file mode 100644 index 480d643..0000000 --- a/src/bio_ik-kinetic-support/README.md +++ /dev/null @@ -1,393 +0,0 @@ -# bio-ik - -## Installation and Setup - -You will need ROS version Indigo or newer (wiki.ros.org). -The software was developed on Ubuntu Linux 16.04 LTS with ROS Kinetic, -but has also been tested on Ubuntu Linux 14.04 LTS with ROS Indigo. -Newer versions of ROS should work, but may need some adaptation. -See below for version specific instructions. - -* Download the `bio_ik` package and unpack into your catkin workspace. -* Run `catkin_make` to compile your workspace: - ``` - roscd - cd src - git clone https://github.com/TAMS-Group/bio_ik.git - roscd - catkin_make - ``` - -* Configure Moveit to use bio-ik as the kinematics solver (see next section). -* Use Moveit to plan and execute motions or use your own code - together with `move_group` node to move your robot. -* As usual, the public API is specified in the public header files for the `bio_ik` package, - located in the `include/bio_ik` subdirectory; - the sources including a few private header files are in the `src` subdirectory. - - -## Basic Usage - -For ease of use and compatibility with existing code, -the bio-ik algorithm is encapsulated as a Moveit kinematics plugin. -Therefore, bio-ik can be used as a direct replacement of -the default Orocos/KDL-based IK solver. -Given the name of an end-effector and a 6-DOF target pose, -bio-ik will search a valid robot joint configuration that reaches the given target. - -In our tests (see below), both in terms of success rate and solution time, -bio-ik regularly outperformed the Orocos [1] solver -and is competitive with trac-ik [2]. -The bio-ik algorithm can also be used for high-DOF system like robot snakes, -and it will automatically converge to the best approximate solutions -for low-DOF arms where some target poses are not reachable exactly. - -While you can write the Moveit configuration files by hand, -the easiest way is to run the Moveit setup assistant for your robot, -and then to select bio-ik as the IK solver when configuring the end effectors. -Once configured, the solver can be called using the standard Moveit API -or used interactively from rviz using the MotionPlanning GUI plugin. - -* Make sure that you have a URDF (or xacro) model for your robot. -* Run the moveit setup assistant to create the Moveit configuration files: - - ``` - rosrun moveit_setup_assistant moveit_setup_assistant - - ``` -* The setup assistant automatically searches for all available IK solver plugins - in your workspace. - Therefore, you can just select select bio-ik as the IK solver - from the drop-down list for every end effector and then configure - the kinematics parameters, namely the default position accuracy (meters) - and the timeout (in seconds). For typical 6-DOF or 7-DOF arms, - an accuracy of 0.001 m (or smaller) and a timeout of 1 msec should be ok. - More complex robots might need a longer timeout. -* Generate the moveit configuration files from the setup assistant. - Of course, you can also edit the `config/kinematics.yaml` configuration - file with your favorite text editor. - For example, a configuration for the PR2 robot might look like this: - - ``` - # example kinematics.yaml for the PR2 robot - right_arm: - # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - # kinematics_solver_attempts: 1 - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 1 - left_arm: - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 1 - all: - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.02 - kinematics_solver_attempts: 1 - - # optional bio-ik configuration parameters - # center_joints_weight: 1 - # minimal_displacement_weight: 1 - # avoid_joint_limits_weight: 1 - ``` - - -* For a first test, run the Moveit-created demo launch. Once rviz is running, - enable the motion planning plugin, then select one of the end effectors - of you robot. Rviz should show an 6-D (position and orientation) - interactive marker for the selected end-effector(s). - Move the interactive marker and watch bio-ik calculating poses for your robot. - - If you also installed the bio-ik demo (see below), you should be able - to run one of the predefined demos: - ``` - roslaunch pr2_bioik_moveit demo.launch - roslaunch pr2_bioik_moveit valve.launch - roslaunch pr2_bioik_moveit dance.launch - ``` - -* You are now ready to use bio-ik from your C/C++ and Python programs, - using the standard Moveit API. - To explicitly request an IK solution in C++: - ``` - robot_model_loader::RobotModelLoader robot_model_loader(robot); - - auto robot_model = robot_model_loader.getModel(); - auto joint_model_group = robot_model->getJointModelGroup(group); - auto tip_names = joint_model_group->getSolverInstance()->getTipFrames(); - - kinematics::KinematicsQueryOptions opts; - opts.return_approximate_solution = true; // optional - - robot_state::RobotState robot_state_ik(robot_model); - - // traditional "basic" bio-ik usage. The end-effector goal poses - // and end-effector link names are passed into the setFromIK() - // call. The KinematicsQueryOptions are empty. - // - bool ok = robot_state_ik.setFromIK( - joint_model_group, // joints to be used for IK - tip_transforms, // multiple end-effector goal poses - tip_names, // names of the end-effector links - attempts, timeout, // solver attempts and timeout - moveit::core::GroupStateValidityCallbackFn(), - opts // mostly empty - ); - ``` - -## Advanced Usage - -For many robot applications, it is essential to specify more than just -a single end-effector pose. Typical examples include - -* two-arm manipulation tasks on two-arm robots (e.g. Baxter) -* multi end-effector tasks with shared kinematic links -* grasping and manipulation tasks with multi-finger hands -* full-body motion on humanoid robots -* reaching tasks with additional constraints (e.g. shoulder position) -* incremental tool motions without robot arm configuration changes -* and many more - -In bio-ik, such tasks are specified as a combination of multiple -individual *goals*. -The algorithm then tries to find a robot configuration -that fulfills all given goals simultaneously by minimizing -a quadratic error function built from the weighted individual goals. -While the current Moveit API does not support multiple-goals tasks directly, -it provides the KinematicQueryOptions class. -Therefore, bio-ik simply provides a set of predefined motion goals, -and a combination of the user-specified goals is passed via Moveit to the IK solver. -No API changes are required in Moveit, but using the IK solver now consists -passing the weighted goals via the KinematicQueryOptions. -The predefined goals include: - -* *PoseGoal*: a full 6-DOF robot pose -* *PositionGoal*: a 3-DOF (x,y,z) position -* *OrientationGoal*: a 3-DOF orientation, encoded as a quaternion (qx,qy,qz,qw) -* *LookAtGoal*: a 3-DOF (x,y,z) position intended as a looking direction - for a camera or robot head -* *JointGoal*: a set of joint angles, e.g. to specify a -* *FunctionGoal*: an arbitrary function of the robot joint values, - e.g. to model underactuated joints or mimic joints -* and several more - - - -To solve a motion problem on your robot, the trick now is to construct -a suitable combination of individual goals. - -![PR2 turning a valve](doc/pr2_vt_0.png) - -In the following example, we want to grasp and then _slowly turn -a valve wheel_ with the left and right gripers of the PR2 robot: - - ``` - bio_ik::BioIKKinematicsQueryOptions ik_options; - ik_options.replace = true; - ik_options.return_approximate_solution = true; - - auto* ll_goal = new bio_ik::PoseGoal(); - auto* lr_goal = new bio_ik::PoseGoal(); - auto* rl_goal = new bio_ik::PoseGoal(); - auto* rr_goal = new bio_ik::PoseGoal(); - ll_goal->setLinkName("l_gripper_l_finger_tip_link"); - lr_goal->setLinkName("l_gripper_r_finger_tip_link"); - rl_goal->setLinkName("r_gripper_l_finger_tip_link"); - rr_goal->setLinkName("r_gripper_r_finger_tip_link"); - ik_options.goals.emplace_back(ll_goal); - ik_options.goals.emplace_back(lr_goal); - ik_options.goals.emplace_back(rl_goal); - ik_options.goals.emplace_back(rr_goal); - ``` - -We also set a couple of secondary goals. -First, we want that the head of the PR2 looks at the center of the valve. -Second, we want to avoid joint-limits on all joints, if possible. -Third, we want that IK solutions are as close as possible to the previous -joint configuration, meaning small and efficient motions. This is handled -by adding the MinimalDisplacementGoal. -Fourth, we want to avoid torso lift motions, which are very slow on the PR2. -All of this is specified easily: - - ``` - auto* lookat_goal = new bio_ik::LookAtGoal(); - lookat_goal->setLinkName("sensor_mount_link"); - ik_options.goals.emplace_back(lookat_goal); - - auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal(); - ik_options.goals.emplace_back(avoid_joint_limits_goal); - - auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal(); - ik_options.goals.emplace_back(minimal_displacement_goal); - - auto* torso_goal = new bio_ik::PositionGoal(); - torso_goal->setLinkName("torso_lift_link"); - torso_goal->setWeight(1); - torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 )); - ik_options.goals.emplace_back(torso_goal); - ``` - -For the actual turning motion, we calculate a set of required gripper -poses in a loop: - ``` - for(int i = 0; ; i++) { - tf::Vector3 center(0.7, 0, 1); - - double t = i * 0.1; - double r = 0.1; - double a = sin(t) * 1; - double dx = fmin(0.0, cos(t) * -0.1); - double dy = cos(a) * r; - double dz = sin(a) * r; - - tf::Vector3 dl(dx, +dy, +dz); - tf::Vector3 dr(dx, -dy, -dz); - tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1))); - - ll_goal->setPosition(center + dl + dg); - lr_goal->setPosition(center + dl - dg); - rl_goal->setPosition(center + dr + dg); - rr_goal->setPosition(center + dr - dg); - - double ro = 0; - ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - - lookat_goal->setAxis(tf::Vector3(1, 0, 0)); - lookat_goal->setTarget(rr_goal->getPosition()); - - // "advanced" bio-ik usage. The call parameters for the end-effector - // poses and end-effector link names are left empty; instead the - // requested goals and weights are passed via the ik_options object. - // - robot_state.setFromIK( - joint_model_group, // active PR2 joints - EigenSTL::vector_Affine3d(), // no explicit poses here - std::vector(), // no end effector links here - 0, 0.0, // take values from YAML file - moveit::core::GroupStateValidityCallbackFn(), - ik_options // four gripper goals and secondary goals - ); - - ... // check solution validity and actually move the robot - } - ``` - -When you execute the code, the PR2 will reach for the valve wheel -and turn it. Every once in a while it can't reach the valve with -its current arm configuration and will regrasp the wheel. - - -## How it works - -The bio-ik solver is based on a memetic algorithm that combines -traditional gradient-based search with a hybrid genetic -and particle-swarm optimization [3]. -See [4] for the basic idea and the details of the evolutionary operators -and [5] for the description of the algorithm applied to many IK and manipulation tasks. - -Internally, vectors of all robot joint values are used to encode -different intermediate solutions (the *genotype* of the genetic algorithm). -During the optimization, joint values are always checked against the -active lower and upper joint limits, so that only valid robot configurations -are generated. - -To calculate the fitness of individuals, the cumulative error over all -given individual goals is calculated. -Any individual with zero error is an exact solution for the IK problem, -while individuals with small error correspond to approximate solutions. - -Individuals are sorted by their fitness, and gradient-based optimization -is tried on the best few configuration, resulting in fast convergence -and good performance for many problems. -If no solution is found from the gradient-based optimization, -new individuals are created by a set of mutation and recombination operators, -resulting in good search-space exploration. - - - -## Example Performance data - -To be written. - -## Running the Self-Tests - -We have tested bio-ik on many different robot arms, -both using the tranditional single end-effector API -and the advanced multi end-effector API based on the KinematicsQueryOptions. - -One simple selftest consists of generating random valid robot configurations, -running forward kinematics to calculate the resulting end-effector pose, -and the querying the IK plugin to find a suitable robot joint configuration. -Success is then checked by running forrward kinematics again and checking -that the end-effector pose for the generated IK solution matches the target pose. -This approach can be run easily for thousands or millions of random poses, -samples the full workspace of the robot, -and allows to quickly generate success-rate and solution-time estimates -for the selected IK solver. - -Of course, running the tests requires installing the corresponding robot -models and adds a lot of dependencies. -Therefore, those tests are not included in the standard bio-ik package, -but are packaged separately. - -For convenience, we provide the `pr2_bioik_moveit` package, -which also includes a few bio-ik demos for the PR2 service robot. -These are kinematics only demos; but of course you can also try -running the demos on the real robot (if you have one) or the Gazebo -simulator (if you installed Gazebo). - -Simply clone the PR2 description package (inside `pr2_common`) -and the `pr2_bioik_moveit` package into your catkin workspace: - ``` - roscd - cd src - git clone https://github.com/PR2/pr2_common.git - git clone https://github.com/TAMS/pr2_bioik_moveit.git - catkin_make - ``` - -For the FK-IK-FK performance test, please run - - ``` - roslaunch pr2_bioik_moveit env_pr2.launch - roslaunch pr2_bioik_moveit test_fk_ik.launch - ... // wait for test completion and results summary - ``` - - - - - -## References - - 1. Orocos Kinematics and Dynamics, http://www.orocos.org - - 2. P. Beeson and B. Ames, *TRAC-IK: - An open-source library for improved solving of generic inverse kinematics*, - Proceedings of the IEEE RAS Humanoids Conference, Seoul, Korea, November 2015. - - 3. Philipp Ruppel, Prformance optimization and implementation - of evolutionary inverse kinematics in ROS*, - MSc thesis, University of Hamburg, 2017 - [PDF](https://tams.informatik.uni-hamburg.de/publications/2017/MSc_Philipp_Ruppel.pdf) - - - 4. Sebastian Starke, Norman Hendrich, Jianwei Zhang, *A Memetic - Evolutionary Algorithm for Real-Time Articulated Kinematic Motion*, - IEEE Intl. Congress on Evolutionary Computation (CEC-2017), p.2437-2479, June 4-8, 2017, - San Sebastian, Spain. - DOI: [10.1109/CEC.2017.7969605](http://doi.org/10.1109/CEC.2017.7969605) - - 5. Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, *Multi-Objective - Evolutionary Optimisation for Inverse Kinematics - on Highly Articulated and Humanoid Robots*, - IEEE Intl. Conference on Intelligent Robots and Systems (IROS-2017), - September 24-28, 2017, Vancouver, Canada - - diff --git a/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml b/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml deleted file mode 100644 index cf68db3..0000000 --- a/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/bio_ik-kinetic-support/doc/goals.pdf b/src/bio_ik-kinetic-support/doc/goals.pdf deleted file mode 100644 index 53fe5751311d28b8e67fcd9ca9a82c46601a51aa..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20945 zcmbT7byywCvgm^a3-0dj?(Xic3wI|tgy8OO!GpWIySux)J3O-Y+2@{n-~HYn5BO%L ztE;MOx_kAknqNaAFDy#KK+6I}Qg>GV0>y++k8f*W0maRYPbY0`W9npv&-^J;gu=(i zrxP`|ax!-KJX-;rjD?L2ZHU<3o!Y1ljh0pI^+J zHwh|~z?VKwZgj)+2V+h?_*c*x-)G(~4e0_-^_(T|_3k>hyH+2PZD$VBb)1*oJWmXf zb<@@*shHbS$+ht_51z!{Mz~Sd&z>X->+IIuuoSi4L=_k~mhXo(>Ef>$6@47$yk4Kx zaShkscby-4rVdIB^P|?wBX_1Kc!zUG=Ay{-oIbdVzLERmT&_deN=_%BEeN1Pap`G; zWka0&Qqp=WXhT&Lv7~RCBQBj+e16?8ZhzWPyFYwE!O7V1_C0|q&$@bpeUP)edwhj@ zXlV5AImoi>=yh%n=Wo}b1l1%VMzx^zMb41V3}2JkD3RWvDQD7VdmOKQk3amC>w~w) z*V>n4xq2~;cRH07fRNFLskGf~zTe4r{Q&r~2h_9Kaiye&g!-PT;aYW^oHfd25+b*bioc;^IF?F)#9q+-BR=Vec{nlOZ;%&41JM37^o&PG1#|`Km)S0O+M0H zQCAr4!0b1)V)R|?UZ-pL!+bzxS&m+{gKlYLisbyH-tdn^mj!pNN{6k8qsm7cJ4Kv+6mG`_- zAGNw&PK^NL-8dBaIg{PoB`h3sxFt7uK3aXeMjCu4dI&nzx1t%Eb@aKT+e!N zE#Q__gqz9N7K||Fvn}gBqHg1ezkFHrAw!oRi!m=0zNHE z^O$6LJp}{o5O=mN!jL+$_{C$_&${m`SakW|Z#a=h9Dyf8b;37G4#=|IuNNq2V^H04B*;)We=TCqV*n~8>*Dzx?LM`Du&#PJ4RFyA)?T^ ze0guc3s-r5|O)$^YmHm;;`dtx#_(ppYx*!ZNqqz0UJ&jU1p z$f6>8yyY7uBpwAUQV97)nOBVmgCj&6c|tEy{L`v~KZd5?%@JSyV=CkPfis~rId|r7 zST<+trV+om!$PnsB0-;$Uns;GSGOtY@Q$SopzCssl^-lwtdZ!!RacHq8l))9*E+J6 z#^NPcAP~He(ki7O7cdJi2K#P4{3sJJCS@paqnv?=7iix?W|>BZ^Wj|e8+xB$7Nu0z zSQ0l(2V4F4HJRA`PVY*7qeAMA0D{p;^V6k$f^R8&SXE~+e3+Ivcyu$nR{rbsbsGnH zuIj24x$+&;cZ#)3SifoKFa~s5%+?sxV}LfYXaM}Jk{TRR6^z|B2rh1KMh6nCFigUw(nXIF2mRlBbAqbYqr?RkY-sGr$=dY7|2Q0O9svd>%+D#(;-cE>5r zDRE(yORe44ftUB{lMXOb+yNWg4E!2PlgB{RhmrLf{2Je3FWQ%^hM;_rriFE&+skIe zg9#!T6HF~ysj#a>{maJ@4A2C(WVyft$mL+OH>1pH@Ilb+1oCBE7kY_ti7g2^aZIfO zVg##M##n6ABd_FWLT#Z=48c{55G>kvgNiG?|HMy0a=<0a+aLz6v_elO(PsjmqI{r(d8k~O5h zaVZvb75*#HT|<+?i$r=~ynRV^E48C@^6c!CO-P8{r1nLY1v~}c# z#^ZK(-6A^H#z4=A0$h0<>gCx7zzFH6)wDFYhvcsgaDVeYQ4`f?0~=G1Ekduir}-n& zGP;(a2YkV{{u&wYyVLyO$i>Ezg^>_jd^sm3N|Ag`3HbnwB-?`&6$}}~pG?9uk(fBL zEGWR<$z2-`&0Ig)B|QAa5_s_;$mzUd%uPme42Y;!;zGAChC{h4ADLu+Kz}9Ra{Tm47B6-l z4eMuzj{6*pd9s`;%WR#FdtHsXT~R?EW8od}L|=lJF-EIYf>%VM0QkC<_*sdohw1yI zY(Ji|e!WZm#=sJ-YK>(NvKo(I)sTBve$)*S0;R=P?tV45I>n=?!$ZAuRC7;f33etq z;4C6GHTgZK`MW?^x9_39+IN9bLHoS!Jo)_XZlkjM3+pJA2e6k_Qw@R&GPk9GIZ&br zYE}nZL8?rO-|oRv7mz{ihHI<(Co@wr;PC7rvE!36Wk8KzhyZu-y)(50INxWjdF3P% z;zW%rG7b%9in9dq;7RNRu_!{nB#i3^@@#dZLKeVK@m8Q4u(q^rvo~1w(h~kqLRpU* z=4m>E4+grD)5mjSQRRX&v4j0E@I8)Tx_pnLs*SgL+VrcY#5!mbZ;Si`;p* zDe3)IuVmgN3-xOR(n}6B>#Z5!S4>f$gYc?T_)VAW=s5Mx;h7qHS`f ztb3@VMOWSqL2acvtt6PhwyWviJro1sx4&*CkC^jH5OIF2wZaR+yNz zvXvfD0|0%)viiFbt|@qUL|ANHf*bnyLCLyU^?he>4hlkZad>XTqMNw|N(;8>b-nO} zkR+(INpV|gO7BEN;CP6RK|nq+EcQ}9m~13YN7u1OT|=k2;)GLNZk&BZO@lEwYP@}T zyrwllh!nl@4_31gG54j?V1B$`1mcAiGNC~?N0!#sKBaG71Le>l4BbyLUITNet?y>8 z7L?whJxU;)y+=wTntO$`oH;rR@kAmHL0-@IS5lajY8{1&h$ebkU-OA@H{r5kbSG#d zWX`^6q(XReFNjy+-1Mv~kYREuFvq1Y?RR3Rj`jkT=D}y}be8of^-%9d#t;m^IwIBpo5yxF-E%Y7}a(DFPsvl-nOA3n2J;3|sPS}bQYCKnPcK$=uJ7JyE96ND$ zE10+X-qGp)2S8i_ICQQo1g2KzBC?s_ga%dh(ou3K>Ys}m8EAIGFtT+dS2A%hsk~u& z^q*x7a#T(~JND0tXeC`JRY*K%@A!i?sRE&%KZTW){MSGx??@S{Zb4Qt1nSS-Jf|RR zB5}Nm%PCZJFKMhG=m?AqI(gY;M=|>#+mnBIpdA-PKSwa5?2t}A8GQT+)Z|~jjG`@9{ zx+;O{(eN^WK|%s5xPX%_X|F?UhMHGDouk$iu=AOz>Gq9gw-0z2?%fS*$i+PlbR~q& z#c2qj@SDNtynD(7o|~9SPPwC=y@v5`5c-y@J--6aKycBW{r!XfJXEM+uetfRJeNZy z6gzjk8Rd?IzA$DoN~G<;;txR;HWp7?0oP446F|AFe?LHA8SgC&h?h_{N03P#SN1^g zt4lsw#(c&HR7#qMDMS5^>RFAWA6lFxl~yzb-j2sUvqSwM*mopc7nhVSAmL;k<(W+V z1wVOJ7vo6j-W9WdT*lg>b3{7TM>q=oj_dchQQ|Xg=u9?<#7Zu10Yxeh+I0vSZxnZg z*${@Z$aF^JwJU$7K#*8Oqqk=wCV1=hdI=>rcw{ z=lq#;F)^{T{89K*Wc+t>_E$P4;_4)(Ry&CVE~ zP99)t{8udU00-kw*9`yI(a9JanF9oEUGcR(MfCU#3>@^dEG!HR>d*ZMeQF6Ce>XQYRumKbqw=qwP&9V5b#^c`cEtbFtBU_|m+YVA|B(W7{5!1w zEd~D5%-^{(13m{kGxLAXiQU{hl|}At=Xg{aF522$nz@dM+enxZG!TJ-x(R6rwcv9Z z(f!4Lz{88=fFcke_6xuR_%Z5(5u*i!d(j^$(jd^qj1gvgVbGmOX>(y1d44m>9~*!&R%nH5ET`1i|UXnAi39z4UxzJd`<>+Ika zL#$QS)Ch`O*jmBSQDw0k$2(nU2u>$$7tWxw@K8@*=F1RJLul7wrrna?%Nle__&KHa_jyyB2ODs=x9g zR({{~zg{w2@bNqA@lV#pN2EZaQpec%F`FuG2M4I7k&QQBS@_+Z6^%YrZX4`O8X2fA zL`^n?@m;ig+zx|iYzDvViK?pGsO56U&w)!@h@7J3)&=*#G$adIn5bR3qAwefP93L+ zYo<-Bt4c)&q+}YzynQ^8mYAz3ca?Y3EHcB=qI>}6IBmQS+mycT%xRD8-Ql<&l!7L5 zkeW$29TsuicxYUfnMvrnQAfw2#o5J#vxmD~8T+-yRqJXvUf%j7r8EtF`&JdUt2LX%i&PSLnsXWHs< zhsiohkuy^~s5kC5#pr&M46v`h92L%T$0u0HptZ>+gSZW7M}sHJsEDAL`L#Zx#(UvM zHKuZf0QZ_j8rhJJ&Kc5LdNc`Zg&KdA0&v&W~@Ji!0(`qJ+Ckhj+!& zo&roy^iRKA=f96)aAI@Ex3k{-a!d#?A{7c%sY`tU?K(c6LzO7yLmL@$-aj~SAzhL= zNR64Vm`LY~%hES;GW)KYC7)%tQK}`_;2$Q`xAh|Q5QGQAgGoD~iL8n2&3bF~xHmbn z#U^O$&(q|azA58tb{WYJu=D&y%uXkdqK&GKsg3s=avsdWFLK**d_71seTy4QS8}Db z+`3yQ8Z8B*ddLmr4AEfv`5IhwjTJ!9CRU1F>vcXq@jJuB^Y#(1@NLg!roHUf(yS!i zH=OB@#KMBwv<*fVLyfEKJKGEKkF~N4P6gBE4{)IZc^i_XQ_-hyt+JU(zZ2M#g*u{6 z)n%1wc1c>()$B#X;^gTYCK(lITIl)_uN#Bl?@4Tl(}~5#rg^KA73kojamMbRp;P5^ z>|>Z(gT@nBUmeof-WjFSdp#PP3qxwdmUrWfusn3PXNQA@?3o`T_l{Sxm>}e(W*zDz z+oXTgBtY=g#OhfXHLy>)!=87y$dI~CA$uasMYcq`!E)9w{Wc;2$xtm>%MQ~#=g6pC z|LkmoXX||{x+|3@;$8OYrKLLSMB0yPv5f&g?(sW^Bz0qF*RRf$71J(lXLT=dtz_0V z7z_!#H?wa=DH?PYHc#ORR_JP;f!f)Iq?s~AVL3JdQeRdEZ;D4L;!rZ0Ofmuk-3w{O z+6-le$hk>)FB$M2hT5e40F_6y-l(`EG}HLGS)r zB*nJaqzC0^DUc_4V}IpR>3GRJNY6lJTAp50s+)YUSBX7ya}poXD`c$ETmTA6|0bS} zkfo-qWKS=>!Y@6uo08iU)T!!BZ(`2vvdy+@vP)f*ZDImN^oYhm*%w_m(_;f?(@<&Y zO(^wK3rS2SUpD+E8SJ4C`cE)NZD5guoqU`CjBBr7` zL6F%Ule6eX_?t#!yNKqObAl4M?MR;ejaU=Q~Ta{WiNOv?d z{SkOvs-y9q*09v?wZ3fU9Gu2=_TX^}69d`QMlncJzvl>-M%RG5z9h+n4yVl1pnnr) zcElg#-lezJKkvy5jIWceXZ>01qr=YxJ|n#wS-WCwqXg)uSvYFmfVZ5U$juu3L2)&M zF(7BE?dk^Rf}Da~KjXST5*35t9Ezd9;@(hPoNB?UHL5x{&ZIf&DU4-+Yk*;t0Z0L?<)13Ln753!P`8vKq**a7A*Wf) zlYW(8tJp=S|>8`BSojka80$AugUvsT-Qbo#7EGfuOyM4 zp`ZzS9v=;Rbd)xlUT;!(`7FAWyS*NiMzN>G&5%@hk=3MX+)US>rKHSG)mcuwrdyRD zs(&0b)A*`VFGN?^#ERO428J9c72~R6K59b05gV7C{4!Q?PZd+-R-M+XUiQd8nI}_? zN|&lE8zLCb1{2H}xqrAj;XHuEn*HuV*jY!&Er*Z2km{nfK<&Qiw1>?ghK*D)3Cv6Z zbw-ppPM&h>5j2~BIwfX%OlQE6VU8*y-cShn}F1~!ThngMLa ziu(H?(OzrSUdx4+=C)M@I#Yh_Ew9k~G=wkAE61vvB=cN93RkJ)wDmA&4uy$|B1RRb zcKuePhd`N^t3a9IaGOMivwmMB*jq~Lej#Fj%LjHbVq5# zTR>gsm0ZyIg5$>U!9=`i6VM@oJxv7*5dYcfoxocf{Bp(^9HX$Kn z=?lrV1qhEUWLwuJ+B2#H#xrVj#<&Pq+nh+f=KSIJ=G-Xb85IT;bt&#h? zJNq?#lJu`?x@t$7hA_r;;IOmp?wg$@PKY`rq#-H>b;;stH50;)ryf(%OAidgX4T{< zBYjW`p@YmR>rxA~1{A&W>*;wOvnRDh)ghg{H(6HTbDvN0dc?l1X}4r%?34oSK9Eq2D-e*e3X5<7VMC*mZq$+30!q?xVo7C;(a6AN z{AZ@Sp|AUFEWO%E_Cg%i4wo|py;-^~%S?=Zq9uy8R@VNi1!Dw`huFrEn%Ok7^A@%S zoS{eY`*svZ7L$kyHCqts%ZnhPotO5cmPx>po#ehZb;9~N8J7<`$cogB+c&a+WgyagAB2(C>l zC_)$`22%KO%I2s76yhLrK3Y2tnz6L+56LiUXwz1MVyM*=w+Ww9isKYdqeRkTpyn4l zfUNwpSj>{{F5#<0gQSp*P#sr(OhL>tv3aq1alN)?F|~?byoF4iCS~ro;4vw$%}O0P zqRF`Zt}6*PCxe9kZO+Alymr)kxewN(xe%0 za>af1fHi#Zb>hXnB_%*bg6Ku%%t>=an?h1)A_ISFS~o^wK{k^AcE(Bu%roS+XTL)o zyt8b)yYNmQ9zc*3gZ#9BsGg&p)M|qaENxVqnkDRP6Xcw+Vg+erM`{>YYee)P%2*ECNI$A!&Lf z8H-w5AG2I@FNsxFG8=ZM*lfWu|E>r`CI4)xR2CISX%6WjsGFl~PBr9ElvY-FLcP}Bfq+pzN56MBAVURUk7;m`DwhMg(eqZ+LcOCFOCOyexyKXB#A0smg zBj6Fo9xvl=uE7bFmo3C?46Tp5C@P+!ekPlyQ1gDqe!FR;UTB={HOpw4;+WuNw$CX` zEv6N73kN47SSD%#e932MgA)~*p7ovUtW zs=$hHSf3}^E8oTN>vWx4#AjeK2S;7E`z198uCSdh{AX=k=`}ydH&LtII2fzbpP3%r zM;Ac4(|%NC)#gmZu_uV;>fe}4n>cD|i*@g|Sab=~%@%N471Cdg(My=tb^P7~!k@Q^ z+H%al&w`HFKUD3iz&4MOND&X&IKv9-t;bkAqBI5S#jM49%*Dx-OizR>FgxHxrKj5k z+`b_qD8pnBFQu>h9*&$`^;mx}>~#_}>ntnsN~WJP`-p0qtk9I?fA5$rWovyJVE}u+ z?ueks{8(TRIqis~NdFL#u5#C~lqP7=VkP+*D!j^vXp zS-1=E2Q;+yH&N|FKJ~(rX#)k)I{^*)IILU=laS>5D-ffq04m12VNmJ?c6M_3-XFh{ zskTJ0hU0|v9MyB@1*r~WedCQWkNvI)hj(HCZFrdh{l-y`LBUg_13nv+K7rPWLxxk- zy;8N+-jw!l0>mkUvr$NdW;*vLm7$3N2ywZ97er9QqiV>=y?3lM!Cai+-Dp9@vffD< zf1^}j3hET_glNX-Z+U+D@GU8DF&B9vmBRM*m|&RmhlWlFUa`FM*CB&F^bL_PVGVMR z*;<$9=b{*laZb}jsiYM(*fnv^nJV{w<`ae3Hv)WW%zZhUN;uv+(TK6L$d-)lbh}m^ zEb@qx6|}w-frNgvg3l~D{HAX51wW+II2@O2HEK*?mU!RD=X#%VXByz9z)sR+SXk0@ zwOj5*itM$kTy^w=$#m&(8*qo35IgV$b&K=7W{2*FJe-|X+50LZeR znUm7CieYn6jP4V`^FwG#`mlAP`dqe+Mm#+)7`YwA;uol|1#jIE0q4l3gzGuB3=>)u zS{YWBMq_j8RAWKeiF7-h1CTt^#UIXjcL_TsR6xeEPOffTsTUJ)yWlrMLcl^lg#<-3 zt9+a`&o|eyTe{a*zerm+@IXPbnd%V%Z^6~3uIb0;%GZvmM@lJPpT5CX-})Ygd-}nZG6mTr{#Ul zeO!qpndN?&8W5vVfA3Sk%&FIizlz9d_lU|4XtR}sJiuMQx#4Tsn@S5>XY^eBk?m_Y z9^_m&F8XZ=J;Jkk%a|RpI;J%eyM|-o%5a!+Mc|pckwHE>r|508?G>_@i8t&}j}}`- z&Dc-3i%o|dUT0Giup~aUhpWi;_M2U&Y1J3xgt2Di|yY#xWxHMm*!YC!Q$Tt!f zgj9>qDIg=HM%#I7srd^SfZ?5^BD|Dya@;ftHDTSjSc>kgeizPnCH<2Y6w9J(_{L@7V^!RF-SYdWwPoP z@J#Nd`5fj%ILjK3bd0f#INsIzUAiMYiSoo^a*!k=r-`sTnbK;FxSd zTiBuRp49&0qy4n;#sP1vB#t;g9Z`c&yYD)NzJkb|(sy`;1eaL!;lLBkye&#OCQ5~2 z?DxxO_;OgrHH*M%X^K@dB+0+({JBH$=A#iWR2xk*@cRXx`i0PZ96PA9HNNp;317qH z7IEOtj@`aCE1b?9BWHj${}COL<067*fN)E+{Vq_c$l}-Lpi@GY#q2E5*jX;Un$ zqYKW7jT}?A_ISC_MdbOQxvHj8SW{_JV=LOCK1RLgA7y}=MTOpM!VE@E<%xc@YYR?) zlUR$Q)cZo-Wj#*8d#u|B5sI@uxGSN&80Klg^rnE(Xy#|ww-}QdLd~F(@wgl2;4*HC zsLbpQ{JxSwaQR*qMVUl-wEFvKBJl^cT`@+OwPnU?e>TapDJ0i0PYK>GZ$Z+Nk9Gp@ z!$B;D43e}#qutRY2%(7T6=W0RX)wgxC<&~5XmkXrWNqf))mE&?O2m6u4`8tvF6E!J z@c!QFk@KdPqM8xPo)k$_DeQ7`HFjCaw~4hN6OBpfy^dSw(`M$0$}+OkWlm^w`EIyt z!;;cF>X=1|B^n4Exi^8H_&5UI$e6FLvD_3cqz5VC1PAh(-K$(0Ha>VKRgWlCYjk_GhTZmQauOr5Z#qvDS#pcn?G1deUV{nWx zyVUqs7GxMrA-eN~1{J78z9!9!+EPKaaWSq|_1LBo1W_nUL0OhD331WtP*NM1dm7Qm zRRjbpju58ky|;AHaeXHzr-nNFwXnAOacyZ!dlZ%7f(%fx1+d+3tZtysxRmW7ykco9 zVqx-8)J|5>*Bl5A_-Eq4?8b2rVT85dI!SRCrWy$cxRO78s04w$H)>2b;Y2Fy^p0?6}v~D;2dp;qhYLH z(lTPq`spYe%X5h!5oeF{8f=&mV!|nRfBBu5+_uxr!+zW;XKNYL@Qp%|w2zKv{@_Yy zG1J#^gK(shR}({k70YW);*ya^2q|wy#B~#MlgZ zy?u;)g~Xr_uO)*c>X$MD%i&aeGN&lbeTeXhE;iF5gqVcT1pZ}hmV8T`UQb|eWlWd~ zGS;}71Q?w;;TP(6jXv#X|v#mYT0Q~6Plo$)2r;`T9|v5!11V%jnu_gJ*0r)a?J z$oZFq=$di-cMGBfb{jTLcbwk|nuCO=HJDzi)lu{o*(QvLFSiJt)le^^%0$*xa(F=0 zxr>DlUsNIzLKRY@iQJ1SqEA%6Z?lmqEU}}pNt%oT?+C|E2}Q&T)2VdlWAQ_nz2}42 zqFzdY2SYcy0`NQ3A<@2ePuR25ZIS^UfWB6&UFQDqfnulH4EmCeV8>Z-H3Mvb*_(6v za>4xWMeAAE#kYNVrlk8`_QQ7j(iywGyI&1B74%i7a|6_qViOvu5cHJ=mP{`1GA`ea z5G-;5^zlmZ@wEoxgBvzWWOD|N@O6gW^SUa{XFDJY3+L64jhXY;77>#zM3@8=mmyViq+v&T;NV%hOc-tFs3<#eF^!12p7s5+4AloFtH%0IYv`bY^Z1_sO&UQABYcG4(n?2@t6S>6~QOb=p zXxmf!Yc@r|gbmlmj{jN&MczcpEwOPEmpsag{YtnJSNwo(MORS zpai1g>Vnd4B!qAK{EB^a4gWd>JW^??0mK~1kl$RJ%cdhYP=NDsgqb*(W0gy=>c6{1^g0N1!)v4UiiS2mu~; z00jH)Ddv=(Q0PIbs@A~E3uIH@b>G90I5eD4mMqsOqR~J1NZB~G^js)LZcAODxv zBi{2U{$`$#!WVomaD#p&>M^LIcHm4+NZ!?1wJ#LFu^F3vYpwQ|`dMVa=ZfMEKc#*k zt;hmJDpsywuV#YYdyVdY(L$-&4rYN()TZB=8r7v&V}S9?C07C?2bKmlD!148vhYI; zBMc-y$8U2tmjAZe;TY5atl2MdTuXZ8W8yL9V*~V+@AYZO2iV7Hu;u^8kbkh(zc3^# zJ0tr)7?9x)(EA^RjN!k?-~WclKB3#ccZur6j@b0z!;3t4f{i#KJRyW*4OJ`WW6?d! z&v<_cw6`eghv0(#@G?y%PiPir$34fPHKHT-`k%8sp)HpfJ8i#p#{W+dV&V+Mj;H z?1q!xR7(k~Pk{Yu^g zA6D5^2u_IJIA?)-L?DO}zyMN#+~1~pykvW@2NS$Iur=dzw7b;C{X-eX$3I-t`Bf7uB*8vdcq9G`RZ4}|>_2vNHGvfat!2d~v{{ZrT=KJ3q`j774?wk#r{vzSZpUnAB z-Ct5cfTQsrmH&75m^(N+37G*L{^;L~R8ygco8!IC|8xs>gJJTOOpYR@^o$+sv zOl*H<3o8>FJ_pC2PGV=F|J3{=`#gW@e6AXQu0DPIZxOJ5jx5{fIJ0nk`u{w$vi<4! zPyNrS$;AAZXI7TKt{CW<8U7Npva|iAN{`RX!Srd!g3tVE^v8ge<f&{~P0HV`cwujDPQw@gptNeY835kDJvn$C5~` zlTcm%WT-a|ukuw-p0gr}MjOQhAC2wvjgR)1M?b>H*8P4+DKU1ipvTubP;KEDe zJ0TGKP!$yyIM+?IWj>!W(-6i3%HliESM>G&dEDU21C*3>klt19N<|&I4DZ>pKIIeSd;8ESe z_sW1<8VZ{B+y2U1D{;m=v~BRF zBzob*xR#0}HAm+fG3!xUe>8f&Y={Pz;rRk@*};Mw%SjhZ!#1;ZG(Pvcg`|NGL8_Oe zS73XvlAkdI*y8tmMM6$$kus9NMsRtERft(IIDzbxm_#1?ctLo$T6rL+P?@S0uO$Cb zBj&Mu`KJ9{D9uJ{mIw}R5D(?R2HgapK$oqGl6b&2?nT}lNGC=jJ`)SEIMeoQ+5_gNXZG+51yPLE(%iri2W0=f}=sy^cpcVTOl-j}QshR2;-K z8GM-~X@P1Fs)I0MstCGa9wL^ID3%eErij=gFQd?V7=&Q1LUFDQk3nNo;XqaLP;y@hSO zHNpS!Tw9-9kNz7!MghTnhr&#AD%pS|e%pEpf-2^PccL3CAuaWgK{{S2F4_6^1hx zmw{i=%7lxu5aq3}Vp+!I@VIk6S`R5|zw7=t(5!FH5RAt)c5}o;a@FB&{)|j(MnMhQ zQ_OOCf7aM)+ zXmPGXnU@;s$O73NuU{!Av@|rL*Usa?)KFSnTwGpWtO;h_9qki1FPknLqhpcp7T0+7 z=7=_4pT$==U4waqQM8m5=L!;|7!fg>tZ4;9+cw6CY!&IDgGm7BP>N_7D7TwI?ieeE z02uhYv7F4$mFhIuob6#PbhLWDacXlvc)vW|oLywC41bJ%4AWgPG5e8DgD#8Jpslj0 zreA@0nA1-Zk1;jPZvql^=w$v#gO}6-n$6Lcrrg8{ydmJ-N}$qgU_Y??KHr#n+Zws z5%Qhw9fgn8d|Q(k_-VkE0y9roIBAzZFY`zClGC?T3~@7{eg^m61o?xa%C+x-%$V(_ z72VTCaeR%hD|}Z`ha#E}HCeVi9-!|{unX2=yd<-xP#VY(!pWdDQz^;6W}vIPQlgXP zk+U`#(UA~Ez>5lkL_F$tDogq)=N~moVT!K48XA;Jdm~b%gc%QIGE=jBW5S^e z<+#R^C#Ktv-ZjX8B`~;Mp(C}E>&fjnciSs%0L3=>J!k>;ly;4o9j`~&$6_GYr()gq z8|~v`+f35e)i9??G5MsC7+9(&NcU(fP=f$>$FTQct7zh!8VXab9#u~kr~7!72o0bt zLF8ifwO*OEdFr3h*+v>5#QZ_zKqsz@nUQM!TGlh>$;>FS*qE%GeS098H0x`vK0aAO zBSE}#aHtPQcUwi}Oa@0!g-N7qxc~<1?Tv22p}_hC^h$B9Ayh|y=}uCFB!N1r4huQ* zC|O}j3A;sWS=;L9m4!s`u;N6z3X9T>#wy)v=S!e0uVHB37QrZQ&zSl^cY55XSAu@L zC;RY=M}=^gS2%RK#PeBv+y^Yb7nP9w!6Feq6dB_c9jbGu_0 zkCCuIs}?tZGtU8Us~Zhzy=`nuv4ePLTD!25wL zlS^S>zJR79@cfkySrm%)o!pB6oA2NkO_b?Iv@0Y1W=LdLD|rDgZ80vPF2kA1@tyNz zG(1*@KH(eTo1zWyaXHcw1Xi8Jyy@IxRN$Q+3>P-V^B#ewd+*glhG(eDr7GsHE2i&J z9Ti7ih6AuTc#K?UM9pqPhgG$iG9YQ^mlzCGt8717f zhsMzo;VZkW%{SVzi1hj!-sC<8^}4%`)yOQ(6P*^v3J?{_wbxS4t(lrbUqd&{^iB%e-@p69Cph#a6-)Nk zq31k?KOj+n?rnc*fVEBf;R5eca_>TJ?fC_!*`O!9N9h2Yyoi^Te2He4yGsB5CsRd! z#F1uw?r9fWJq|l!hk<=ZdslosB+@HZ_w#R2-5LeFspPjl;SRxjZ#guE7;mJZ#A*Y1 z1fIp5YNxk>U-@Dn2KS5BN(`{L`2oz{$MsdYskrR(%kHH{YW6mom}R-p%2S2;8Mw%s z7gsLwj~8HH%^fDhG$RRU+Qf0)Na&FaqsF$GMUqFygCUeeZL{T?k3rtk+ne6hzh@%h zWRp+iC6UL`EFsXw!b&)NZ=)I7DUthK!!n3KoG^^&D$U72x3(i0QRmt#u2VdQR1zj{ z@Un>gmH_>`LUqqoU8ERLWQua5@`iKK=)L#ma;3@+ajQ(UX8-E|@t%-|^pL@X@X?Uf zD#@sjO+*D(U~lhE*zW>gopr{BWkc!)l+IdO;%l?l>|w#ZJ(8ZsuF(5Zs)f&dnh$!b z=i`r102~eZA)EEFmX^EB3C@YiujUe(7Mo0u#`Npy4Jlq4Jl=TGbj>&8r9l>=W8Lo@ zdNNlsI9&QmN820HB|bBIwd_R9rRjm{Dbs!l)u9QDEXS4ibr=X#Tte>`m5gM=`<2ku z4Ouh|75s(f>`TGH6ib4hn|)HA`iIc&uW_*M+I4uPrhN{*v>~Igc>vu3;VucV0^KiN ziaD~>8eyRPp0VhvuF}+`Jw{H*Fe#j`e4$doLKHtkg-lZPpJ6x^jDE_&!@Ce7*g5=e zIEBX%4Z_>zd&4{le*rv{iRSHr;UZViRb6 zi-j#wg{)GFJt?@Vd#byu>wcT4?3*^= zfIU$*QtUIPODJ2Hhkn~NKsYjA%G{aTUdJxW?=sNBcS=%8sn))G$4AQup z=$dd#V`(!s);yIt9aSpsP@7Y3x`^3*mKaZN-J%rx!~T|at2tL)>>&7JRT^!hDZ#Hc zfynv>At-Lp7MOxIO@wZkm{&CxrvQ_!wih=<)ySakuwQbg)R1OpTiehu&@G`P_aXTl zuWi8nV>b4Uc>jLuJ&p_9B5#Fxnn4pzIYP(TSy@3}I5D}eI**z|Dk*=0(Fg-N7b+v5 zy!)^_xqGp@vHP*xU-CCHl}5fYh1xOYT|%Iwp|x^Z&AbYh4we#@7M2>8UfO2KFH|Shlm^L3uz)?DJCUhm83HcV1<3fWXutQW6_o;qIZ zEKS*2cXu+=?BqhiA@daQmiewEKxd&z4v%8t-dcm{o8e~Me)&VDP)ms-)}9G{3^#Zc zgUnz9aJ?!cqT2h#8d$1s(~cSu&Wq;F-Oxc4hl2SrLrPuHi|@DykMnyi9wHmvZ8K&! z7ZXrXBsy_3qDzVS6?1g5k_(*b4K{hOOtc}_LKKHlq(^3NwOhG(l$GOK~9-D`VH_GH~eJ9gAwBH0~qN_HLM8f5FS0xSCUds`Z#UA?F7#5{YNy$dB zn)~kv6f+VX1kzx-{R7vY!9>Q!E3e@gS-j;f8)i6P^udD8O`8)WPw9BCHPzn_z5+oJ z&)@X%d4bAi<%Rjn3qAA`GLM{-r44Th+!M{|4HhrP!?=Wa`gB|bvESF{W;!*$qO2Nx z>lL)P;@4t>B1{%;qdrodocjr7hRj{8;|Jc_-TJ*$=G`?S)6ApwWZBSqbcLjQgl)zg zMK5RBmGrs><|#%(yOTNqLi_?dt zoI#)mgh~R2ofs5E(KaSngwAgxo`#HEqaHWFxdRo-_-Xa6RI+-&GGdmof!QMeW$>&) zZk06O_!fvW9&cBs)p~%2Gd8l_`NTYNh%Lt?bBE5~3&~oGddi@WNBv_i40F$CmLZjf z=*|lU^RYJYLC|sTaKW|l+f7RWWKBzbx-jnJU_p5+*TuAAt&6!A2!nJssZNcsB|S?_ zTeR)*KSUI|tSgpqBqmR@T2kxuCw-ku(rVF-IfZ!` z%TQTKfwZpM`ALU_D9SpyuALT49^PXKtYFtKofx~~MCcXF>*swfwbf|Xm2o=mfY#{Z z5o@!dx-n%q%9m_)NkC#mk#Uupw23@%i z2!13S@5`1(l;;vXgQU{z2xkoIYJcn4yZ_jbi%y@_MAC^`;8-4&1W-t}9zf*GW%ZUk zYe#41l^j_vZU}H*wWfPY!CaD#_2@#s*9+fnTG1>Ozg461l7(&Ja|p>s-z>hQ?}NN2 zW)k4KBdBS;tX1V#WDSXRt;CvQ(_cnmm=NuZn=v--7`g6%VWmX6iLMb>F3Y!xXPey&9o$)6!*Pa zo_O|$oUXx{AZK|q90$G;>C*7>5~&KVtMx!*Zs4dZ1^JROokRApH8_uW%@o zL6`H5BFDCsBWG*AEa=y!mZIAdp9{feMJq~%{}>2ep;Qu4M2^=}bZ_f1UQb#$LJ!@N zGW?>nIAm@5@Y?~+enbFNwf)ft){9geK7T>Ud&n(n7}xgWwc^golb?Y0Vwq1^5$0EPbl01ui9p#RR>R_u|8~_<1`Hf^x%5IkPmPWaCoj!c= zu|`EqsX_Nx(t5u^h+Xw=LFIMV^1V?X*J&gOS;u2AySJZ_Y^5JNqY&3;!Ed*N-=4BL zY~Qb$yp0gDWZ>W?I@z|Me6^kSiih`IGH%2@QoVJTO>HuL58*wq7sg+7-)O&~1F0Zx zlKW-pM?cdw;6p8AilL<{Y>?1rL1tNkw3)&i^NM)yh{9{mq1&0^;=5bM&V7-4IhKZ5 z*dBAeuCdGmdc|y?6rFazl7ACE86q}mCyvc+WvRkQFr_?~%eA?!WuLbXv^RSM9??H2 zduCrE|F15uJEdKx$p-u>$_RtgHBPC7y0n@?Eaj*WDRPk%DcU7ATu<*pzFr@Xw)_~I zKiJ^b)(bO*ArAro9G)=q777W2km8NGY*Dqr9&d5;<#&v}R58%Th@iaiyCyOI@=*lZGb zj?)3#xx35}_Ns?+g_B>WnEfA)9n=E%kj+Qt263TGtHw`b;w|O;&k--%`oKJ(V?1&A z2AmocR3T{+9Lt#`Kw6+)c{HCVI|i*s7UvekbF@8KMRJoq3}ehl5L9n)lk)gU4hdRX zWV)E?w4u)L0zB{%KWIbthoDRrN2av%8avEUyu*Npg%whRqs+SV()YVBA0IQ1oFJ$; z@8<0TajS{4q&tzEHxM6g6BAA0Hi6$$r^W~wp?tC1-2#dB7FjSaayzw~0Pn!5VL%n~ zu&`%DFKZ1y>5r7-JQas2RsVNs|vx>AV&U%4BQ{PoX-vF}(3cb^Nu^=xc`g=1LArZmgp0 zJoaLNjk=O@yDM?i_*&)~+41W&b|CvHQ5SQL z1A)bAMeuPd0HOQ9RcohQU4(}DUQ7kZ?9^t&b>(P~qeH^dv!^83&w1e&tI6YopYeHB;0f5BfxtjHkIGG}zvw$PTEHvEVBs;Z64rB|%S}x? z3ya)5AFWI++D^Kf{Hmtv1%9Y}P3S1XTFccJW{QG$+#XO)SH&%n|W^vn9;a*~P;UM4r%?B<`PSrEV zyH@i~j7F`QlCEFYo&F1+ch+KJ3>J_5&f?EzHNRRtp}w#C{YS4Sr^LFr%g7~>!(lRd zx1tp*xL#W~CuF@_9X*-h3lHaBkh6FH!%QheI5x6nSElTJcx&$@<$)oYsmC-V7mp@f zpq#h1@jtsiw?+s2d=0+A@?%V#KUdx6v$}3~)}=+q9=c#3z+T+TAU4)D(1JTZIc+>1 zS7T|iHYZ@v*y5Q#q3>1~*`heEC}3ixvgKt`b68%wm1buvbpqqO1&baTRj3(#*qO3} zyRkA)xURjaSItpo$N4@2=$ShqTBSWOkLla4Z8=bCAcWwp`NwMyA#bfX(&5&`8>_C& zV3$-^E(y~tJ(W4IXK-`a+CMu0FjZAsdDo|++OEwuvCJnnJ<|Ev&8-I*VUo}uF>4EJ z@W!@?(D$Kl<%OuXRkCOa`3d9kq=}C9sklpOV5jqCbY%msJYxS}J z55ga4X8OMV4dFjc2+wASzYUgXE&XpY{(mzU=gb^`yZ&wet4t9#`|~aK@AfnIIzQrD>p$ZnWZF_-4*x%6Z~$nK zzWNx5lmc`093meB|LJns;p-t%PE3#d*D^FC2Z*c#yypPp5q PJv2^6Q`6jLtIYoZtt|a! diff --git a/src/bio_ik-kinetic-support/doc/pr2_vt_0.png b/src/bio_ik-kinetic-support/doc/pr2_vt_0.png deleted file mode 100644 index f311fdf8588a2e0144465570ead90af16a5df1cc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 117770 zcmXtg1z1$w_w|6ZNGQrkNlB-Gv~&$UAPABo-3;A=gh(jeJ*0q=(kb0t14s>xf`EYN zcjo>5zw3kg@Gv*d-DmH$*IJu*S{h1sh^UAl5Xc=BWqCLRf~^6*S_rVgcaqJEKZAb= zU6c(F5C};R<_jx{lY|-qp@*o*%jkM%9<&-HF!y=X`z%Y=FLT--w^E~|E$|~_7#WBZ zu_^A*v&*nhsV5Gz{e)RHjfedW=Q+ElcriI_BbXmelVCGBFD&SKOF0BW6=PzzKe*@? zGlCj3%7i&r^?y3p+iP8XG#RB${*nBnL9PF_WZ;AIlOf*F?(Z%lG)k0zb02kJ{uk@_ zK0z!3e3Qnt1^)(ek;F%=ikv(+_>_1un8ZnXm(d{bb%~hyD@a6O8AG0r=)z#I{ zd|RCwmiEtzii@j^8mrsdj(p<2MXZ7I#c?bBdD&)C)&A#XNJmGfs{L{>^7_{_kH|X#d-Vo?My_03ywwf>oEfG!$~j& zdv4^Xx&5}|3w+n~!nU9u<0`N&5+pPuRVVF#hT8pqexn@PdIE77eQ1%1V0e3c4Tr5> zObMO^^;cO;0kK@*hl{5kC z5PAqi9tYA3wTO=O%~txUQsI$}_Q}3(Yp=b;^^zTW^qo6;o+33WEcxDv} zh0xC2x&4W%aB?ys@Nj^!O-}_d3$X>?w2|O9&Af zlDeicjIRQ%s%nfD0S$I?FF;qX4Iz?Bi$(wnskA&iPNpWCRGs#gafa830e1_Z@hs&6LS7A zkDanbk$vPZN_?W^IC3gcHMftnZEF6^Dqpkbe8IWQc{W)nuroIl*hsN_+^~m<@>>Y^ z!lQlDe^=k$Q!a=Q^&WMTO8xm67nE7VuD~8cw73TiXHcO9ulD*T5Ug3e%V?IQy%+5)<#j$gno@Q_Oi)%E@^Z{)XPrVAfuHEm9eE(RR^lo$XUZ z?;K{jba)O9OtcZ>g~DxxOM5ho3A?q}E%5~$8}Pf?*a+#+%jhz?^>g}H%PDHN={yB? zkUm1MkUt{*F+0f>M}%m#-+C;Y)_&Vzq@XDTJo4O4U=D0ZU3Y5bf$aQ*#>Qbkr5?xq zLG$mKs_f(w8{-6GsFwK6HuO)UH*47%xhZ9Kk-=pYVWcMou9#Ok@u3MU`};Aee}4ie zR9^)Iw9eSoEje;2fffi-ebQ*qcS2 z8s+hRom_JzQ4{L~F%pYE#Kh}idb6i*3O3XijZqvrzgCm-8N-(^?duG~cHIj?lw3wn zC4>_K4mfq5-M*iZAeKC2DK*yYDxCZrDFU(i<`$c6*0`Qc)+$n({B`IhxXW1lE^yMs zm?sRr1EQv;rtPj9y;oOaD!H!C;a@^16|~cfbPYA^y;&N*4XZ$j@eGm~UXkIbYvQZ4 z=jcBfNWw#=!BY_|I9H52mMzq*Lag4HwOsY7MIMQ-UXD&_N=fQ%nx*Pa+SM6W%*Id) zH=!!OxGNscf2P1v>`H}5cLy946lUQHsmH&4@T2Up6w&&K$p`jFAMGoW6p=EpVYgks zK;C<;%vFjzIAb8VkErA7rwJLNe?)VTK{kCf#OCbk)I8sDTUjLNC?0o(vvVlYt9#vt zJU}ZKqks zTFpb)g=%lVxK9tpo4nw&3nvUwQuUfDiTx^w;Z|g=)5Buz59l8mRkz?oJh#&v3l( zwZeazHls1TJl*y+F@AHZ_LX_Jhj>}jX@Dug$Y<2hK0LwMf&dbI>j@H(RRk+j^KRpH zDx*d#zX#PYWGt`1vf9R zeMJG=Nq`iw9F&N9DI;@0v0B$=Nsu%FzSub&Qc+me>|1xxhw?NW^iW}f5J|^Cq6Man zN{v9#%0YLXdMBTD#09J1#nTOADy-Ki6T631Jx#Q(5A~3xY>HC<#YRh6@Y!>6jM3RYda9uz<^`Z8hg})QE4>y}qo%!dm=b2n`n0 zn}Apvi=zTj5e8RN8FoXR%^&DK)V4I(`(JdtRnpwENvh=Y?DO<)HHni=$;imq{5Nd> z&|<`@Mz7q`VAkR3VOQu42U!P^bL_L|tiyqDtmnQ@H&~E~x*l_E78HflwA^)Zkmi+G ze(CUjtoy-=BZZ2{R9$~drgC|Qz_MYH-cXs%cs-CVH?$;P!7u(^A8V2rSDQbLGFG78+ zb=B3?wY9Y^EyT06qP~|elBmL%%RW!F5l}Zo3)}HXj@2aS?W7ypSJa_>@;*fG8o5H6 zs0!}%LYd;M-$eS5xE3LGGuc<~!PqxgXu&q)Pg7FagIe^((!S3xftSuFbjB=#2&ww? z$*4UrfDf9=tTL~3*n8C|F*YT5=q}cFOj-L{s*W1vygBk|a{{SWV_P}jKh2ITp{oR-Z)huI`s_MjvazmC zsDOXF&cS)`T};6C|Bjh*{SA@IxqZ)bmgtgn@~78!xST3+)~g})wyk_WUUVcF-M)hz z@n$l^SJ>`2cq<+a!`gONKpOF30D@Cz0kenU{>X^>U`tMRHc3dZd~9H4WhDOB)TjDPfPes&=I;MfbcM@=z-$blITAE^|BK4@H zZE$2Fs{v0+H=l&ZUgGg}!IP&={O2_F4Zq_^J`yJsP112hkjcKyw)(JTNbe2FLvtvMojOLbebu?naIVsrni!A8 zy|KPtvAYi|SC=CVw?f-TU}>|-u5R(qpFhLHYON-`pT4NiL#f%r@{YQcb*G&46~B?n zMvhy*QPvqC>lvR!5t~)#ouK@^&4?sp=)Z1fs-}_R-J)7_QOdy}aT;zqmD13>MW1Ya zo|GD{m&TZ*jurar`X=t#RmqIqbB-i{hOcYBq_NkMhnqvgwJ88pndnLQGUR4u5wdt+ z?{{>9Kvg~?PH*b*J3BTVLc`lQSh>y|G>$Rz;>a-BUTIgnXqn^l#f);Dnj4F3~Q*T>|(U9Q( zBv7S=*i=**H`g~dF6^JpuUu|~c1qi$Yy`m)uCM2_*VJ&M<|mtOHU~YuEp)Waa^_hM zl4`X(nHUOjB>?IyA$dz7TO7aPwD*Cw<8^Zw!7P)_CXp!Q$+&DJmue?BN)%iMBC!X@ zhZyb$wy%OdCsc+)!5kVNl{mCTPmn&c{p*)|qftHXr(Dl_IL^9bClbdO*H%3C-*?_p zE~eBq-1ZbvcR0Pae4i9$L`vW~9{5iY@c?AON2J5!^hqjmOkfmc&GPEJmvBO_^NpTAg*wq>dmRstHg8aSAsm(I<>2&i8az*D;PpeVvQwInAhG@QM#vYE zKtkx1<8jFSTC>SQ(qF_zWoipazOHRNebMiH2>Rx#s@n|NKHF(_bz&SAE_`+OWD9N6 zEz>0NvT))a93B|>wjD>v+GTP65Q=Oi>QiIl|9p_8Sou*PJTs4M8S{=}`_c-SJRoG@ z42({Wj@nNxpyAi0w9ivmRb*rG22^8=tpvtpX&Fby#&X2HoLpTeYAql7RWCsO)T<3t zK=1NuF>ZlDy}(n~1MOo%0f`XQ$M(ASWMOS>Pvl%8;4J|GHhl@fL9!U6?rIe19GRXCM+cCZjpOn<9!&B1r z4^EBl09q$o+NMv=^cLB~FKSziv5`HN(qx`WXKG&1{zhVEcus?{7A5Wo5QlH;io;WDD=Wn%v128fN<~x4yW)L{TzypH*VX4|yR6)c+7BQ3 z=%n}W$@HcrF3M4OHhj+ON>hAwU^SEVfPgL4Dw^|g)*>bn-WG6qK%~!JgYGIgaT13k zjO@%;lDONqRU^y`zp9q07mpd4`10jTadGiX1jljBSc5Q;ower!zDxFDyNWJReV9)+ z8ZqK-`X-JZ@>>~F^TJl+9R~qDxCDEev!i2}xd=mhFQ}!l5GA8MNbnz{_SHED}@ zA9oT)CkKZ=+rsVIu*WKHfB#&5ah-U{gX(;CKI#AohDYJV&0fl8b0F^%$akJ^7Dqq5 zd&?|WNWcH#RCMyu@+Yt}T?>7TKoI+{#%-ih$xT(&FW#4rmH!yH_|(4r{ZyXBn7+P# z4VO5eOa>BIFK!dZUNzoOOprJ%+F2`>b0T+I6R~l)5hLU{7B)mfN>uMYc3MOlQ+_T<>eNR}wV zN*3g&!KIJU{Jl1VZ$$0nWACIKL8L*0a98&2v+y>U8x&6eCnfML+w}`_$85NbY zgNSPffYdhfwtmKoLC9p~wQFl?n2^(kV9S+x3bw6zwO<>T&y>yWYg63sE$>oprR@~u zO&OY+n)($_7-!ClTEMIPQ7$<#zBk_lE?QBgrH?9}5*gCkQgG0$5qyf@(Va8Iy! zL5R>Z*X`iU5za?{X~LQRajWqYe>Tocf;eQW_3<0r;-0J9tRPDEV^v9 z?AgVO0MfgV$J+D+_Jyca6@o(lZqe=j)a}dxfVi}8QDSJYX&--3pWktSmte%hlz)8P zYwME@Qc|l)PF5Dua~Qv6`?abPimPt6z+`-7Uh|OX!H47{v#;qkQDh|}sblwdUgSsY z7_{nsf^3`k6_@sA+G0^bb)I;sdV%*~^Wz05$>WBo$;rDA?`-RQPZ=@*X!(e3;hGGA z-m2%~6?6LmCj+b~rv_b)`!I5rlB|WIk8)vX{)N=u`H8B`({VH%HQQ?{NkIXl3$|rC zzle^cbFyFo+(^j0lb4a74X7z4r(eNU2pGy{IO;_5(n^@2nWtZ7u<@r`Chip`Vj$+T z-)o`vbCp6=F}u6F6Vpi{&YVUO2o3d7BG!yyHN) zQZfF>NBqzN<&)VPwS5*~i;HPzhmE%3>`?n;5P)G%FPpdo9V?_ZrP)fql~B0&n2ewa_@s z3s`04RQ%paPXL(v>zluT4F!dYGhC4rsXPMamb1 z0v9G+y{O>=)R3;(kx#aEvt6C<-ZW@S z*qdojUz4~|ji?DP@D%XU%shqK7Qf0-tDLU)jZ`)+Bqy&cOP0WTx0cSoM*Q&BXoO0- zAN4ziCt-9Df}-Fz!-$hQr`=J|C2xw9Hsexc7M_m_?u-)9Ia1m+v6?!b5E>-kfUw&KpjE85)sqcZokb#-gC@KMBhM^~;qGyB_Ot&^2cES=iKH!plK{@^VgO+k`SOU7m zuiYsN>i8HQ)cy0gC?0B_-Gy}P9|2M-cUMnEZb{$A3y~WXK@;z`M>Ft2*1D4D*O<$Ujl(HzR% z>n*4;zoDUKv5w8EwAMQqrlMnSfR`EZpnXQ6Z{MU8*;3#!j#1ot>|dHXnJYL&Q)^`? z5JLln(bE4Qiw{ND&w+7BT8@4 zFM5J{f>Zg1lN%!Ka)a6luFf8_qZOgjF(@Ti`_2e8QXLDe%F_Ne`{ z!0V+|_~{M;J$g9z)YqL9oe7t`xa}58w6vpf-ASYr)4-{_p=~T78T>An@zvV3A=3uE zNF-`T{B!cpU@RL7qOWHCP=ZnXP%Mk#$7KNVMc}h~M92_m^Q3rrdaBtr?tW8XWkV(> zCzJh5v(8TATkr^RUW)6Q_|mC1Pd(37Xqld1dlQ#Qqws1<5b4N{v^>Gd-6odogo52< zoi4Cxc0h~Gha19ALG#cet-J|z*rfL~_DD3L;V37402iwF>mFK|L$m)w`akEtSZGsp z%421Ga%_gRo_fxn*=<9}RxK!?@l&pgo!J8lAZU2*-^L@gK59%^XNl95g zV^^VfyL)_Iyx`9E=F9NM&I4GKKWxmjve9tZ-3+dNNdARJmI&(f*W$&{r=B!oj|YE1FANVm zK%2~pKq~q0^;M`i(ufBD(5dD>cTO4GXJOh+3yzSA!frEbe3=HY^HEs^W*VC|i zc)js$%`VzS=L7{F%=5Ev9UO28-KAclMo86V_}e@&+!u*$MmfbZ1B%V>`miA<=&z3< z)3hAf#Png>bHPQG?vYdT0$yJ{J2zvFq_P>iW|STbX`9ZhQmQ-YgV7ZEad1Y-gly!^ zXhib+v~8u=An5{YvRX4)I+*4&*9Gq7MbPa6`ei<-GW&+ev%o(h>P-uphqbdX_TCo^ z3ny#mB#_!!+w@-7VNog9^`4i`t!JAFnsibiyWI}AMAT|o1J zlBGJP4VGu$x(+OuU`rXQb=Mb((fd|u)CjMd8_SUjYPOts8(@-@GIk|t< zk85S{00#>#-P$dPZ6^(p^q)l4Pe34iP~vK?g0W4PNB@ySB;W<5u|M z7JaIs6#?X{IQluPv9;B>LJx55#+S7iG*1so*R^i>j7Bly=E|e`82~0mJUY4YIEw5$ zrf8Y}?*&M*eCW7b{X1G0zFS>bkR86fA|aZNlUnEv%M zjp4B|om&;!sKL{gxqVB6r^3P)=S$A*Kn7;j$ozb>x+$nJ&*^Pe2jj<56OUHW(tKk0 z<^6_;%H1mij-)O=5N~Pj2@g*boe>D}mS^ z_5xC#*NTs|ukWHwhoeyMTXQMCmxqABv{QQiy#Pt@s|3h!UdaSW`gl`Uomr2%bT{Jfa#;tOuFtKbCjTd|Ea{_ zu%N(BLX$3g$D zixf`2i^*rm;p$rpIt#|z7V->L1JNzMNu&5)-++ej%5G7~^X3%0irRRe?2XTNYG<34 z4v!8FHhq9dywj#Q%%*(X58V@gcp9)ud^g`3(M<**bZa*fd7z4fr{!P{DW8+q7!{v@ zp_)tFbsgu7O(p)dhsSeR8uP%`Pzt+HP#11DUuI4Y0Ck4@Ln*vDl77j1rKM_qo}TBV zf7eK(%{qEU*$X0~vma`AlBb6Kd};M97S`2JU*80py}9=4me#hJ{quMesh>EBOypOe zD1!SQ-WsmvQr9Wdq;LOPTg!~Gl7PJ8AaX$hfg$5(7e@zPN3$c<&WW}vl-U6^0y;mS z3;$!oyeL^c{8BSA00qYiz)-+dEm^)^xlqOkX!zV3jVkom56Qw2x8j*?8dXr zTcB2)KM+uleS9#Y@&#GRmy)R)^RYuABD_)m#A`O!)uzTGgO4*fEI6mH&Vl%1XZ z%=6$ydeNhRUlZ%=>ww@K%@PND9t6|k;W=gdZ-vQzN>l%di<_W7eL+ShCeK{gH7Fh_a9oST==KdX<8vWaf(0`r{v6H{Up_p|G z5(2?8#A3|FgIV)G{fN+zRnXSbsf0juGr`sLfcXCi%5DS=*G*9J;s2$`wyk2zD=2mD z*>Kh`twe`?ZN-VPV3n{->ZQdfdK(7d;oe!sFec(f$E9DkIO~8bCfz>x;a5N>?Udv=rH&3Na&=CIGNlt^>s= zVUDFoX{%#w=2Vfm2r|4kFC#!fMYdEpSV;8zH^NMWTNDJGkUgspgVo;l>F!xsS*`f) zSIn6P1ptaX(x}m>a>i~G-TyL`u-pP6cfMr0_WS3?neR=6)F1O^l*Ux;)UHvh@7e0V zn^iO#U0hTITsg-ev?*9p>05X#TL05%hTc zGV>H{WlNvP)FRtv6kf1fqnh<104gSpMn#^JmA%U#l@i7kOt*p<13)gBywR<3*+$St zCe|Ty*Y|m%)F-QDx;U+ENG9QCv067j14@F}C?E;zf&(G@QFN5_{S#eq< zskV*zkIUZjYkGHG7}OoOhWECnjUNS`PedlavrA89b^L$}xEzi6o>Pe_mY6#kp7Gt? zt%M>NU&l)c6S&vauidJ^GmN*)s9*w+J%X>6=D&vY$B4bVX)!j}{O6%LFwm-k6XJm$Zue@%ayHT85Kl)H1H{@ToEF8JD))Ex+H>z8X&@Zkk}Pt~T4TYi7wS(w!1CBQT|@LP5OtK_#0B`uf?OBNZT5+1D*jPgAJZ6#RL=wPiVaIXO95s#6SD zAm_RHhYD$CyYb8h;Pw_5_bxX69R~g5qy1HTU@6|NKx0bKAC9{l8tCk5{%k-u-aIo8 z&XtgA1>=Z`45TU_EXI{7kDbtbf(I}S7s`8mu244uSUVvT%u+8)H1l=E!3rd`T?+7F< z3E=zM=mm~R5CWYsS=_cPIY?>Hm^Lf{^n-E&j6zUVWu=2n8Z))j97o2d*;?&VjXQVm z7mFp(-`{Q=EgW^r+G@)6DG;+}Otz3>Kk-BXl|;J&Mfwh3*at(f_kl=p#M{px|NgQFu>Fnn*f zIH%~0`>EP&s@Z{q$HLeQJBADJ^z_sd_WgI+{!GUp{-UzP-Tqg=Lc04@HeQ`zw^5#6 zV>}#|q{@vPL=Ga7&>RV335Kwk5-ctInB>v>Z1d{SMblcip_zzw|7L~S-&;Vo?PF8Y z*<0~H9r0QX@G>`lW4EDbkyOCw^~3%A=2hx7ZAcz{8XpVBrI}ve1S}(R++zMsH$!H)X*@6Q1G@Gv&{=F=&=nRbM-m+jJyR^!tQ zdmf>wClfx=9H=Dz*KYbhv|w3V90?pF&yzWkg4lfXQ{R#F&IR+25^rYlLOZ9YrwiL- zz-x})C{aOyi=7=cA3p=UtEc1gd`u{FUbJP%w7Q60P(a4+Ctz^NJk={#7{g&6dy8e$j+Xmdl#wuM88qD5$;pzrZ72#^C+Rh?)BJTbjZ!q zx|OW76tqvB;hDtTI;auLbV_?KH|$NX7jN^nm{Ag^=G=RF1gj3EwGcFx9cBPV7!fpR zyw*W7bSUbxt-x>!E_QanUmGX&*W29O9B9_rP8O9$pWPGecbia0Cns*jQxAIvP8QE| z=d^wFNoHN0-Iy$*dw)@^7zw~JgJj-9 ztGV>zp1%F@_HHI235zqLO9^(Bwe>_Dfuyi2P0au49q~^bqX)(pZ0xoDyEz8^_21;% zZYwM6jf4Zq*Zd4H(eH1+h$*ccl@@1T{pcp$6};y+Z3yhdwY7o*0*LjVcLTknu`knU z$X=o+1&s|27w6}d<6jq}{{0KQ^vSj|JhzFWbB4`{(Nb|S+rUz~eB5!Gu8C;`a9%gqf+v!Hs=#lqg; zUOgQCI794sQNQ45j4xh^Dv^r72rcRNoRs|G?NrL0miO^ga!ea^_ZTevUqr?Jx`kD% zs1T3Tva3i;hN+ec8jbX~(BZPUW%NkP?J0u>J{^8EzMwiqHCz zao%pRmr@L35#M7UVfdcHfBZzimLT|jG z4<*X%Y@^Q49eMAC8%VPnInQn~R{szzLlXfz(2Sjjz5V;9XK-9>#>rZz zUT9qjK+D^M1(bqzpj@!#_ya3qA~T_o)@U^_^#o?xRzy(Z_e%7>C9s2FKUdQyDS*|8 zz8_YF%O~X~CwhziLfm2TPvp64O|rV-rX4?#Zf(!$2x zU~s82`I*?O!8_%XqnJbaY}l%1_rV@0c;CGf3o*McV85aP2$w|mZUt~_5%0dL8r8G$vLXvnK*Z-DRo>^Wi$8L6Ch{0QG7G^6=2B26#wr|{|Cfz!u4;OXl^ah`y`64~C&y~>x0>q?{`i6!_dH<@! zfKU*n#1edI9dvFDI(i!F+S&%C(By_0!YvboYN=cgxJ_VN-TS4vGWu!WxY@H}!9hb) zGe^Q_@>GJRG)KS>)D$290w^Cdy5;jMGC?pQ(|qj>u}FRj8m@hpnf})6Y<=#O!H;Xi zVmC&%5SG-)U_WAVs5OP(^j$3`#^>ZN?Oh7G-Z{X?E%!|FYq1p8ElBMK&c=RNi>#SEE(}5>JciMjb{)yx+h0E z&Yw~DoGK@6{cfDSaYv*9*diitGI8wMl@%*9ug7&b7Xt${&5xXuCS?cMd)J}**8k*tFh)sc4;BFh*u%b`gM+Y|1x&;3 zb5eMGz40oa?QQM=A%zH1(NvKVx6iF#aujzXWefoq-4WHar* zGpW1{%IrZ$f!g}5n#!{3| z?E><6ch?remIOP$OWz7;Q4q}OZopLs)O=|8FH-C)VJxkUyQQ^yG~*j&za@y|DSX@d z9)@!18!oXpm6#BR$HjI)EwED|P?n6;DccNJUBZgf!@W#XEJ#O&MPF!%9*pSqt>?PSTPvEg?cnxkUuc7AGr;d>uVr?wDdOz;Y)+ZZSJH+8N zqhY3^uKu1D_PIhkEq$ME0}fmeRp1ZCT|74YK+?$bfg-_F6aqt}NC*i%SAP0Me~l^Q$&ZI{*obr*_YLH)lY`7^cUqIJ>&K0^?4*MiOtQ zfzpZ?O<Evs(AckiQf*|3KyU_@`4sJ)@Z?biUSLnl`Y~QH z)B3ZQ8Z)!?O+I>UqTZl9oZg0rR{@8kJR0%rbR598cPupWM3*d%_Nq#pw5P#}oDJyw+{ zC$9{DSE3Dwy|e+DY_v*a@e|Z;CLpin=y>!orF$bu--KPHv8#T*IDaj8Ao*f+1?6Nh zJi+rSECycc4*GbBu^1_!JOh^?jy}~gtTE-x*BIt=b9Gi%S2xc%cFL^MPnDjJYsdlU zPhfA0-2kQWzYiW{?#Q(OR z6X-?owUIY+Ji{q*R`?nMDbcbvR2e%}!_B)?W262S#sgNG_T=+dvC&IR5eJHx#yR5J z)dj1W^ZcmZ?TK@#=k>#8LG^*Lhe3+%@qYv}Ml_{vbYUu!LIMIcZEa3mv}sZWE@AuK zE|I_xm71Oo(DTm4HQ-=D%WA{};0RK;VQDMV+Tbaam`oc+7@55m!o)b0j)rrNASYqA zNHDZhr&ySr^lOad>G11DpH;TCx~TRq3G|SBJn3E$#sXYr@pioBy8sw{Rh6KC&pABB zSbqSsxwp1xMM+H~+rYv(o@W{uIh5`z{#h{YDUeLw-BDmtK@ShoFoN3g@5rmYpkpIJ z^Vx!39Z&*h)8%9Dz9{jJp_pi6lF=^*o;%;0&4C0kpwVh_yf;%W8>v&Q@!x<3GI7Rk z=B;qVESZ0tU&*aF&ToaUVD|Fi^uew;;RJFE`5D+2Tt9q7Dd6TjNw&q<(E{XHHjnQy zymj@^K!a_ETJKafZZ!$|cXP3Sc7+u-vUW&Y<=ecR*DC&XQGK=@CI82@SA&2b_(q)? z)X@}(NRyI8Fb4{}>fmkwLQ4Fqe8U|xI?rXJc0}|l`jTD*9eo=T3lR)DDq7?x%Y4pA3jMXN z3$vsP)nUM~*-o*6TmfAL6g5sLu%CLp$LI3EP|{!=lP&E>@Y7MQY1F&Xhetgts5~Jq zKDz@5@UVY#b$~0F6}}<3Sv>R{+B%7N6!`V)SGKpG#iP=A(mxA-|F#@ke~5ruB*C`8 zOi`}WYhb{S{sNRfpbs^XfQj zBz!RL5LY3b3;+32wzc!`(edA-l=eScV66aFlP&sy6Q7^L;u!HV^#Mo09P`Ng{2kC2 zP8tH=5-7=#j^*V{vV4qR{=R&nXVz2C4VdPEkB)l{-Ldwdp;)te))UDeazWu`0vn#FutMrunb#TZ(od^)5cAmCbGEJwrddl+SQ!1+SX zpH0?Bfxpcw^rnD+u_Juygedeaq(ksAyV0sLJzy$Y*rLG9?)a{IiK%|gLZLE-cr>b- zQ#t2IiaNdqMyNsNb&;ZrR~I2aao%IU=_gBzhuPC#m)3cT0l&@>IjQ1a+s&bK+>Gc{ zOWE2ckr=+0xVQ4Mab5G4C~hO;0oQ#9K}fC<&#hqiV06uwAS#bPb3r$A81>%Ty4a-3 zz5;kcH8Vv%qJ7A4fZ6~6FZiP;(&f~qkd}E4rT60?3-}Gp+UEVx_ZwgJ%3Vi`i;G`Y z%x-TN8Bi4VjKWJP!Wr&R%gWoK;?Z#L;NX%*csjz$*Pl+@^MEX;1v9bm_L|$n92$6N zZz>*e^^5yf()Bm)5#&*egIS`S=_$R;&@C8yjiGF@Ema>sNpVcK%bhq4&f%{M^YbDA zlO&#iF=-%#6U)Wt1BJj9v}(X`2uwPgnZvjvL$?>7Hbsbe5HbKwiVVjb+Uu$&cLq%S z!^6>J`6wrWUeG_6lClnPa33gWGb4$yS5Jp* z5e%VU%LKW_u)-6N48ZsW2w&4b>y)GAGy&|%E@;r5|E{w1|eY|91d)ul)Z>T!jJ=O?3R0+xDpnh95 z7+vqgfAp!c@&ScYVpm{O$|a}PL-DL+wCL97@ipcvi;3nlbOvUHV+k@7toHQ`?1DS;{*5k=I?vGBT1TzRUh+3ENw%(R7DnY%>2rZs`T zUVP5|xbFmTgTS^EyE&pv+b9;_1SAshWI*X;z-ep>DT_&xY$ka_;WU5wzFX_mCWAWh z2Iw^Z`@H~yyefAMg9Ko})z)c77M_gP#VaDTtzV~R&3wQ@b5!*q3(LuwvOFv}7b&1No+^2VFEW-d0 zD?k85cU+=8I@(Wz}ZRE}pSVnno8^^`cZN^$P8&;&JT6YdCRK7?21ykCaRe$%Y0JXQf zNP~l=Lc3K6Ws-wE6ze6m3$ox2(W{2cyK zUB^7lVRZ<_6o9RPM4Iw*)x1-*XTv}dEsYF9nE#Og%?b6$3G@65z_C};$W zY@2dM@4NS#3)^DTKvhZKx~WJhNd?HuMpjl1!h=yX?+ET0>q#6fJa26$qFX# z=QB{3yI458M+n!|$*|Atx+~Tt5bKz;s=J^7le4(!yPakYFd`drt7h!k^`Uc+DlRI) z!)bbR!ctvjb@SqzXz{bx=l9alf*wZ@dL+>Aqi-97Ht@Q;HA(F1sNe*>F;=POQ=?3|HFq9f~+ zV`jQ+LwEy&3rkAgELi6N&P*g0Y&v8D0YkKt_Df#5Z1gs(s6agDRP){QMfFp<+gH}S z#|zZI5*U-6?B0MnG`1{PDjxJ#;vG$ZUu-N3^trfe$#(?)=Ccyoqes0GzfoS|mh1|4 zfE-|q`u@TNAw*+PDJKtnAA7a4z_BrIczkdmuPtw^AKfwmkr24*&=Y5JGxK2QWP1?Z zzp|r!tv_~}?RV5D7m$z9j3~cGz~^iPhuI!p!H{Xz4bzQ&lx{7gox|eV;xyatP}LM> z6`DTx#Ay;moUyh~14hUq4p=GhjcCbhvnv4R6j6h6qWmjJQAu-OG?mT6A_i6RTa*;z z@Z`FH?E$@GM7Z?wnL0Y7>15Z_|> zBEXPFh}%~A9BLsLlDpslHgqMh{EKCQflx3IPZMFlj2jke&TZU@OvC3n_(Gai03I4Z z!-i|y|3;2CO;l(DzAqNNsqcG0rxy8gg~_1lrI=mNL|p44R*bO>I(iyFuw$@X%4jua z#IyzY^$>m(Z))vZcXKGi&IFkJKt~M(X5hh?ZnpRHy9U7Rv;Lvl@$Q#n-(Rgul}E@F z6Dndn4Gd=zx@DqbOrgMc5UKqGc-3jN(2>^;$GwY*ORbA<`L^dNsT^xOVvT5pT#Gb^ z&pivJ&y4}FiD&ja-EkQMc1M_hD? z%l3dm^aZe6@L@;)Sj-!1Xw-)*O$e-B>@KAV#vRbo3YEj~|IoE4g?QPRFCE)8=r(KT z=zkFQ6OUpfx?JqgIwU$_LaI&GYK=clSAe^C7l806`cpS&GGz!%-0-biX;Sm1-w%Y( z_R6bf?S!feAs>oLUw*+@BcuEtd-Q&Zv7@uJpmR$7-HPz`dLQ~*w5ENW(@l2|4COg~ z6flqo(@S9128tc7euF6^;lZ6Zu_(SJFjq?l>-J({ z%l)I{o0VhWvF|j??Tr!ne>7cpIF{}Em%aDO%3j$aD=YIc;-N@(60%oT_7>T)cSa%E z*?VsqB!q-)g{RP=ecjh}o}YC>jTm<}((2*yrlaGNjFD79Z&A0& z`7%w5uF8{f+*!=BEqAG|;F-7|9iFPz6x&(9H}Mm#W2S+h|1H|?lfzYfK9$yniRRR@ z-$&xVhfzEucXcSY*y!^3$&+Fbtt(;i;uMv9e08G+MVu_lHc1tFThCwSv?keYMd<7i z)AlJn%L(N7t)EFja3E>M4}p`6Lr=`y~pFREQo5VjcTyzn; z%iqzDcKH_D%EQ@8&|x^iP!phDH9_8fDSirl+T~G(7$1zg^K<|hR+wOn9)M=oOL+xCE`Mu&AY$iS}P>H1Qn>W3^)AvLVHSDyo?2r zAI~*7nzw=*a(A=N6E6;QRH`H&Y}&VO9G@9Id$daU#9~_<*?^ZH2BZOgh@iUpH3@jz=p{rVcSt4;ll@U@R2xwc9+?$W~_pwQ?vH~+NC zotXY4Jqn*Ld-zaBJrVQ;H}fd0Mr7?C07ISlX!G0R<@Vy`ra}9^gRAlHndfSWMaTKp zOiX8~=?MuB(^DQ9+&3_|FF1Xt`=O7uyp3qGH8*)Y_e)}_4d%8OfhqN#BwV$nt3BdZ z6Y6Q#yu3jWim6JeKyLg-?9OjTkNba+8avOd`k0oN$(H{TxBsnb7cvtOsVQ%$HoNw4 zhY95X-}zg!TjO-P^SqyL+^cy(R#x@_0%W^T^Y)n9))}py-jtibUa4(1JWR4fn$c|@2KA> zBlEmWd!>oTL9mcpNX<4sC|rY*>3xyU0ug@9Rmc9 z$>$Vgy%E2fYfi4=j;h?hgcXU%ByxnwoB~;2@Lahm_bJhTO;`&k8ngO9t$KOcMp`hx z9lQw)(%acC-ok2iVIiRt)+I@V$Eu0B7W&FhsxTpe!8_gYqcm7$*YY)v&flS1CDSgF zYlV_4x;HNr78d%qJ3V|LoBQ0vpxPXCF!LTJorxj7InTG}>O7k(z6_=JA=eLXtl#Ri zzt>abb0~Knaizmec@oyQ;sScDI-^0W50f^aAF;0ax_cZUt3z1bPDDsZC@UzVTBa=$ zY9|=vmCt`9AF%9iqB%&uQOuF8ouq(I{PzqfwK#@5aA@0Uq8ob4 zFwq}sU|@ud(~KLGuMn0@PBjIa*j(GOR&62osq}KR)6}|-%fa-Uy-a;&Um`&*aGP}c zW-Sx3np%HZ!Pa?wY4gz6pBERkhP90LrCl8D8qF1)9LaJ=eV+PmZwf( zHMe@)n4-uLG)RX(wWL}j}Z9kvoTA_M!j5Ga}}| zjk*|PZSo}j=Q~mEL&dv_ZzmqdJ!2Py`(;{%6GArTR%@v$O3~{n56@3L6SQSyCojL@ zsTEi&f;C`v4dhNW0(ZTFDRdhIT|V zN^mwh_TNu?bwdsmfP14$GB^owPw`L}2D+l3>+5HPM%uX>VNKj~dTnd#DF4H9#Er{U zF1h?%hW-uDW+<3J1BfWz_sSZzwqP9E@c#XD>=rz#6Tf|iGDpC+b-2|Q&qeep38qZr zg+vcX^>niPBGZ;+a38U=?`NbSE4PBzjX(eK!@;On(>E-n5SUl+cv$sW4l0kDnp@o1 zV2^KKR0Rhz)%t$6IMn5M&_w| zL|0U5xe4IiA9~J3I+6o5Rq#5qMsK7$5<#oUi#_1k_z4v^V065@xcL3MN?o2*C??pt z)E+EVTdl#}Ir#RCK{VC82Xw%rgW|Tq@n3yz_~W^K3QVzPz3IqSEZM!{<_2%?ETNGTB+@P%XlzWI3xweCpU!CK84Cj zW{>G7NQN0&%lH>}3U8+(MQaBrgQOM#p1s@v$HxYk<-E(7m?h~LbUQl^WVNHiHdyout--_7@qhI zO#S0{OLOx*tj@y%srmZn&!5Zv`&9_~A2wdKAyLBHcXY`_Ti259G>z~ICvzJp_#>9E zoOAb?XKjzgh=(Pe{rt{BmwpAWm00S9d6r^oYLV>P(c!3*KZP-OXy6|Ei-$=?Qo|qA ziZcBlYA0=WxSjejm1}G~v+Q*9_4Os)h042Rnx%Z@S#6z7JY z{Bm0@^VpZ-1I0*GDjtY#E4Reyhz7!th_;X6TZ$J1Z)6NS2Jdm57X}=?YWrJPwb0^y zxUmQd0CRyLs^stj22Z@766f zp-dW|Djv<4G%_d#yf|5oNPUu>ljAAH_#WKQ!F^N;g1|$*RC@|fGpy^dSc;|R;_TSW zYdNg_`1lgQR|)bVy9^$LzJu@xO;rQoj%#A!q?tCpTWzN|u|>z&nz+N~!Z2# zpljXU*%8kiRbsuCE5j;&UOX_lwD8%~x5ALGe%^sE1&@W9gF@GKLGiX?XZIM@ZfW@9 z#loV8$uV5uZKv<6ck+@I8;8N`()`t4F&1x@qqfTL$z;)Q)RA7W+@zOH`lJ1#;p(Tv zSgY@GbgyN#Vd&d*YIcf;PBL;nA6}04;(-v9^4aqGpAoSj=mZAn z;^N|JU+xm;dLsPm%7!AnB}wK`LsQcna7VrB2lt%~)gRhFuW$>mKn9}54$OhMN@6}j zL+T>;aysB+ef?kXJO-Q!UafU?p|4BwJZWD9Jr2nYS+H6t#0wclbWtWP_Rh^^kZqur zu}wTgKK^^^#!)D*9Mi&kdmqW=G}({m?bnLVCglYJWxPa!f*gctJAH#&u6&kK#>9 zb~-Bc^QF*XlrT(MZ{*e#zQuxt+)bcVdmeFc^>gu@zTX8r9H#!KPePT*lIWJ8P(tx+ zSL;{jxz;sNy^8N1z$#+F^A=rx-i{j*X=0~g9SJ$@YtefC*pvCg!Qr47}U!IapMu2_Va!5S^?^+s=UphaNw(;!pCA!0&7lF ze)XI9VWN533Edne4i=@ivjt%S!h;mFU#rhkH94PjOrzO|XMWK^D14jAsBUf5!kVy; z-U#zzbyvd8uUOB&DBASzq;DT zkvmZk&s4_9!@khh%ou5t{eiBIsp`bh|8>2(79pERyAfBnkz8kJgcvNiA z{sjF~w;D6sbmF9qlCTomXOI1~rvg|jtQNOF5<5eGIoR4)! zRu*azaNZeQeTU>n>GnfuV89SOq0}ME-~>c)CM`G~PZp|P_+P;~RNSjEt%7X80d~fxm#|l_d!9_w$!+3WFnh8zUI2-nr^R&OFJ4yiYRk!l3^gGp{5QcMGm<(jZ`qH z{ai}KA4|CwAmK29bH8YMBQH6;8vZv(%rb@GFBcbG&8cOu=@tt*yKyE>^5!R!`XoEG zqBlC_w5(e~_{Y6u#@1X(GnyB60xrj|z8*d{Nc4_5GU-*AmB?1i)1BXIFsh(SghJ}% z!&0LDZ7~1BM_O8=M}^H=APu~A^$rlE!I1@tTpo|EkVETj$sYpcgunnFgEO^VC!7>eI6)5+T4HSGrRKT*cY{1a- zk#gj|mxG>2>Q6LMJrV1W2P-XuxQ6-ktih}7HRgmBp;yXsUO8!h5(cdR`QM|+i0}*x zpIEU`VNF1@O=#1{$RY|R6U*PL;^clQR9_^wR2FQTVQQD-7jO(Ea_h^%-Jz zW9OojSm8<4vn7ViBqEAy#Ux+WvugS@b9-bzlAnhVPpf-3#vSt6Y3}po>@6K<4_cYO zh&KH(Y@Ls8%-yQ&lK@g-sWd%ZHs$eLL@VC-94TcM%`wSMU{_`bA$ zWn}6eD7Ej8AFu~s>n5B1T2HjbPD|`cUmy>_sbn@hFdDr;mB`!IDWav>C-E> z5C(CkS7#TpWF5*^U9vfkw4)6jr^OQoSK&NX*InP_6|8-0lAX0VVUOHPluG+7^tZMC z?mYFk6y$+FY1^!(PJQ7wTxP}39%PQ9+1hL(b$9SsdVsgX`nj{SbAH}3`IHXsL9dX4 zhhWQI7no;Tuq3)IFgUpewDXdKO`0Xfy~w{$Ppf$d2h^FIG%R}&&L@OL_IH^?zfH(8 zPfhpJ(miFer98$Ip{Qp3w%J=`#{5;qHO1OSk4Lfg6XAgrF2X~C*y9OjQjii;kyNpM z`y!X}7T`4JY+|G)Dj@)e}A@sfUVz7Prn|!YCjJf2rPiiQA8?EAalVW9w&K7 zr}^sBaXkKWO+S*_KXS%CPENZ(kh*WY)o`67i(P%M!Q{I{h1^4~-jc+d5_Y9}GQ(=g zEioIk%Dk=cGUT}&t|rBAzF~ARR$a2ML-3_({Zh;qo1a%9%dAAw^Vn5~U7;;D$X zAPc2>6EpDp?kq_b?X0%D{>{kQ25<@UvFvgU&%on=05e<{z*g+)@PD{4lo(aO-*x3b zdhh3@s;WwwD3eP6%E?(3H+avZd_a6jVhmO^mY#{_8lhszddUV0GQof3-uCtQP*u;H zGlTk^IeU$qNrcD_J9CMGk5(dsWpT!H2@mi_K{TUMj@l%7J57y@g8Puu_uow0e%Y|d zV=s#wkk=$PjR()?B|OHR9f1NRa=KV-2b}(k?(VxRm|c&f2U+}OnDi9&(GMB`MJDZ= zoFwBr%THTTkT<7QZ-!jr+G{=2`LY{|kxHz(eAM8c1P#Z@o`ra3>+@ZvMhj6o1&&g` zdlE)(*;tqTs`g9wh#rZmcI5p{^e@OsK(hK==h1}-3;ndL&G&N%dmLN(o1UxHOus2G zP@7`Wz)+V^KN&cwOD;N|{}+-pR*WfV&jA$Y8EC={0z9=l@P#)xJO zSC&0_4aUij@zqFFC-9+LAZ>WUadzuZQ&Uq)3Y9D?;0SM(FG5St2d>cBd^aNWot*6&?o;Q}lz z!`1z`yLhj56Y*{^5EeO&klO`ae`kD4=cOHnF{79M;Dl8E*0gA;ZrUe%m0E}1 zk^2MljOjS$_1)6f=znSMe2BGHRrfegm0ZS>wxH`7rq#(k6Kc2j*{T9i@ z>6Qt#?W9dDxa7<94^CT_Wl)QSz@CTW;0+>`!dMew#G0`xTBdy>Do;k^LfRLE96BkM z_F7_bP)wd}MwTz`?rv{C5lIL8dXCVw__jU|Q973IE3g@k2X@Ecv3U8Z8Q-pD=M6PL zb5HzLsmVF**ie1m=X>|m-^tl#norNNr&*_~nc+v6Ev3YrJ9Q3cu!lq`p-5_5x@60UkA6* zwE~7>O7wef(|)Gmb~O&oI^3n|X<`+$_q#^rOJ6@AuPEpkT|zUINz@s@GTvCX3@a^4 zV8|w?{B5~9=N8aPnhZb@yv08RP6z`XGyc5z`~O-1B@r3|s7({~xG9NANEUr}?>Hi# znWKPcX9AqwK;rI+ydA?z!V*$M9j zMG9b}jh0GNErraO8a!ah`nVuqOo)Qy+5+EK2o-a%gwHBokHda~luJBU=zfwcq1Y2R zeK%8yz4IQKMWLKpBmswlR44YUA_hUVeGVx*40R8<-xgeDZjpn*m)HtramTZw!3&}T zRw5A4My3dmM&P+X!A_-V)nbb`MY*-aCB%S6j#-uBExb9Xxoystpq7=D^?FtswRHGo z>jFQA&+zm%>U2#9ZpdNM`HJA6U&ASTojP>?X1XS)dO+!E^}b~z^(sl*K?&7=(G9T| zl*<_|(Viw1&5=6lM`ZRMNt*TpYk_0Msn4V|{Ux{C1lS$X2d&6=t~T7IayFBZ@3j%^ zv7dG52qZ814{QO?N)R=h6Do$k9zji@AgLfCdMTY?O3}i9BbXI4ECc5DX zV#DTG|G1|yh6(S2$L`U4+KJ9w(Szpr+9DH6Tid8o?f2ejlZG$buYG=ptsWPb6zG*- z@14_7Q$sPHaMW0w46sz~H?`3v9zM}i^2D1|;xZ^mrB}(z?h`=*Lu`>Bn?8}msM-t1 zblM=n@IwJzKBktNd0$rXHOD-s?wP5c68&OM+Z630p!zKkvGGI!sc*OLdh2uE0_jez zQH5!70wC+y<0SuvuG4G?RD6UO#Y1l@>Hv0p;U}U`%zyFdkT&lj#%?-#Ig}PCwQO3PYAbR$Vl|=B? z$HGb6CbEJd&0CQx1$(>1CJ!iS622tgI587SN`31Vg~V4#imGjVK&pe_NE+1VRW2XM z(=XxwjD;nwEs4fJtE4osP76RuF{QoT1plfSty^Y(KZJRp>LgQCon&D_VEx27s#o&* zn^t|q!&EU^!lma_GHc(g_c@y?V=~lnXpLr*&s80NBZu^P|7eON7^SX#6!^MegRfao zO&fq=`tr5}_rO7a&=&y-vV@F%imV?i4;ixMx12lUjLgi&t!s2k=q*v34mpcb&~;_6 zme(@lbfj$3H$IFzAZ)G+*uCsuu^o|k?2k;crX;1V5||CLjr=UhvVXp!hN(vWs8qjy z@Ks~STZBO5o{596&vvo7zN!+re%m#L zN~$DFt@p07JNE{?d#>xws)z|1a@M+P$^5ubFA6I)%D=?Umr+f@*(DPTkzW3$gRh&= zP$3DPFh2>3WJWWlWWxG3%@8oXQOg+-HCQKQ%>8|lS-(6<6S;c3GKG5tZNCeYK5`KtPSy}Fm0JcWAdEt$k{SLN;(k}Ceu=~M-1u7gPoxQyb{<>Q zwSJxGwU4+dcfPmYEM}kNHN@T=1uZrd-fI4xi8H^ISoeR#2qHUpE5!Yug()sW^O?_C zsL$2Y=`KYyt>qt*{2}nR%A>bQNLCjUp>~B9eJ*r9?L75B^AVakf7DiJ_VA0Iz}R4n zji7pxN;B>iH>)ez1P8JVLIGNk(Wz@ERkaCf>`;_wF4t+6`(b>B(J-fevNL6YkreGP zoFd0l8}ql0IR?6CzJ75=L874g4=!zwWC};RvN)O;AAc~Lru_^Pv??fAUR~{3T|*UB zcFaoUzMOeMrYGz{Y;kIuVv=QlpR#MqxVMQNH!<#BI_sD$O z4H7m(2SE=2PedV0pR4eGvYw(I8ur(lUdm*oS||66?oZQ5)2yc!{Ta^dj6)8&WEl&T z+$T4FTf-M~{;4E+Q$i~o;lcjOE@x%@f#&W1y>a-A&Ev}Pw!!on@-TtIuddUQuDTaD zYs4a1g4jP1T+!~A`OiyZ!jFZGliYhbagWwTqcl&tbZPQuZ`)Z-t;*HPt-(8R)I z4$IvlBu}}dX=02;rv~YKSZI@5{yFZXE_(zURB;1=g_D}4<|S|b-j1(M{fBo;xouPb zp{fns_a!CO%%{ChaG|4&zI43!sp*Mdaw^!lx)?Tl*#{^4ykr1c8+ujqj_99J2c^g@ zNrQ5>wq!m6$ydpqK0ZLO^AAHE=ia;T#h@${jWxL15I9E})BnaV;B;ZZq;sm8^`E%^ z6t(4s`!|}%MUREq*;&^nzqO}86q!4F`+cZCl6l|`$r2FV((~lHwlU&;vy|wlmCWGs zuDMX4e#HKrnSW-}7Iv#R#Ciyp3=XrAyXAN;DC}VO! zRAxvB2BzFf$-rQiXPUft=6wWY%Ev#bc;LHHoQAr>4cq^C+K^7Pd({7sd_&|{EW>a2 z(*B_`uh+rwrQ&#kb_3L7%cS(q^Z@%VB?1q8 z_;VClhKWZq`@@|fJu?%fB!^f#hNnWb5`+*ChVngS>+`~aTTtK=4iv%#q!#%|WnaQn zAQvyMNVI%dr;AsY$4P@$%?nI~*dPLSVTe6Y{{Y6tq`|;I*y1Z5+zQS{eGjZVQR7FE z777q_sxD#|=nQ;bOJif?$)kJ^-eR41I@};jE@J2?P=tEH9qIYQ;j zLyaox+uCHMrRi6-_okgC+)a)lFHV910M+buiuC|pa%v8%<47~IKBLpqK+*a_WxnNM zd1mqP!}!N9MCqXK(=LIltTb!S8DFQ0F-#q`E>7+-WVW8Kk7CBFzfjji8~%`*mS*O& zQS8`o26-wCdUg`U$>6AitEhLlx@f;TM)8%PMDF)k@N9nJ=lA=gBd}qZQNA-LInQ|< zqeD$7tVd8seNPE;p=Q<;pC!7Nl_(%?#qWF{v=aR&s900KgbZ{qKIbR&cj~_(NO6Ij@4Y8!2CpT1(z~=r5J;Co3l>M}nA6J??O3CKL@6P&g8&i&Z-b z5u0$908pS9S!OKoR3zQB>h>tx@cN-s+bO3tEgU=aizj`c(%oCUIty@WmL%J0~7ul8`w;+cxL@rW6IPy&*3U9%%EN`es1QENT=3`t+I(D6I&ROIdhtRYs?cHW#x z*Cbh%+O^?RFA;8{%+P*!oYU$cAijARJ!KRzuWT=pqH)v ztO=LX=$OJ^z7$z;I)7$X*zrZhDAF`NC7^$ ze_@)O6v`g%q{hna!JG2RjV}oIdprR;%hrAz6<3qiXXQ@XzahB5EX=?z(-;M7qVC4O}umDblGE~keNkq4HNr?39zldFItZx%f9Z;uN z`m!ED$5Yv}zHhmoO7X+c*_rsGQ_M9})!0SS3GtOU@JSWrwYZAG$D3>+R!gMTiICxX zv86QT*>&7h&qolPW|{M}G0288g`Q8~raqPDQ_<|b%@Us$8vTo_*{{2xvvH>qZ zjJV|PlX7&6!Myoo|EX~1WaKNRI~fNS{QUecR_GU(sPk=>fq=StCPa(N0^1kt{eOHy zI+>5|=Z8N`RgQb!;|8|op;UnRXl2}fAx(57n(?ti6!LXQ+f1-O{|rl{7}`r|VIId@ z%u9IrxzJ1dpZ{par-We!{Vy4WZN;|hzXTgpCL5t911C`NbxUa~?9QVCC2bv}1Mxsf z#d|bK{);n@_*wk%OJ68**2Fdp5&R9J4$46J@-`=C7rm z+=6qAFo$Byz`U=NdpvAKqJ@b}OQSPaQC;At8fAe*xIdL9$@Rl$E!-Gx2!B?ychMHo z%hM?;{;cv147J~+g~x-YdrOp06{}4)Tm=ab6wq^)j4&6>MtXMko#$Dxa^&swaz9lPhGrk~H{J{l+)RA++s?H~`*H9@y$dnH zT0OT?lzZp*X{pirZ72W3%qC-POF9~9UX)@a|an9PhV6xRP(q6Y2 zw-u#B4o%vueiZOK*!&~qdazIb()(s(%A5bG z@BCbZpJD{Q8qQx4YYMYS$KebXrZTlj5qP7nyPE%Isp3RrKkUx9w1fm`B}iZW;u4s} zqQ7o`%ArO)(N!_&32et2xORPycTh^k>Yn15-XBgEpPZmoQDRl7jL2C}Xkay-gz+0C z(+aYjV(RurPCC~FuJCGfo|+3`o8Th3n_m>p&In=$&Lw(B$uBX@++RVJlUjOWSGp>Bhv3y0_bFY)JM0uV@lDS%Fr02 z4HNZ}^i_AOJ*nu9g-LM5#lAEsWG374=_#5JdwsK;dNfymG54(M#jfun1Y$2O<>_#L zsc-#Hf(l;#$!abxt|+B0mn@*rArcXTLUVO$*fMyNE9l&8dYH$Q83=<>c{?RaYnmq1o8Nmm} zH~c3zU&y(1eBcAl$L{UoZnyOd4-XF*NKuo?+!63hhKcRE?faJpk0Sv^V_;wy86D*p z5SUUvmDl8GUJ$U-N88T!4U68ihs6Atsg z4eg^2=QHfHP;|DRHMdhJf+Yf&9y@zgDn(un3+_fQ9=}e?=R=G7=h1sZMpudG6w|kv zZiv+S&QkXA=wb!yuM2ZIuHHL|T|9|}$rGToLqc#PK}; za_idnb>LS9t(-mDH05_?h;WS=l>_STg;YIT!sXRR6i;t}>kM#%*4AxrQ`uZqzo^XH zPM@~^fb@s$y9u#4i9spLX5&FIN9=lSQ_7Qvs9}*;KLjt)3xx#OAC9O{PK)=g?0@|V zf<;hPp5MorO~X~rqv7^fVJ+zQbAyVMiubIuDzBC zT~kwO1c65HwQ~E+g`jPRmknt0%)4g9AZCNKLVe!J+X5JGmn^3+`6S*=3S4QbeGTE( zm?E)I!va3F;!y)#Y~TLAOSK_1r-(8Tyud34o@4s`9r6?Xa*AoP92HlqX!4n}$yI`R za4#E<0%Esr5mtu{MQro)-M>DqKua(=AfMd@`pL}f9{47eEK!2hwaBhA{Y|jS!@)C< zOT$oj>VP!Dpju)fTjC!dmkeYpLR;0*jJQ7C&E&>J!tdtD!K)b21Os?h`eFgKf_j1u z{>(t)J9>t6F754+G6k9MScyo=$-Pn!Cxu^;vnpw*_zY>Qe-FR#qiCaq@n^`;dYul$ zU+Et{Y}40q+yPPqUj>o|A?nA!y}haFVVOQSSU)KUL_?9%*f@aEO)e_CGChC&fu?ViPXaxSQ1Xek(+?CR-B z9a8w@A;c_yZ}ko4k*rH66NiU7G!%XCB)QbW1OXGs+?ukG(N8f9l9aGmL^4OMMXqZt z4iLEI{H(L3@1)pJ6G$WY!&G?tk+ex6w?jw(VTKl>rR(8J6A+Zpx~0Qofj(N_9$a2u z?IE=T6G`_6ZHIAs9dvZODKL|B>tnJ}q}ffc62H%-0z_|M#;k<2^fOVqHPY7WYuu9EYh7WV|9{El&vag|tYRh=z5YA#>t$kAhkjr|{ESxh?`xf1T>*k2AIcuk-XL$sVi2Un@3j<8 z#+F$`*VE1r)OLH7Kf!c;k!VvWSN4UEvoq=My|d%pXR5#0ps0jh62`DDI39}rh~HES zoUPp#+IA%8RX=V~75h~fd>r!cg+qQU`Y zY-pQb%=Vm)VOxF3PhhxHPrELurawd@QEPVkR$h-uiWaLg3&$&)_hq^cSA*|M-jNtA zNp+#}!egX%`CW^I8^1BBs*P!WufJ|4zN3t>dsngQ^WCGQ4FCfH@Prft#{Qt2OkV_M zdPKb-h$CFrgf1(_y2jhf%aD&6`nZSRpAq5^M%*3nb{C+@jHLawGRt0}$+_}z=SYA3 zqX2pbZ}Egs4mncDas{VQe9r>t@{FVI^H?s6pLa{Gwmd~iAEHVp03N1G?tw}QS*Bm6 zpOKypZ)N!Q8>J$wH|7s$g=J_&4~w5-XWnI}c=baPO(4~3gQ-rbKf>3)V#0)K)J|Az zAkZiK)z6`)%@cHL|L{C<`4!9}KL+D!8J8#&evDjaihylk{LrX^8hBrOYz5k!Td%Qk zaB!f7HRQAV7T!Kb&Uo5Av7yiz7tlpFpZ{`2V*14LZql_;xyqZbKg?J*v0$yVOmObM zalI){*c87W-p#_`)yTs+Sf?2IpBxKB^|HAjs)zh%(5RhAs{bz}6Al*`KbV@nA6eV+ zvf%cdPTLE9Ui`Rl1P{Wa&)}feAgHnzPagZhMrp#itDW7?<_Uo}Mmf7@U72(q0=K{7 ze$P8_P$@X!QQ=Zo{o#Pb%^&*7I|`fy)Nb({?u!;t6{ z|B1m!-m(QF?kn$W6Z>vu+FJOY)9wF|h!c8dG}>fd83hHx^8c8ghq>^rVSefKMtnN0 ztml~9d1SJyDBTm0>vOYe#PK8E`X?i*p{jGbpFIj^UXbR11`H0T@8>WCs7OkzDXWS_ zKPSmP9RuC??W|%BDIcq~2#kaK2xK8x8eu>s4E&~^ESwy0$&$NVk!yQ#R-gRAkPr5j zf9z%V8#^KKnU5O9^d4Rhw^R(eUS3Ujz;L~;-#`l~w{cB$!sGOv>W9+K@zm8fa$!o` zESOoywW1t$uS(0BfeGo}W{rmKajEA`qWHQa&n3$dHRv?jY7>= z83_rraSn34Sje^f)b~#PmWE$SEh$=wC>f{qHK9i<9^v)n3yxqQ0!&UZKYrAjBB}?{ zFK~zb1xBK58k(3zPXUbSRwtgDG#0>7MaS%bv}=@xl{LX@J^MK~yLGFn>O{DIdw{z; zh>EqfwQDtSc0YdJvRKGB7BKs9?M0YH5+9XdpK`h}`rL%zdDQ(G0y2iL3K%)NWz@-H z%6v|}S3Z*qr5Zh1!|8nbUL2WZJ6@y+gZ$MJ%hP)lXN|;ebmd7%$lL?_5W+_M#ZD9i zlmqOtPJK%V4VDUz&x82y?@g820!h&&i(tMl8C97IX@&-FzTXz&Ltx>tT+$#Y<@N>A zHKpPTiHsE^Vc=bTecuW<9*eW?d*xDw>m&l>5Q__wBmm|@{Fm$b{JF)um0t8Kp{MeUN&k_`-cs_MMqnv1YuP8>-V@sB46ujvp5Jy5 zyAkyNT7bp^F)wM{HFI{!5Q6-#_I5w$)@2y>Wt?CVLPWQ_)WWvsf3eUZoa$Oqxewds zN$NqkyzY&gm8o3c{C|3LxY1`g4}>!s!h@%-Uq+&%;vy z98dNitJy~$5p3DS-_{BvQ%^7`wd}?AH)#W1?ggx2)7pJAi!Y$ZqO*uA2*6wHqnX#G z_Yya9H=kkS{G1fBQf4_MpHYq}=n}QSbC1k_M%bs@i|y_>mEsWuC^l?Wn_DyJewa)4 z^JK}?7D3hb3PVLN{<*h&1*$S4un21#R3Pr?X-%H{ukYgY4WE9_Jy z5AKzZ0`{WuEwR7J2YtL(*Rhv(RCMs9;@Cm~3^}RO;SSte!md33O7Da$oNRPkC;s8D z>ri`bCrcJH?TrSMtzZfQ9VK7_urWi&3Eu|1xv*v9hoSN-Djvj)?8{K|e9j=uRA<2FfhBNF^S*Pk>Tz>E2WAXM2Y~W z@k1^4>(RWCgM$yqazj2v!O%ZcJ&6r`lC*?`^Vc2?Z95BcqnP)454@B;_k|J zv6}Bx1J3fYTa!_6rJ@o{SA#^5GW~-58;PtbGSbqI9zI-_=?5_M+3DCqegg&DWAGqC zSR3?^G^Sn_0Rio^b_H_KBzjV@#NpjBbA7VhyZhrU`~GaOl`v06+S;bbsxE4 z2%3>$dW91bv?l!v^X*gy8KxkEN4u`SM~lCyx=fIX{!4lnaB>ZV1H;Ol^}3Gb=UZV-2;oS<}r$+u2%OEn08{VKE~kql3LYbF_7x(Zc*Z z@V;Q(I7dm+92+Bq4Df-#+t8<=2wGu*7w4>QlIPOjg46W z%(TO#3&>97m=t$ZpRg+(Ofz2*swnp@rExiab<9|nzth( zxOo24%zJkKu)pCMBg$j^(b9qr ziOk7mSOwP^Q3jhG_d`EB-#_Xp4)cbacRigFuU3n;&~f4b2Zk!0n?Jud*xBTG5A|{O z_I`+4{r)}L0#CdqC)mN_M13KU50&!b^3C$n$ZqNnE>@K#dGJ1ot7+TloP0<{^ zHilTYH(yCFS8W^}iR)VwBO#6pj|B)?sccTfH1^E8$#k#qjJ3$DcpMCrNSBXdyo<1Ssbiqs{&g+qqXL$p zR;VuHpG4T7sH{uJ%zZ!(EW6gl6|cq)V3t7IGvKOCoj}0I+c<%!U0IJ?X=M0rxZK`q z)$>at&Qje?@flF-(s%BdJbWm5j17XfhWUl>-#LfVAfZ6`OAgc&0Q#|Dp1#BG8(Ad!M^@sB++%dDanf@=dar2XdyI#1t>G621k1-k^EtmMowtxJH zNadN^<2~mZt%z;!e;a&3gW=ZtKFrJEx%zJ{T&2Fs~cX>7o8g=fR zEcv#62i=qnN9X0suXTKHA)$JE8;k!dAz^{M#;5X(rjIm6nmL zbT@yJP&N6v$pq(`s{qcuTv=s$bAtjgPDF%y|Av7fkvTe(M!$1?;vY3t|}$YLrh zys=DbPYEl%0{-3|b_&<0_`amE}(!=(X zD7GFeGH62!Tlzuu4`OlI%M)OX9}^Z*!6||R~e)IDQKDh z3&Hw{O3t3I-}+eZ0OPvgiw=K)5cSPJ|A4PjdreRgFIpWf`3yWt@Oy54FYF8>)%FlQ ziizQia9vS{QCtV1#q8gsY=#Ig{*X8CKT#xjwn!`XNDIlxqg{u$(PYh|-yO!ye?+w_1OKhRXmH)vF)}K@soScK%!&mZ=;Z=Uerf< z+ckf5sX-2c{I(uWd(z1`g>nlC+hC{Y>)I>OHyOvJ0T&G5Q?T&2r*|KZ`x+=qRJe7g za_?z3atpCA#;TlXFh3|9AW*oH**T23M?7CN$ylSml7i@s&+44LcER3i??nY(1g-L( zqG)B@AQtX)O~rprAH5ptTRvVaw>iv5wo1~);)iuPOT4Vg@`o++cxS;EVsP8`W1T2t zlzG;iC1fDa2W4?BrL#;f7_^%5Sh%`qzq-vpPAIGKC2qw=oQJ*6m@-T;D2701Xu9f$ z#(*7-1Meva%6^3&#`nR1a%KBfr9Nywd$I88&DHw}BN4B~xK-xZf4P zr&6?+Y(bqh03=ZSPPtq3xR|mrqTRMuO5h=O^IPK#YO~8+xqkr$W`-30%vwfCaUv|l zrVY~@P_F}K=O-I*b$J2k<$uQCDZ~gF%B!~NE(N+X(<=I{#BticmzT0cab3A*&N>|* z-AE8TkTmO55DwM(1UR!aIFr=6*o%L=OF54heAdQ>R!Lb&7L&|SlH#S4FbKYO4!dzHd#sa^Mykpv0sEN>;_k`Tqr#mc=ieVC+AP7dg~z}XQwTa-H_ zf0E8&Gn4mJ$^S^FHPo)=L;o%eMBu42T@1<^>Phx#JpXXhz;-`=eQk|q(} z(Xlc7dOrRa$FjmwSBE!Lf2!nmwp9&{+9@u>017ePIa+6;!Uc6=|!q1#_zku~B z=Z0ROSDY!XW6kf_?>ZaaqqJ=Iqy)9^wv9I zw%0q*_YI9&PrGDQ1zc=_P5eZ>fc*ySA%3zX(uSdLikxPjY6&yF`c=p=N0Ix6;@?q@ z4PL4Q5&I%xCxmrwutUq3Go{zmv-L!YZyiBQNPTD`Z@%j922^;y70 z%;LrHLc=fm&LbBYrlcE>WSC_APM~*rgXHlS1bPXYC5V@XBz)H4Wb>6L9j{n<)$2N+ zqS03-!woUkNlI#>V2SLo#237Y8%Vih?4ct3z-EtQa`gXbI_sdS*RP8sCEe294Fb~L z96D4$x&#F25D=t6=|-g_4J4%dAWAm|Al*ub(!85H@9&KN+!@D9ob!C2z1Lcwbs&v% zg(kmEthnsiXj4@d*}n89m#EW&cB>z|O{^g7{%$i)u*2bPtglb73U8q|pkr6X;A*H~ zN3A*}g=uqGva4i?x;Ghp+`P~v^fmD(t{j4fcNb}{*m zL?YeA@h~d|1+(a>hq>g~&p_q&mYnkIE69!f6uR5KVo>0CV(nRlGcTU7BreDs)0kxN zdwqQ!Oa=22Ti-zze<9EogEa)Mf|p7GMzydGACoe>z{KZ|(EWz1&n7(#X!_3j($0C# z2$Va_gN&GnmJwD(CwfyNYRR00KTTh3h^ffTe+;g8vZ}h@!%Db39J2ml^TgSXQQg6v z<+7oB;4pH>Lc4|x-6z3n0?UW3d|(vw2n>RAI|qU(lP;oBYsX%(Hr2=BVO297R2rJ5 ziUGMrU+`Y2sE0kU25ZgC%zs?{FOe$42huC?-L@-ltHkc*Qa6ML&z&4ECBn0&P%RNu z2w2#OvjIT~O#qKsVSa&HD%0`~ocwzzWR(11ar$~=Shtp%Ra7S_vrRJRg^{l#HpYH& z{+I&t#SaizcHuX#dWR*mf2PT&|DXk|Ezs4Ss?R1+@l7{M!h!x|l7&^J7SmXjQ|=al zQA`X)E||aZsL`-Pkk~sSZ|1MF(`}_f%)6!$IKJm3VNPTx8%X?jNemoON(r`ublOq>kNZQxCCX~8wE^sn(CwJ@wS6IA7)hY-tc81WgYUZA%c~D z@acG7=R2%Z*Wm-m%NgKI3Y-*8Pl&uwYuUf)}wQ*$0J85>e%&=vTGUEm#<#b(b!;qm;_k@$iR*lu)z$xg;468IBQU z38_f9m$_KBR!U`c`=y1w$;b!tKQ1;O${k4WTz8jUAGRQGUVIKYCY*ylr!pOI8UrrJGbKsOzMCRl z0Ot;ssqXO~F657$aTjPjEh{%Rl&4j&u&~wcgmvPN_Di2&+yd=R4&dED5_|=uFRtQM z;qLb9=@(=LIPH&4R{}cb$BrYuI$ADCd|8|rwd9U!e`|!ZFFt##MN6P?TBTn%_&inr98`t{j$iL8} z3j7;*?*VEhf8h7;-y*ty-Z0Z8OV-J(8g+-R7N=Ze;JZE4Hwgx5zcS#IO1geNcpCA< zjnnes5%48=iXnr4s(XzQ1ACt6t2@*~6(q1z<;!4>hMCU#wCE|X0yC2~VZF%eoH)Y` z$C#vGLG->oS2jNXfBUz{hYyH$!Yr(AOW1(@5VXJz;kEx3;5)w>Ar^DBIefVZWn{>* ztdDK_wWcT$o$--DLoyz4I_7J2~mJ!F#@5Xk0+n8Y@oD5YZ4le z+B;ifEro(d(dT-b**s}$xoFHlOTmY=e&HZ$8s!$$ef%ril{>z9#n|KP?D@TySjkNh zi>rv__>zw+a-TD}g^jJ`C+e37bz>nWmuz+8gY?Ox`0J?~<;7q1Ujw~ygcdEgrO~4B zVp>c|p2)iCA+G6oH4V++y%QEDZ83plZo?K^EBCQ~H*Y9K!Xr!kY*eC94R)W=M=TsF zRr96M{osU57E??keYwR%#$1stuO^v8Kt=Sr-^&Q@;s$RI!4gn!QNch@&&-Cp05fxN zaNGC>rEcyphUgjxzTm*ERG~`|7d1m}-u=DJeIURhajo%GK6ntZb_Gc|1=^U9Z0x_STZ7H4CYzl^nS>Eb-HsRt>e>G)iBfFpY`{Q8r}c-M;0cSgW*Miu8&kQm3I& z^1LmA-3e28$Vp}hJ$Cn}4##bio6t!|kGWYYB;Ms9@9k}< zFo~UAScZ((k)D|8m}37zu%{z_X6PwmEj;N1a5u2@C=**PhCR&1l8Ne|Lb)jEUT4af zK_8?eA*W@=KV8>aUF`{#!wb01zvNhJyec*On?wYy)!$SKKvPs0KK&b~d?m(OTqbZ} zL=r{qwiR^*MBkyxiMH_fv>BCcy4`Quts>T+=n}?7yhl2S_)?S;YQ;d7AVQ-}d3W?a z)GzU*BbsNTG|hI)?;((pjKAp%WsN0swb-F34ql*BgSn)#t}bJE&8yK23|S^7p>4}y z7tZc?wKxS&XwXJBvAcY|y(3V&oN3qI)tzY4PDn(M=hE-36tX{k89{c}LRsXMrs?Zx zf8Bj5evQ2IinRUZWxKu=$%{7LKamvQg|p)~}pBOf+1_(PU?P7ypGS3|Jb9V^0pIAWe`8Q`{jJ28{ zTRsfZ{)|ovlIdaCdt1}>3zsx4kvgSl?@_moMb4F7KVK*oNl8dZ$jW{+;)W0Z?7#M_ zr?$_J8X3DLpY7C{mIL1!h>ix;Oa4J0476X4s$S)PAU&rseK@eg4kgZ@02WBx%b ztN2CFbb@$=hBMyeH=c4uRGC_Y9|R>i$v_qcy-6II%Xdhn4J1Zz>dNMcdHBd_$_G*f z?P2{oeQ6;*0)l;3P7WU#PPW$9YTlpC9@>RgqhlRQZ3S}|I zm1kf8v0%mhRN{;$@5}7g*9zDv?hLy+OUpX-`ir4maEXoA^}o?c!D8+=6~@Q;H!fn|DINHUgG5&Jp|A( zjB4xZ99(Y5qM?U}I$6HA*`UXG#S7F-n2NL54Y1_3Q#j5U^zCrGLiIfum?>2rk>Zu!=EFssz#s%lQF?HKHO*AoJ`D41Vy>En9@_ z&WDnlewqJ-jQAA#moeSo(W6IT*!U<_bX(HM09)sGp{QOiUd)MQ+k3hN7GCpYvAgf( zYO-EO_(oJSJu=g=B#w+J8X2&i6QiXk3mf&~?A2hGH`Lg6Bqm&;sZ7564kSC@z?qNl z1{{yjsS?DnZSlpNkE8|ihTBhnzi)Z(5#E9qLLMacn`5G5qkL|M^lB6d5TNpV03icX zJZuHP2n2;PM*-`0oG(AfC;ckp!tkMR-Pzv8VZxi1xerxDaD|u;N~TBed@-)heD3CI zZOxR9Fd+tSyy?LDygzf0!t=JUJ%2pXK1=D~QZxR-+n~=kp}+pQtbaf2|qY< z0F=zV{bP@@94ys9q5NBgCHt<|Lk=hT3qbYH&s#lw*iAJ1FfO*XY2H?|bUeN9++Sp9 zb$`Qq%;%;-pKmkYMOJx+J<}t=o)pJYW*FB*58mHzxS$US#Zsu7w^Zlsv3T)O1=GAp zctMNh{ELdD`pU^-=*rEJR0CkyKCXY^@vF{tnHl@tnbH&fDAuu!!)v_8QALQ1LkgBG zV>#A6fe4~p>L0@|HT!>zZD5b8Rn^v5eP;Zb>>qub`S)+rQZCD)!sHb*99E_8dPoAV z7{HKF;z6AkycGPeiwCYZ@7>0Okt7my`j-P*T(zK=G&MPEnddTa+6C?Q7qd+Ld%&H( zeg~$mY!fMO{)-$l5{9HmK=dx24+U00}lv&JZi_1FRd3 z^y6$egxWNer|AZ;w2*_;VN#s_X)jK`DvAJRsY_TS3PPkTVOJ2jiNaP~}3;x3l8Ni1A0a(05cea!jh zGqTP<=A#rz0+sNb*sHM#1qyJ z^3rTh@?3X0$v}66x5feiP+D+UCEjR`e7nQO2VGygY`9GDwoxnGyY8;vxtzIr`t+o! zRJx!Gkfji83AA-PkF}lKp0y|(6_+W0dcIe77h9Q*sw%7ASuJ9>F}4V+&?p&2&+3o?v+?<6@?4#S;K4A zIznP%eo)}}*2uY*8jFxL8JA6I;j{LqNfGCPwHqIs@GcQ~bxY!~0*756kat4%gSPS> z34lwDL9ER4RvpnZG1{B1*J;ENs=sHzZ4b>R9r}F~@Sk(>3KVKk=gjz_p}GimpkEDZ z^=nF#83Y)N+Ifl}&$gVvk{&rUIWPQ-o-$79Y0BE>eyy$?y+?#|RjuDTNwczqMxZop zQ!auV^OQR#K8DM6x@42a0O5F!Y&%Cwz40Rb8Qw!C3OZV+o8icXg#rWaWcBmIQn^8g zY6ApY+PL3e!oS5y(^As~GO-Sh1E(M~7>3Fnhs$+*g^w|_b@_)6hi5}O=R&|=`G5U+~n zaj#rMZpBJ%Pxa0Cyf~%TBe14^np_Ub_duKTr3Ek1o(p}7h7apNvbq`uTn3f9IutdT zYQDOh^bf=;zX71o@NglkR^MChZVxz%zJI@0I)44-SY^LfsNzqW0viRDD<}XBax?GF}e?qVe)z*KXGg8^Y=EM-*4vAQWG|} zx3QS4{Go!Ny#pDtuNz!C^Y~&qA zUOUhV)}F%i5UPq0#n2`Y0M$pnzN*Psm|#P7xKb&&2h ztcO8-H(P9ToZZQW^vdVd9677fZ(xAapK>S_Jl0a=qzrFnb=k1-@ePr327g|)ME1|2 z3zhR~4yVj69V?-??>rutrGI$~Q;t4ZQf>Fm)k^z$h++&dx8yE*b%XceQ+Ra_ep+*0 zasX%4p0ntC-}-F5rzY~a?wsr($-_D~Q>T>pO;TDm$XGz*iYklNiUzmj;b9u8Hinz! zD}*g4EWE#=4%3(s6KyDWNIwW9hsT`-V?9qnMI5fL|Lr%9x>^Qwp#p4-m#_C-8)r5M1>OAxK3TqG$jo@8J%lCNUF z`~j4fL|D)NIw%cYdjdcj|1?ili=aIfV!a?-CSVMM#j5Q>;?k_ zb+5o}N<5e)uMZ~Br`||zg})@3%H&}$U&hK^1Ja-}bLf{LBW|$Nk;W=F&!~#SI3O%1 z7vky3oGl;3&)eTw-D-l%s-?`F)W)RxY4X?XtgM6t7;8hhPoAs?E2>23^*Hjqm)6Z< zL24eG;_|nu+`;Qe1LFfY_ZpUd*5Ny$dAzAwO;i7w!A6RWd(*m}%Q$3zHC96>ul=Ffzov1&BB#8vk;lS9#JCvPgd?<@768{=gtR#vL<8Tf=NR(PCuJ9!tzoY z5zdmPlaY5T1Kno24uoSc!~iQasLi56sR%-SXjcWf78TJEc6-`v_;pBOq?O>$sj1Fm z%_+@tJUPmvPR*||ox|2)|86A4wFJ1sDcM7y%0LlX#Wtn(fmsv^rJM5xs>NTh;n zo}9FF9CmoGThfj*A}xy;lcu2?$WtJS!IJ^&&3Y)zL;<-!9@hiR)jy#}_UAXGIhTFm zDB}`!I1vVTZ{aQq=+nOX`G1yghi%@nAo?kkoYqqE(mbW3oGlcnXlT#U7PS~jv9l5E zJn4OXeMc^$niBV!9?EloA!2L;?~OUnkp?-|Z1DE)(%ad8-x_`j7o)GBpOE$0TDrQL z3{2(FsD5v8Z1FE^Z6Kznk^6mhQ&ZGq9}N|jTFj&*D#ii-;mtta!?t?m9=w5?XBfET z=V)1>R5W;aarj>qrH<)$pJjwbw-|w!3G1!CPt^+dlu| z2Ku;t?ypq66Cq@P)MN0)`F8e`@U|4TtLq0|+rQan#M`5zc=%<_onPY+LBb#XXYM|h zfncKhj*@t_+^4lw;W|CBv z%WZ9@E`RBdKmNdfJsf&n6(@h}{?Z{~uLQ+sefIoZI?UZlv?=%^04Cxee?XLjtGIil zaNy!uDo(NX_}W^;p%u7@^xhgl(wIF}Van3qAY#10RdEw?5;FBEo}IOk zqOoFQ-%&}MXQY8*qhcJd<|4Hwg2}~Svx~IKPAFs8UUS8!O8mw0-G~IpDiaTK>W(|# z+0*x^HSSUSmoFTvqKD1EW2qfy z<;h)sp7b}>lO19@c0wk=Mr*Z@rgy6~?cs^g6uE`baC6_}hx48O{$yVSqdAr9v@e+R zSxG>#n{@f^Gow>_a9p2FzebSGOOtzU_#QsSC95>vcPL^Zp#kUY!iCjx?=2DQQ7ue6i6Fpi1aif9q28_q&M6NDePe zFLp!!4P(-MtgT~bl82~VKq?`GXdr~v7dkN-WKC5MPxoTv67EoC@yOhD7ok?Ecr&{G zzM}123wi_?Tqj-FQ#BqBXXF-><_*JzySBakDk~@GuR5V9NVD)mhF8%`A_koj!O2BC zjaQSa`oBczbuEmW8+@(5Re!K!=Hh#s*3oXq>>%7-y7!YV;cMX@8{yjg{_r!wbM)s9{>xf8AFx*>}*tfy?D9p9gU{)AzH`y ziK2(dDko~BTujTh33&MJ?X$_KS-%+@;Be{nsv=^?Tz`L_P*CJj;6|O#{?oMmGd-iI zJ5gCSE}}4Ty!r#2TfF3i7m}$I(ZwP zvSV4ES`ro+;txkFw6V9IK}3EJo`#1E=SYV0>kQzU?7zkhwGLKSpW+#Pe>0RR^?kA* zL^{)4Er)PK%d#p(M&)-7=Y}BEiotW1r4H5V*+rYZ%6%0Bky$|)>SWJsWKve#3ROjj z*vLd~KigKs!QNC}egwgYn=9`JA;ul6ya>{Qksvu`T%rGSJ0a{JgEKQTKre$;axocs z(4>p6z5SQ!4`6(*%9LMeR5c6K816jcCqH-_D)$@kh(Ms`PA5@RW+smK#W)R!=T%t9 z2xUV_m@%iO(&v-csk%0TQ}+-Gh8*m3T?L!_TuFz#-}a2nOiAk)KFLvk`mFO|O8tSN z{r3%8`ea*^dtiZ0M-UY?IE+zrz_&7{%oM1;qDWU*)ARW=R!?+us{@m|L5nNb2{7;$3Fn=}O%5F*I=M!5p465yJv^|FE}8B+{uj{dC~^w@ zV)A*atz)yiv@!B(@ii+yz;X1OF>VD_LP1)X>*?E)?b%uSxWPk(fgS zbx~{;>9ny^{vNkfv*$S78PeHm_z2^}f-i%}$=Od(Z+j(TmduqyFUiUI~vC(huu5uEcL?it;7IxHG!DqDr`ibI(NR!KPd#{>=q% zVJ=#Tq%|+Dcu)44I+vK}clcxZWaZWmpOF1jze^aNc4j3|PhDSw6fUJb|I$~=vSz99 zxhVE>(X|3!Yg(?yxm(<8CD;Jf@z>ktSsR@mD8_OWLCI^|OdK)73N?y{M5ImbcHfKe z)%Zu+$ZXtSL_>Q$iqBT$$d{rCMu%U+g&m0uwKu_9>tN}?@A8=eySkifyymmCg3E&Y zfXan8MY)4NZkuCw`uUh}%V@24j6h#{X?+)>%8Wny-~t2=BW_q$m8Xr_&>_@jJVAkJ?-HXFyJ~i-g^{FJW7<^pl((+zVWNc2i&F5(pq4| z4K!QcB9I_>(s45U!1i$6m;u=4&d$zm8($gwu@}c#)6_#;!-m@)zQScZBZ(IeQ60mo}6v3sKCOKkj4RlJ6q=KJ65%L|&Pid%BLU zJGOgi)DaRM3W#iVRw$gmp`GYDot81<_r(efz>kvugMBmBrREf9L;Ah-N0>xx~C2Gvc!R=SjWW@MtxK#FtYvQYtiEK5U zv*wLUZ}A%mbFj;D0Ap#;iI5PP4JBXy?|*0h+C@a3v>ZT0UVJTZ()G=WvHR+|M#oZV zHo7gPPX2Psc+z+ALG#B&`iREuhPfU5o8iifrL$B)Z?wXF+YaJHU8ZFN|9;qI7kd(^ zjpjdoUaX;Ot+X5%mDo*jKqM?eOK>Bw{2ot^j$iW(I*Xtojs!`nap&AyY=`Kux$oas z;SY!GUTmgiFhp6SA1J7@?CmamZfum02lWO5(TyQj!3BB^@FhnS<^S6bR3q@2GU~F+ zdi4t4fLvrawyN{WJvCjHJmy)~R1wy0(U$rk$G*jyNxrDRyI^W&_F71VeB`Wa%FNggA31PrwTiB>lCa?m}o1t16KA>BCK)*4Oup$ zl+2~+)%27rez0_~Jl>YlQ`lj{B6j=LFuL)@Hm)9gC zxLL-tJX90Pj`$jvYxGK#TF65r>^_Dzho8Zpo(J>!UuZH@Uzq+z=~jsWebg=BH5!EF zCmyYwhMX)+O&vWop5<}PQC{O9iTy9876Z&N;BYgE&TPsz(A1zVU;jy zi;VXInC{nj8hE3=8e}KIE}r1(q(hK;_?+L`&t`3*VCVRfPc&edDkPgCt&QgP+HCk? zL3Uv850$mid1MTi~%ii)d-li>y$D*JPLHvlHF6q`d1#mvxbDsT>0N93^k7g-?qB0D z1ew7#7g7mQibrTZe@xg2zg>258U*)3Q@LC0kklC&8K>6z z9AUm3okF|9SS5C}g}{7*;%{g-g%@s`zASs_NpGrzrTeinchCRVmalO3K%>`MLg)>M zz0S`+3&~o(lE6RF(dS}gPHURkhEcZtYE0mQp*kWn>r~KemS(g zJ%#&z&tHqG^SLh9cAs34ajRq6tsUARAS2M$)YMd69p;}87FlmenZd3R zl=KQdkvh&6lTZa9bb@mZs?TzCG7)UYEU=T8f4)dW&`U_(wdWtVY1qd4vxYEK%zA*K z#p6-+0O?K7A^Fi_Xvn4|po#wqSGfKC{ZvfpZV%W*;%C}Iy-3sNp0oWPf6iz~V39|W6JLCTT- z#a@oVgOwtbj)wx;D8Zbd@IjZI@ehj@|Eh1!$eN(5OK4GpD4zZbw91;b1yMN<1j$-* zr$bB(j4-LgYk{p$H!XutC_^%t$!|`A6z8~hbbkEP86_a25pz@2iKYwd7?-^Q-SRJS zmzg@Iffh~gPH`G#{I7$xd*d5vU!x*Iotf%OqQ_$L4#WfHcb8eiFr>&|SkwFsImP9W zLI1rd@>(}=SGA0*+=GaOIb3Fxt~d=qTRTRlRU5k=q`|zwPcN*ijG@*dt>V$chd1w) zLF%Fv=mZsQr@t2$-_V&_B&7CA`Rj_jAr)MyBlbXr$fG<^0^kf?Gq%P0%?ce zgERDje>&q9ls+8m+?V1(QA!?DAY;TwxlZdUimiwbU0wn%`ePbNh9c+ZN42KSGhaCb z@V==?)60ciw)|H9_=9 zMXrRGgRd_)!#ok3&YV`A2X-&*hEyeDy_^m3H6z}NQ^x~b@C=?~aHxYz4`@=b@q!MF zrC(r5v0A{FJ6|A7eiST738@bz^t&%*ik%#pA3VnzvEIKGZ{J{YMj|ey^3*h#sW>{t zE|R?V;-@fu;_oBVVuQhzPqE5o^eS0PA+Ck&Sfx&XwVu3`$O?;1Kc`)>cb!M2#h}lU zTfiMUU?c@qZj;HL{jEqO@iE)QVRYMYvGyz2q_&?G!)G#<<@3TWl+ySE^BOdMaBUbH zb7uB$cu)OBP-P89B^}m`XAm>V=Y&$(3Q~47;blg^Q+ZvGuA8Zc$-+r7P2ly}BB@%y z?cSxD1g?ytv;*xgcI?`6n9-h;uk+1eO<{~i=k!Yp>N&CMV^j;XwJ?*>VCshc$LQ5F zU$bjhWuzj+ukz$j@x$zu3y6`y3fNi4PHi;jwiD`tKUsLG2%;l{Amat5>QqxIUVi?? z&rpj8p4Bfd{$YD!UIau=L@~FK%paf#q|uWtZrD>{&%l!BIPOl+b0qlTzi9+ z+wq2Py`d>$Rmht_&QdGXcPiYGcEoRzerzrO=0f?N(&`qgCPi2~Yp=(XA+jAt7EkO% zR+W_BJq*+qBzk9Are@`9Q-^{Yh?9BbQ%<|eU$z}} z2x~>B5O2LD0qf;9*QV{k+c(aqdCyvh(vo~u{Adzg&@gK$y5=a<%1W3z9@=oym~#+y z({w#vYUb(~#1l-Qe$gius?)0sjfAD^(z_OXrplDa?KwvY4D!+iu$x6#J%c4~l`)>@EEipDg2Q`Kwc9`RM^fwzX7poaeM~2Gn=rR@(8y*gOK)z@DWoNTK67dj(xDfl~H z1R%b!BcEA~kq{6+RCw26#=mxLB8JPH9;bW@_=+-!{WhDG$CETFCE}_;!F1PN-XdBJ zl`1vhT37GpU8=M<**!0MtSLZAWlwA6H}O+uz*hYROf|gyihU$KcHV}+g{5bbdoe5| zvgG_(C|y4&Mp^up;x9ZVXtOt27C`uQok-IZiS`z!v5xJLYONdG@XWQhQ|!>4pkZ>G z)~V+y>Ddo(S@bp3;@f4T><>>eeo!)AxBE+Z?>7v_uv;_x0pTxTPPO{)E&5ACF`C>- zuP$Kj$R#-+xl@b!xr~M5D?cMJr2X1=Cq;`?NJi4rAa}Ub&P15Ez38ac)W^%~VDtB+ zjYpIuZJ4v+;xB!vAXY)CL(f^I6_rOF{(`v!4;?NeLvK2&g&^&$Z;o-&Cz8&qv`o~C z+}5IBmeTV+w1f&U%cn=*t^h`O3T^G`KwktRKW)j6@897DDJ>=zjk7c!m$K^> zvZ8mh29_fUOMo-!xZh^*7R9NI&Y2s1`FP?k0#GyXa;`<7Mt^OOsPAN{x26r#PeLfN zIL9-GHF^&32i>hvp`nC$csLVWP(IH;9}pKIAX>jc=)N0{B122SLG`&_{3fX9^sg z*eybBnEZ_9$JPb61RlHNX0o_~-9$uGif{Upw!bI_i2IF&qsE+1RtKv8GA@J+mUQ0a zDXMRiBa~Sf>5mPY$Zc!Vc4uf)_{eii5PLTAhz)aaufxxdRXFDKuNhVq+`q>(1)g4= z&B;-0Zk4Bb(msUSDl+`0J1Y7s@k2g$yxpy$edfJT)iSaraUFow{5vz~g9{1sY6BO8 zuH{ouW8QE6hraO{zuWh6V&UK}brU*q(H8M<#AlNzJKp)%yZwaBKkG3I%>2JKHTF?g z(0q<@V7dokV0d4dkmLFEczUi2It$2TYk?jHXA0B_jfzj#X(Q1xh{ySgBgAmXR?m;H z<&qA*nBs5!SMveJV#R#KO7vAa3G@NjhDbhBL;rk3>kHYs>$GU6i#iWlrxw(h7)!FI z_G+=iavWupNRfuthOfW#4J;Ik4Bjdgdh2k#zWy1H6S{hit2WQDlUsS#bKv{5&5ZlP zR}m`QW9PZy{aobrl;sxxg27O6<2~!z5R-yLpEnzYC>p zwd9>H*HEIK6U;1Ih29Nll!h5#47Szf2W!L8lo@FTux)^rMX&;jP$Tti5ch7e#t=D> ziZLX1&r@@nY&7w=NW&r&KWE2Av0AVjX0x|O$AJ{7pEPNBh(B zf*;xnShGK|#6|lZq}J>A%S-^WInuuRNt!rZMK-NDuU@4lTJuW~cRGhR$0~TBDn`hD zH^wqhE?IT1<-V>cva5r#>>f36=9T)H%N8?MSC}>0eB3$q?iPA}TqqDeey?^4yK^2x zf%QDAWRl0VG_BL88@}oJxc!qL$(-s|^eBg=Zkmz}0lbhhrI6F@g|l*$9qis&)YcE2 zEK|Ba**SP5NjO#NxSr*u(_euTT%RYvx@X@JoahM(fc~ z{T-&RXz9@h2wbN5FtEJ9gIBfDjG&JHQOdf9`h7gh^MKavK*J`M`^E229@-8q693p~ z;!m-|(PceaIr1b)JegkBB;RNpgJmeI95gCONwpeGcKpu^AVsulCvQM_`(%3nzj)%; zFEfCuOzN(ix7rcNrI$(wBfa!Fp5D!DV_A}_Gb#uASd~hu=EAyW4?PM7DlMkwRb5U}>5O~W&K1?!onYEi$^hV9nHYU?B+xb_j zkmi}$t7C?%Veptq#k(E;r?@pYHz%v3Guq_}$Dfe4QVXZ#T7f1?oQjSV%Nr*Bw3^7$ zG7#}=uk^j0`f`DC?Ej;3#n6$NF zMPl4G<$O-#Bb1+?mEs-n5Wpo>g~642>>qWdD` z9X|gv-i2)B&+5Sw$|~`{OQr;#jN)Nuvr{=Amc$*3Z{F%Z{w%i6&4eyZfFf#k0VY?ju75o&;5Q9MNqaN&m~WHGPobO=WzQU=P2ja^3M9m z5ANcw9xp#ac{=%7AM>m~jCc6?`GaYTC7CII3vz^ffBNKm&)mK4hY}5F60s*ji>Z;4 zbC0v+26GIakNQt+ct0rD5duk?G`agDgEy<>-&O)TqPYvE#hk{xPwENoFSlP*2#aX5 zC%^P=PD*{Pe+Hwm``^t;@CJm3hjWqc5MYKQiiKzi^l|-O>fb4Sepqdr*oq*o45=~Y zvs+J8W`=a5kj0|n_SCzRTIgm(|Er_ZNa)dNQ+03IikUq$qKoWUE5(tySUHlSsd+WK z^I;g*%U8bMm%eGhp{BbO59`gRbz=3p^Cl0$rG>JOfO0jU%vSmt_}n|#YOt_SdmcIR zSnc1-C?ej_n`9Q;v}Lp;y~|dDWQyu!X?ojGIO4>jvymqL)bah2;k~nzo8dQHe7+YE zqz*0TO~%U8$>pUscDt1-MhDKc{dwolORu|b&Dpl~T0E!6T+tXs&iS@0-))Qot*s*k zTaCL8yW3U-1@G1C4z?)eQst`aJOI4{F&${Y@H*}5QU5AViPyIxq)X)D`#IA-yus)u zIbr+mz4wu|a`{91s7E!G`0?}4U1l|@zc9y4J3sv#^+j55x7zH7%8~Pa%WkWljwwz9 zYhSz67L5_Yn)S{FQ^&yhx}c-_vsgA_r*^QTf;Q24Ut}`blwIK-f@;~|&za9zZupH* zj2E+S(>%p#r!wwx0#!47LEV%=b^{m5)J5 ztKz8Z-@)$m+bDo8L!BrclN~cL{L(m|x;sAN(9zxI56TzG=uUKgIK}(X(*HN&g+yf* zo$5s8SJ|2$ePBhvVsfqx7YJIw1)cr6_HO@%S|PFY87#e@Zuu&=b)YQDW6V$(%e627 z7RtHL-CtPgR%j*`?P?;dJ~A@T`Zfv0(oHM^wBoLy#B7)_|H+o2A?@(qzmA5IL? z3~EjB3HfC%-bt9r2XJ?kudtE;x3Klor*SCTJ9n6k@giXrmda&L_}5qG>hGD*x=zfs zVkKC~2U!3%LoQF_FVAlzTy{5XaLG*D!R!?oyi0{StqOinBktoeW{U#xA*Jwqonwy& zOgfam2V`m<;Cyqr4=h)Wc94cjddkI^nkG~+;*m0!4=QM3wHy#l6-hG zj?qGxraiRT<){x+-9%pD7meISXj{tRIa0j%0->pbb4_7cY(k#~9Zx|@M$eK-7w%%A zYybnwYrQJj@{ZT)>UrP0qw*8;j(`qJ0iPqM;LtfPiQ>TU@B@pY(2|bjowN}98m&=_ zvcH~@M3A>XebE16IyF_|v476Zxo;^%opr7s{EsDUJRfTke(AQz3dJMp4p*mvt-lC0 zj9Q?8R2X1|vaWvoQ<4JRSxkIR)5)Xm%iMlWP8(c7k4g_cSkHccdvB&K(2}1A!e~1H z7$JfPwhFjb*pck6fzu~9Q@ zi5O{~_e7BkZN3lOENvZUp_51jxxsi?XjTNR@W>q_2|)%a)m@0`bW)EBTt|87SlYs! z!&(gsHuz|ip`pL=c)eu7O1l<8RB7rlTTZoxbrSTAm6|j&mmu3aoXAY1Z2Xxr>0Beh z_&=W|k0h?BfG<*n5-CCAwbM>$;IybbWu4ORrIjj9Bl9eMf<)Kv@doz=Sz3G^Rzrd^ z3wTk;U#KzrR{Wre?*<#mM@y7TuAH%GF1+Ip|J~=o<&3m?w#yRC*&?A$?N4iP!&+n9 zEC!#e_)o%_&h_egx*TB-&Qx0m2ll+{MJE6q)w_~Uf7bCiJ=r~8sBP)ZzmStzZl2q0 zOCwR3)y4YDdyn?TqwJ1uC#kL%$o)R|C6X7Bm@M|pDar>NW4w2nXyz{e)&m^k@m$+e z084V^Vg0*65(b0T_kcfhkaW4|nd0=i1j!L-|1hdD6v;S?P`kflwb-n-J9$nVydO6( zwROx$yD`W;%=8H!xarPA_?DFuZOX0-aIUH~zuv|@7bG64&*OiOq!U6NJ2+dh_g=pp zKJ~A)`B2#+05?VAu1cYPw+B(gF4OT56o0>V7SQd@WPg{0>(*<@p9%BQUArtuX2BAA zHGTDYqG&D=gBSUf z_`*G+@If@%EOY}4m^U554&vVXJ5OhT;RV9zb(z-FlYYy;gFZajm2X^0AD|Zq(l}#l za6!ywAIH;Zi5T^qqebja-0hH;j73bh6)@%mWb_ly!^7we(L-$79My)u3`cBIK2~2U z=PFK`%6soBLS=eevnbX}M95+7^CU*LDbv$nET0U%qDt51A3Hqv*;24m7?oa17O!Qq z<4}$*M=^+MD^Ly$I+2N}C5HOyk}=8*rjUV8$VueipZKT65eAB!OhrkZDT<1UxYJ1n zreW53S5b49gbagqCQ-@%(u0x~~ZNt=zK#;A0=1`m!>kjz#B=aba7$ynFV% z27h2;<77YW(}`0`9aee@52XpUXInu{tAXrnDQoesxj9%&p!@WFDw+7>(_al%@zUD} zx&R7SG-XA41qv$I!l4K*@*=plD;5HV=nN3zswo_6$>`0HDx4oH)>Mm>WKwRQ3jtvP zJ|S>u_Jp53r&d>oL`GHUFIzAbdC(%eh!^`i$8#Gk>}jz``47bO{u?iHrmXfCRqIwK z(VYA5(1x`TVELKX?;Pg6HlI(+y>U9i$40yMa*#dER9X&|Ln!5-@SLMoHRO(vACB$p ztAk??2hqo#6CsJ(eRW%4$*QTbd%{!&7!jz2ZGw>lY|#)gtzXz{sW}Wvc^_))nxZeK=?h0bb(Uqu z9*o!)7uZ+VPpe2bWSuK2$?9&^zPWQ*JpK+VHB^_rckKO`GqygDEsJ4Uq~6l339MHU18d3bEV zN*2EwzRm20K=P)z8FEf7t3Jt3$&EHC2Wv64s-UXHcLDUMlWdl4 z(0w_tfYYa{gD?H&o>EZxgB=wzi(F)Fgx{RgUq#YKhkZYr(&5d3EsSK2s*ukckO_P3 z^7q20;ne;DoFv0wXJ>83-HYoYGF+yCw3LOSw0B0_u=^#HMEVKg3N#A8WlQoBO6_GF z8YW{Gf6sRL!kypJBVtbf!Pbdvq{N>F3nTw5uwe7`D`v~k|C1REFU070Ms>NvGZ0y? zF{Iu3={(s;?Ud$~eNLTmmQ|1VE@j9BnX*PZME|Vyu|iKb#|&9g>2n2GT&C-IrIOVn z;>NgSh7S$#4Ty$ft!yi}>gk57cI{T$mwe=<`LK1lzU};Tn{A(v&P^TY_z*xI_) zw5|V58xAi3Ikb9jq--p8=c&`ktgJLFnhId~ez&7EYw!nfG9dv$1Vb)ela&xh<9G3+ z?0Ge=Nau%8KEm}*KXNF@!u6G?dPa_eCu{)fE=4+=?DLSfM_}*>~^x_sbyDuuImhJ~BK_#5A|@ zK@!{ew1I6}Sd$EWU|diB#!p$r9(*pqD)$}0ibf&efDQqLURaf-&=QQvTt z9$CIh0zHZC50-=(xir6ti}(#`g_0=i2pp?L_tb^oHj)Ym?5Ft#I6Qc?Z$7ZKJ$ICj zu{ZJm>D#;&;NjFZ_foJ;?*O0VT;4pkwB%02A#XBSl8)h*35&7aeb-{q?7;%-zB}0cj>|<^>1k6bY^ucux3PsAs zRk34g`8^~J{)w=YjA8BaLW?Fl_Ak~Po$zlqYgU5u3(G=3g}OsL!6iKl(Gk z^RI8(k&G7uK8~b8srQ{YB%YzY!p!Ijt75p*w!nO2G5d^9ApDbWxm6FcSZvlSv)FNg z<3KH3ktC@7#~&!SiD*@2CJx|7HFfq9r#;2|tzla*f0oHm%ZocKOG}WvN42rIt}_3m zijh5@$Nh016;@aR6Zwl|0`RJkd48UQ=8#uBL5_Px!>YFo!3mYD-aPa7j(Xl$Rc|m< z<_x&$PG=YZO*>yCOQe19+nXosZsYZIsR~)`50@Voz@`#FQPM%jl?HF~<>Pj< z_LHRr3Ac)#pjPRxtg&Hvnh4&>A`r4oPR6`D(s3E&rYv^{3ptFm+THILV9;n*;Y%VA zHnS?e8R~N5?U2W7aEZVFW<|R4%_Q=n=+bz4FzS;2(5oFn4*tMwoy4Djl7akO=I<~} zbP@}g2A&fwth|dVhZY=Z7K7RONdPXFjx&jkCb=1&y%-wFXZl2)i+YTBX)ST$!o)41BM|cL(CdjQbjR zK5@16YaOawjUq_+wr_UCVB}S(JxYN@+a|j?ROVbFlV@ zE3VY2t`o!m9Pqd5Tv8hkbC}=i`0s93lc0C%#w|TfGFbI&H0Fg)BW(>8ohb6Gu>$rf zl84=w-jZ9?>BvFic@Ft69PwIN)J9-&0cK-!^ZS_}EfhN>_e72E6CQK7^ne=axjr(L ziCZ{PZEr%`n+7ud22ph%~e>LzW)Z&HH5|iN`aua z7s}pHk+CFyd;8Aqjj!y|CA6{1T6fs_J{ELO^(6ZfIk$Uqm$E#WqfF_4StBxwE1D8+SUx9=J`g(go~N2OGyC+s3?S-1i4g z>nbO`2rk7BRhCS@c5f^^U1etBC>KqAg1WVE>b&tyxIlE&tL*mZhsiKs7SAaWx8Luz zI+I|VO`^f&n3zjU;VVgpRbA(*YdC+oLDVOWOlPN0v~@*yqB+81qP0yYk5%6Fu4GvA zlrTCD|2*Z_c8~)GI?YV}EdZvvx#?OGE~f=;xIf4_oZwaXg+o@P7T_?VBYjJl{@!5W zC>Z88cXreYfhl{RKa+xj0;c_k4i0ecxzYD zFi&wlA*;2b|A#q><(en^iPwABKOqbg5Co(|x}{r0x=Td5!}n(1^`8&3X04eKp68tNJ9q4T?d^IPF2bdv zZ^l(6%s`KzeAE_!^i8ikQHsu^+aDpa)?(%aY!fihICm;b#Bt-N;EV8UiBiC63s?7z zi@~+L`vER4NJcUgbNheGCegllv_J8Y>Bdsf;wKYp-A9A1_O85c+I%$s3JtmXy#3cs z4RdyTKPj-d(TP;qIb-JIjl|FPy{*8q!$cS+17!z6WcPzdB1M=) zk8$YHUPWX}ws}&WLKG~l89*#?ez*Z^`tr#qgkj)y1BlCT;IjiW@t1AL?Y7~BMoMn; zn@k84qp~PY$Pw3*aC1?n}wA&!W=^_*dIA2(KRdnnZX~8 z+i1mgisMIKV)HE&O60ba5k>Q8e0XC{CK)X*?jEXS_=`y%aXOl{s4G}4#l&D- z1X*+*o1eYs82J!u7e2noWneR*(?t6#83E@UsfXm$qH{OO)InH5aA7`}T~RVWZJg{QLf=B3sRzx@9LH*F9ZxM5G>l ziWpIlnX;*^Sw?dh+Yl$k86foJXud0jhaglo$ZM_&`^83(>{L|T@p{x>3A`^D|MRBn>FW#r z?skw7Q8frPNay>Mm*I>eIDPa3L6J#ZyHlZ1wNzsmyfZLwg?(_ITnyC$_F;M}4Q^k?M1L$8~y2~Dq?qDaW$M*BUWUUed-O<6hh z`RnF-VKRcT49|4Hm507sZfJOo4Rw&vEHvKREhQ7j z6a7Z<_p5y1tiuGs@NJgTa)?!RJ~wo@Xfeq~6~6#;@*?$VyiD0PO~4Es9UxfaBuNTS zWYza{;GVR80UBnoL(4K;Kkx24lI04LmGezoo4|cwJ+z0R`soKqvOwGOx)flFqV;~T zTZ6225TbaPyz!1?KF8mGfIJk^JAi!^wR^J>ooeyjB^GtZZXPNQDZ8}sspbf_r z@i!QcR4;wzF^?ND6m!I&RvJ-Yx_7t%&p)UO|9lHZp{kXB{z`E8>)WJjgaoY#-3DiQ znJsSYcEtc3m}P4yk`yrnIYP|nIac*GNzCXAE;Xwl^VG=f?Fj+rS3^#RF=90KaIrxT z4#V|Z5pS2h9!Qp9&u3<~VLxy(O!}dy^~cXHCvJ1MW!XaSpm2t8w?z#Y*g?Yt`D_i& zZ{NImlad5^u6)lgKchKonTrOYZQzfYwU2)u-$KBCj@*}6Qr$NC7b0)wY~UHEeMb>| ztN*d}x|{(L3{-*o=Y5)B`Y3nprUbrWJFAN{;zXYCC}Wv63;|3gdW@d|?>2Xn!hi`;0vZTY?SNV~EghX~N8DKj@`UEHc;WZgmu=gZRe@8ElB)3!YjDre; zG!{Cta_2$yjiWpf1Kv!vZ8B;G>z@0@#^Gg33Etb8+Ph@-_S$r~Tc?*e-@rp?lp}qP zTrqqcx(v1NCIe0@LJ4u-2e6a2+LN)+T(%4`2z4w)Jt02JBZ##^b;K&X&aeI4WVMw) zn~RF~C+Ik0fg~&jN_$XIgP#FL#vKe&GOrl%_pn}$j=Gxm==)gMj0b&aoI$EESYSPz z$|^)~^-co{LTu($9!TBmowoi=RgHJghm0tQOmdNgqwck)erfe@LHKwgCH-+4HBWhT zJ+gd6ROCDGppc-f>`5bdgySHKjF*VY-};D+~Ax zlSbW1VA*g=f64Pk0gwTtz}VW}XA8U_hM4ncDNYiYauI}eDRn+zfPcuUWF1;ED;&?( z_G6F*m^^%pD`&EuGkvVz+-GPn(CS1#1v!8sRFxhh_LI)W|YBQ!+{rvq?c;fySSCsqk+d^b>JPvSV%L_lA2pE(0*aM(aQ?JgM&XJtc0TBB+D#cUQ zEKi?WOw7~-yjZwDTX~x{A0>upO?&?Fg}Fckvc>35rBU8A^$!oJd=(Fde-fN+(Frll z+>#&YcPNQSJQRPPAmlAoMk07qZA?(D((2t$Cw*K;`{2#vc#?1lP+=0*h|#ny+}LsZ z-yt|xuy+|KvZ)0}3JlE1y0g`ozd<&j=sRHhJ!>KjM;1u89ai**V7=T?q38F-UNYu? zaj&B1sJB36_;_A$hv3SI?avWKMUHPp?ERFVh>3oQ2?^kWfT|{22~1{Y!x>E=aL-ra zc`EguG2kBQ-VRgSACqIuVNAxC_gZ1sy@v;cU{BARA|ID#hE!j2X`Yq+Bv-N#ASMG+ z2;ycfYkuTS2)oB%?W|@?jRb@5#rg!5<%jox7V`-RD9PTrW-G2NW+(0Rb+SD`NYgp( zIgOQxJ<^m)5dIh$Tp7~75VY+%I2;v${O-F++Yvr~B9M~~oDL<~P|)AfOwm8Sf(&1U zLh9~u9#>8(q)JKzjV==jf+gtzxQ%c_OxPc;xg{E8Bfn>3LwNTYhaQ-61|<+W1CUtN z=CxeF&lgQ@!*ZZb2jX{<*d%O)k?xXueK@L*eq!*4An8$WMhwoYSn06p_jP6OK?QR9 z1EP@uaV99JXyI@xA=3HwrJ*70UK`UoBwBs(VRJ?KCzh#)l1c48<~_OPWV0wI+w?yz zz(O-jMS#((si_%hPkV@q&aiRAl<=kv!S4#&-#J;XVALEo3<`z0&%5^hw7+xR=bW4M z$cwv#hEs7$y{N~xe&^CWsUFS}7K)DN zh>If(p|!Y4J#8Jw9Y@g9<;045gH_2s<`-@A@JV)q%Px4uod&+{RGAF}F2!Vm`c;(< z%kc1U!!5%efc|uURF|E;l}50N7cMcWW;o|@WIl4w@dlS6-zRDT#SH0g>N&Q->J6QE_-ZFSdQt^v=L<(#V^a%1GsR=|G1PJGSGE86% zXy;P>q}HU@0xXQZEY$C8Jto4UOR^CwkZ!KU>%uWcmi)(i zJf8Ts)>$Ap%>%T6yzO1u;p|1th`8^-3fshY9 zK1|H1_}khYrT%L+(qjfqJ2cTH362Ze>dc5ScvQKkOfRo*AWZP>46sscZ2lHKKy8%m z>pzzjOG4r7GNMe8Gtj%G3(agfkvtDEx0k_6ak&F@s8xsat-;&#gVA~3A4S2=5t<^@ ze>vTl7ya4V9oCK`=HDIK6xbpZH4XWBoDce?8&^)p=+N*mL{V!Hzqy8r*&JC$g*rMr zf7N1EIgxdr?BgJ^N8Rx2_$MRSt4?b6td68rT`fpTCV9~G6DgUsKMu@}V|($@?{(XQ zy%A=M20mO=aO^`xjpuE>vFQ~NQqCY83q5@P+G zcc|M1+){WOazz*l|F3VJS@R3JOD9e~RZu$FClxqXjO`#%*_Z z7jEE4DiZF+slf6BM!o6D$#r&1hzADbH4i+|xF)fJL=V)L)W7(+!L({=6;7VX?N>o- zt#KKim>zq_lluSG^l#QU%!d3=pOS>r7_0X1(8u1R6lP>0V13@;vdO<8$G^!S_v-4| zJ=NI|X|gr`XrTu-82r4i8~D7wXJw2B!1?(wsQ)$0RAQy+7F6VZ(TL~o9mO^q_csXt zW*-|FqmEG;%%-1$xBqve3iwG%*^gpMIMZk=8|_57tAr*e%{doqc_1&EWgKo59Tphv znzAHC`46}d7c!9CCY?uigXsf^V9_BZ#(f$S%1|lr}fo}74(>TmTvIs=XQCECp1Z!mvxyKTi`(Hm=;_m z)Bl9XqoJgLiQx-Q(eQ}GTKa2zrx**F07_!iVZyE;kytfzp)YOYVdfqAHF)ffs0_}J zI0JeoQL9{Q;2{BrsWFqOyy+K=rTgogUzn}fh||{Z7^OUf5fHul_PFv}nnrl!LfH2{G2!VKR*85I(6wXj`x1!sKi{)8 ze~R)hV<@OPy??EKjwOw6vD=mf6-m_QE&l_u7blgTR<(Y%3r&u#pzc1^N~o!>hVK#^8@mTLJETq+gZ_;P$@`40LgT`>s4JF!oR09rVC)=qUl)GI+8NfSBbBHuKTFIpwg5}~N(~qfO;|!WD){e9%ge#G z50-5_Am@RfZA1c6=1WVT{-yp@dcQgz?4hoEi+8G-gs(^4#S?>4?`J$odHjsH=UBE= z68|NK!j|jNnKwMGKPM*>6BBP|BtMuRPa{imHzUL0+!{Ii92jm%B>F{xv(XWVJM(}f zF84exNy$a;tGv`Uf=3=%K2ty~5t82Y%1_+|7W0{!_X<|??iV`K?i7B_mUg`|F=Rvw z(JBA?B0K${?*Y{`2{M1b?0QH%UnR;%30jpt<;%ELOzFAm*Tz{t=)#*W?jB7zM!Mm> z_xEvY-X$X4aegfmg6qT?)r z{yj6Z5&rUH+z(frOqHvCOrLtVRq~#7(>}XaDIipl&lxzP9R{U1W??4O;TiS2jt z+RH*~7`8^Z)pBKd!sP!x9jlTek$AXEEdOEYj|<1-{+d{lB80u6TBd-6kyFpB7BSX8 z)Bc8?wZDt++p z-*#p7+65iVTYTAbqu2Fex>{%HIej9KUnotYm=;6UJvBe?30e{uI7Ky~Ielck!+)<) z8t)H+!`H;eu_F;QiGHTolEM+reN~(uhp7u!6Z+Tc-(cd^ySDb`FMF0@WK!JkZzOoq zr_mdGAZSk@a+mwS5x};9g|}lKSVr`-*pZ;u%T{QKMKd2Nm1?Zq6<2kF!f+%wY$G(npaerZ=Os0|QE(v=FYY4zd9d_d zTU(=gG8soF(Dbkl6>(3g4aIPUZa%tr(53020|rZh)Ue($OkA$oS+Clsr>Egfgc^tw zVdpP!Hnn>S7HgX5Uo$hb8(c2z3qgB+G=_QK@?_<7^-;g>XRa5D6^pz?B(u<&3?M?r zL2Q3m%J5AT#ccD3m-j<56)A7pL_FNf>av5IJ@LwFi@0?;Hw@CEgLjn^0p1CkB@^%_7SW;!KVM&`KY0HEpjy~l*>7i>^zQ%e z(_;SG(lR`mzU1E?)%?I=9H~DuhWm$x*;uyKVRzofou!D!3F%U*P&B)^xIjdRq2ZEL z?Z{c;bLnSCG#cvpm2~P;P3|)Z?`8n(aji$BzC< zq<*njptuq!r%mX%dHQw_GM^za%e;0b#Vr+p1rW##2fNBAvD{H$*vcDk&HOX1f`C+= z+c8sf#d+b;>L$+X^t#_^f!Mmf0=1nFL%SXu2+#u-plln`%wbp*AUc;VlC&l%f_j#h zWS%HTzeHt6*E*h{FT*tR6(XU;c%OxkRU6WfJrLtn^AXlyi{bu2t;&`~<8S8|?l%3% zRJD0yFK9xdwUfV1sOwnnSW^2#gQ1`RbG=Jfo$O^vp!tZa%7 zLHmWlcd+nFrQ#lW7aq}VqK33Z=|9hVdg1E(vV9}%OQ%E%<&woe>vFm25Ku+yHj-T!9Ho&0#!;TgF@zcuFBa7kwUm(nM#~JQ! z(9KR*a^7gYn)y6kyf?0R(Qe3{XTc}h$-QGzMUDmYs#GGHEdMOs5∈Z5}UQUoqZN zhAXnX(4jSlV7&YGg4vSe&Y>ebx$gq?Bj(#_Fz(UkFq=2wj*;8|<*ymMhQU(V4DW88 zL$($cXr@)Ll#FI0`k`ov*Z;h&Zz5xCJTLPxn>gc;k9k|s4kqub*Ix1+dYS6pbZNP) z;ruOyE=MT`62I6aA=-ixUSDKnaWeUPWFt$EPIEqLUyY~dOORqfCqm9USe@Ifi0|u& zq;M8ua*neF@u1+j-CoNR`Yrwp%{I;V?{k5n{qK>IkAbzt1dV*Abr@PFPUJr)jhqfbpXj8&p^Hp2v(+30^6 zpx;IP&|<|Ko66BK#k-pq=U{4z{HWireSILadH6Y20@#cR^};1O-rc68$K8C=C<$68 z%a#C6?5#pMU+pp6%+OtIIn8_hyM_)+COYoN41g(RnfWeLQ!15u$t!vFFW9({6OKC)pcGejRTu`i7(ElL?-RX{g4MD08)1 zsgY>NHD`KiGYk1rzSDKFv4IgE)pN8K<8O2fBF_|O$^~BT25!Ge3+XZfoweYDH(q&N zgd;XqRu~~kj&-wZ{fULb+zSF{Dt{MR^cnFB>Mohmsm`CUiL(ve`%omTbNLFlpCWd^ z_UuzQX43*hxi>Vx({gy;`25{tt+zYnjl`^Ft$AslW3to|zUihVsVaVA0+R*Ha99t# z^C+~)7rl2^seID!1eT_mqO@1XTX#O3zx-DcBIe=Y`~7WSH)kgArdbryI%vHNGJviv ztY6#v@Qi2&V^2k9RY#n{Y}$>nCxDXu^l&1cZA8IUygnKoyE*Qsp?ba?yry7h2Af&^ zSFb9V`)f0DY;6bAAv$S%iw-wLB3GH2P*K%cK>k44Ak>>nGN9*!U{zGV;-~^Dbw1sO z?X+mGSO&K6L#W+h@dkMt=wD(zf0FMY>3@p&^^iiIDZrzzaf(T~O^4?JtDd&@m{yAs z%NaLPnj`;j=LYz{0OD}@;ovSi9&5DN@{_@&86;fc;e7jfR#e{tMqH)#d$wO4e8 zUQjAAJ?G;Cj55MI)0JieXU0Rc-RxblE;|37p(jYutwcv(ocPQy!2|*Oc2=ElY!s28 znY^xD@Vvy(KNI=JC8GqSnjh)RD)usp;So62R0TIigRJ@j&O9Mz2&_+8Z!mK$-P&3d ztM22tzziJ(rfFIZSigHHI^fZ3OZ3^G%8(!)>v^+e<#j#jRcim%P-_cyRjQZ|a(IWb z=QBX2+u_&QyS zne<_e(2l4l=rVzo6e5PG*WCQ{(gq1@DEcK1&!;ShCKWrV0%7#Lu$yac=ZaB%4WLEc zCii#`OPc7OC*_R}$G<1Q5($Un#bfIGJ39t!XWtGhBAK?%(F%1ILUL2LGwytLmy6LW z)4L!M)kstf-a`gBj8jtNcwBjDM3{>V6GyROiba-Tl7YcjKzV1hswehr$*po@Unv-V z;c@w^)5A_SaSY9;wZ?w3ZLmQ?>5iJ*E(r~-C1Pw{_O+94V$1XVINR@7B&(1Ki&5K5 zxeKf&)I|gmuaOLrgNobqp$a8NCG)oIJwlcN^82gY)E*R6L1c&p7Cf8MP*T8fyy%D^ z-FD^#Zbi^cKq-4(nAEgZa)*YwN`Yr-*q>(X{)Wc}mKm?`AUVOm3@-Nz*u_AcvGujR z6y6`XQ+_=^Ec~Hq59wzYO$(Pz@K(peM1{ERYz}EgiWAshnpQoGF~;%7W28ySk+Qfu zss`CR3O}1Lqm@0>J&tr*pI#Icp;A^NtMYXxw0yvl6~>QyO~s#*00pBYTmTSwfTFCP z26a9H%VB)QUj-cx7IE#wVwHh9;^8nXJa^BMVjAB%0tE_`Ii$9Y_ArJ4ditAxTR$o9 z(eE|hPW->J6NrGBu&GI)Dg;j_FG)UqD1<>6fObXBYe${-*rCF_V!;X(elh1CaP7G;8*+7| zZAD3xQqTcqu)Te>jeKieEWxxyL z@VWC-z~lc)PY;-p^X8n(XsROas6LfCEC9;SxX9ywbD`uT-BRd7IOM00y}2RAsFP)`M&S0$Jj|22(>i8w#%G^Pzq4Z=l_k^4%&z4F2->PPx zm8r=Dz%qcW9Y*BO&q0(JvEs4&egX6VWNZ6qG;!Y{^q~>y9F&U(ng$oR`sRYqFNm|5 zPMs1}kk!4@D#_RC%B0pmd#NCmM$p#;eU|W(;+V83rv6;;MkrbJuz<=Y*P-cipVzUb zR~I>}g`c-l?8bjE8qllE{L%uJB{esLzm}dJ8n4rSj-oIj%|g!OEawIShiq<^W>2I~ zz`$ylgFEkAj=xygrOg>2jByy=S^~V{$E+QhnWra(3#zz665bD` z!bFKCrGPt~Kig=8mkf9*>U4&_?rd3+q>V|#U2|u{Thh07V_Zj4Si39{O_NF7=goNS zU2(FSyM^-4(#LGweRq*l9=$_3%BDv;&j})EAm?wzY-GLL6FrK7LkP-8zueAvPKETHO0n28MbWj zNCOz@W-Ie%>vp4z?3%~Cb9xEz3bn`3M|E>0WR68MGXL^c{gxojx1*#ya9U(EG)PHn zsNiZ%bhBL@)b3K?g`;!tPa6fQ#He}PA4&$1Z~f++z%RJ93xO;ows#dm(OkF29@(fL z>|`gIJWD~09rDlXLh)Ih+xy;?SJx4mKE!=iH8!Rx6lT|B9*m=*)LGCwqYgiNHyxZW`r8|D zF4UX%svrT+p_ubBt1(NC;@s1G!5=5#4V-c?j)C5C9Wq3srA%yW6!;1!EJ56U0^cx5 ze?k=cRS)t*teI$zCTQa0%6q(I`eX@udbx;s(meW&eHF-lL%#;dM4;KImQ3V~niq~I zXJ)b{_K$l7#i%32t2kgFH6qy2TB!M9&~$~Op;F%foZ_k~^ZGNYR*PS{a15;>U~l1q z`CYWZpA(rvw34d`A8+y}3o11g7Rr(pM77O&sIeqwA`Ov?+J@{{)}%<$k#)mH8-$Qa z6j2*W9t^82Dt+LKxAHAeTpZ}1e>)L;PTVd3A6 z*K#3iFmD9x&_Hn>D@9k75RimDA54L_!TrF`cUux9`LJ`bfjp?m?RWTP5RJFy;0BHZ z2p2`I$8b8J&J=RZ^f?U>SvpwjhtsZxIV>(YsRFnz1PT!s&?6^1Qhu@3D1>SmKAcBzS%?A6f3KUvbyUHOWfQK0z9tN%q zq=JF-z~$Y$=ODL^?S(Wa9$QaOL+2M?f`<=R#*a9AyJgw@YXs(M8?6E8fyvdgfM;;T8|p5Nq{Z>w+_@S*8}ZVkFHzZPq|_IB9;)Oak-ScMtzQD z+c(qY*Cgc6xuX8%m?@Py??F)l8^N^Y*$QdAEe}`$b*=2fY9I~cRN}!M%A08GU(<@Q z{u?Cg+}LR%x|4HLQ*g%xcZ1e~KU%7lzzPsAKy=KPf2FT4DUq>qoRA3;RB$%!*8hcN zN>z1roE$xzk=2!zl?@H=;nf$_`qSfOrcAzU{@BHZ=mK6w^Y9)ALYa?x&j%*RzCfMA z!{2@RH$`zvoCwDeg@`*)4do4n8rfbezkLzjyf;H^FPL({?tI15uHVcTu&FL`JzBpmlPm{+^_}l5oYYTS8pY2w(}mgUS9vII}eRjzDaFxE5L4A zTN_4f({!0CwmgGq&3V!e)%-wLV(S7u8l14^oErURkLO7Kj@i_r_z44EUEK%LgSUT{ zLTWp8>R<{+xe0r(AeV}@+&|_cw8g@=Gn!=@G0L0&(*o#BEi6FHP8-Ng;v9^PMP=NI zrjFw^Urw2GDiBIV?EE~!^tKYar!^aL7@Vis;O$9KPX1J4N1Bn%LfYzxMqOaGt(!UL|;0HSDp@;k$*~I`oYnPH+iX?CGl0cS z5SA}0UJp`dWDUEwy5JYeqeH{phAcCB*MymhZ}@^fC5MG*@a?VM60 zRzUE>+z8WmU|SLr5!LXZU5ztG&^ln?cke+ck}Pi1MaxtjSlg8jS^4` zu_t4m)?+?ck0`F^6ppZ^5`0c4f#JM1dQ$gzo_20?K2`BYxYHFm&uj}rB zm%NL=RRj1}pI)%M9dL}O{^#lxS{^AHEkzeCMK+^FqlH7~6wfG6(}Zp~s}bawmq?jx z(?@$}lXH=*Sp0RuaRXp$y$%?>4j_PV8HroVQUfkX9Xto{!8}_Ow_+5xT=7}Pcp}CD ze4UVh8-lddb|lj;syO~5&;yWHsHXc}7s#@d-S$}>)|@0tXSpi(+5Har-|o9Mj+$q= zsn}|!=o^Cmi-naH9va|1)s>Ojuoh`N z>F!i2(BBv*H+pcXZ)sk(A1) zJP&^_ws>3VYWONt*?c#8cJ=_EeL&m=xZdT$<^1Uv>!3^5?p(cc&GJcj4PU~k@;r4= zHSN8e_Ui__A}LK~LI7jp<_52P$34riSSAC>en+?eK?-0O{}X)ifEJ2KnmP8%(7ELUdqO^2=^Kb1$^V|o0_HnN(Ucq$Uy4l9lpAIFz zJ-yUs`%;rnP1bvNT|5^;DQ**qwM&hVUyxvRu%QR|8cRwHsQX{$I+^`vD?iEwxxf>S zU#1V}>AU9kRwnc6&Fj}I zPHW!jr-tuwdec$`BL^-=5TJq4-F2UY{LW*agn=!@zjrad3> zx9eAD1n(}TLDY@U;7p!q&v(WOUUN!z?ceqDHnqK)LEGw|Sp;K-{k$pvQp%5iA)TXC z!D%^t-vSH1W*{?Csq<7xWi9@AAd zemgLJ(Mn7C;u>Rn4O$u4qjdjCpJr23;=FWtpY`kM){(cQ>J%-dYegrgJof||TT%MV ztDl{Ixi&=w=@A989zXAat`qfXM@-spjW7arIpM5P?*^@=6pbCFG50v0y}IEKe*sE8 z0H|BoQnWzBSyMBy<7~LfU-8;8peuC@DxxG7LoEu|7ylL+6wu;0V2lPp^C@eU2`+XX zqcx;)$_&2t9RyE*N1H*X_9eCgvPY4=55Ra0j~BGR@EEK)@GuKmw8replYRZBTkRI5 z8Hv&Ee~S7V%(vjI9372DnQjY2E4mvkK2+3%N!<{Gm3I}ULp+c=AHqVo^~Co{Eyp*X zDS|d0(VpL#$m8TbTj$k+PyY*`6`W) zY#ggP9pqTi*1TZ5Bm$hcX#53oab*}I3W)rAb_fFuJoATnN{rywfl5`ttLIIcS)}AwUS!Isd&$s+USTLqRRpexuE@Vs;+S!&B zxvJKyeDZg8%2w)$eBUmZFZx9qe4;z`TFg0aDq!t0xKDFw%u*Bu=l3>mJ=Aq*K1Ea#2#FGX@%zFnC&{#t$`P4Ij*76P|^4h>;% zklE?6BtBrji_zV3&eifS!lm&rUQ=(0rSyrtw)Xib>kE?b++13N!pxFyvpHBLW)>5R zO_ANTiFa!y@YMY9cZa|HTzMAYNDd$dQK=+?FANG@iZfQ(}fV3iNBP|ze9e+nb?7sshtjj2C?qqG0TXm9i z(77ReMC>R5)&)sYJ(R7DMqge02kw4+Caw(0MA#Fl*rPG!ZHiymaBPH6-py-{Vg0p7wQYuC3(?;9dV! zZo~MXbeQ3Jfvo5X-I4|mf$Nt&-;g=QAt6qAaI)RaR_Np~eckY_qTaayE6p$Qo~)Yb zz1Mg{TlF`Z3umNC{#wk0k=<>pVnwvBevSdJ9*s+F%a8ue>+~A~vjD_F?My#+-O8&S z`zQNbVk?R`$O%!5BfH+BWg$``_lpl0p=@epAF;uz79?umg@QYXnGn8=4qhwK?2+l| z6o2P|l!;OejIhUoty@yhw)7>7&HiEfb;B?Tu;MTSMkrs{1!)Mp^=oV4-3TMIWP)MD z6e~)QPG`?*{Y<&zX9~gM=_Ceg&Spob)1V{d50(kVh0lQM>uZKRAN- zb7yLT_gM5D)jX&AGOt~}`G2+L*C^k$*ptN;^DR=uR`Q7A^)x%vIhN)!a^}w9@q}Nw zT(^NRTwsMk&uGhV%yKBCUFiQUr6B%Np4BMedt}g>4EP%;D&~qGU z_ztx~^Aq7tzMRtNx03O1PkSmF>wSnNFp#>tZq$&`YaO=orrbs6xdi}ZAcWFM9>Xj3PiCaFfNhvTS2B~4Z%;3-vlofW|vw*NF^8Mor6fm`n zkE#A5*+4Eqxle&Gyh!4|0!Vep4&2CiAA!h=Dssgr-SF#v_N4wmCs%2d@#@UtwTe1a zF>t}5xXLOwBqmNWJjN8>BU;P5I}a@&9K2e!OQB&BVI+d@X_xFn9gX{@XvxIJtIy9N zNw@65#vKPaAwIM{Z=nyefVP65iWjN)tSHvB+Q@ioiTeN0jhRWpLA~e1`;;YA{z;;O zk!zf%kav)QecXO}&Rh>%MA#%*dQ1Oy(0a*WQ%It%Me&>jYmuyK0C{8`R)=KRw8g%U zX)lkypA(7jU13zW<~=2Z*~-~6RI|&=fOa9WN*`G})AZ)7eKg3l&oSMN|66vkq%gxB zJqlTEerigZ4!8JG&tMmteOwMBJg#EzwwOx%8p(Y^yd?Chbj%xaa&?-+RPdoOHK}@4 zA>>EeQ?cSPRE=hgY3L9v;NyiYYuo3+UfOymw9}^xEt?Ty4~|%Z>%H|f+4y_+g}m$^ z51qqQ_@i#&dlc4mI7A_cLXmiCpMM@TlNIC7LwI=H7K+Xuu^15>1J(hSB#zMii)Jgm zNEK*eAVm)V{(4nyZBVdeC1X+X^zJGA1td+7HoyZ0JNn|{a}Np^zo_BJ9J^IyC)r1R zjOkGrXdcyn;EZ(@MtH}KhD?ZwJo^5+W>T-i=cVxoOA}^#veQZ2-#^rn_22t_+@zMN zxAReYLjFyUV(Tj#;^%z>TCR3!;3A?Ha~G}`n_NJL;g4$>08Be+J2g(#rIU8hw%(Nk zIZGZ5L74b&TP}DV3d5FBrUJ4rv~wbytku%08sc!`X3RO2;`+ZlZyVt()h-2PVn9GZ z)vIPOb#Q!FLL*s6kCKfG9OZPS{$9a z*+`{L?!EO3dcku(1ZyY)ZzCZ=L}H*N5f&mu1uI&~}c9@kiUd}Za> z?Mks__m@w3*z-?aSgK*V{Jhe2SaC6so8P^Q#|_LQCliivbKv7{@^OSzYc@=zyiJV~ za??Sij>pRi0m?YCy|a^F^T_wUh9S%0G8+A81VGpO_wN^ubr%M8Qg{_)pGe0H4>z5! zlSb(8{P{C5IH(+JzYCE-W@8+WI1rfdw6J|Vwwx~hRAs>NNgLgBbK?mdXcr9xxNwXw zhdzIX%&*&cb_tqj;O{@=k8}jojLlqqSHuim)4MEzHXTmQs8)^P(%`SRrk|M|FkURZ zrsMm&yDR8+NJMKkV^1Br05yJ z#SQ{J8AM+0yrG)iOh=i1_8o^AMJDK%tizAwgiNyC9m)CY?0*}ty}F21@gGq{4i62* zuteEKkn7WpvWhSnxXC_$wO5{$423NhXs;3z$-~;-S35dwuWkN(G*v2ia8E>4iMRU> z41bguA@o5^W`j|5pWpr_E0SLdb4?%d1e7&HCH0bzau8n*wwan5xKVsa zL_>2L&(~A)RYINxvTttwGDtC{kL8)Wd^c(m2S;z7ymqQ=!A5kqJMuD09V=NcGz!sD zvkfkak8}STc5v$YH;ZW9vBQ4nTA@&wgujY>#jtQr>2;H;Z}O2TH0Hl}?Kz{LTcrj1 z4+?Q`aTkRv3Ljo$#Y8!@)$fbCJ{z-2-VfORso8}k9~7YCd|{q4Jj|ob z3PHZG69PXZ1XR^w&X5Bou2;a7;@l;gE+Pq^`J*mKFr{-~yH zxL3Blx3{;sxe0PDQ&>s-t=o_r7gGy!}44|2bPylCoUThh#5r=iz z;p$Gpg?VV~Ba}71l~my7 zwC;PhhrnYh%I8dWPHbFX=JM3=BOLmcHR#*aykd1zfG`SXl5ig*Z3^eRq*@vEvqd}8M zmb0PDAq&fK5qr7zM*e(*MO3(X#Rtz-8Rl2u6aZl+>cSo}&x;p51IhBrhn;>5`qKlKRE9%alU^k^swUPj|us6kw&y>wwrk5$@Cns&; zv04ytwbP%5N*s<>j$}EAtEiKeEP{);NiA}mi%cv$7xeOIX>BbY?b%_VNcQ=;TbzI3 zzV~_y`A(;o_-sx%DvI&Pdx$=e1!ia0N_-pYs_^^m6GG}`4Q#(0#x)1P1vFnB^mEzH zH@Y1b*wpxRu0pv5m-jve|E3e~CL>?G&+=z)e%IGD(U7A|V&25*e-ikIMvGQagC6M+ zT)NO?z*+f5^ECxkx6TN5sI)&6!txK_HR(n68MI&JGsFMg`yUN;DTsn1G7*D4) zBYgF28RjAK<9KKcSrZhI`b^b_Og4J;;o(@T#~u4j@x5jl9Bzvp`Q*Xhm8V^y51XF{ zQBn#chb>s<@zG)WVE-f!;|PJDYe@t3O~c&Ht!_{zA3D5_CAP*7vkqrWp<_GFy9+J< zfQSRy?MQ%oN#z&fWPqy?mfUNsUhv705q$ts!Sv%i&NekN=((}T>LL4s+l7*H34S*ly04wcKite_A7a`$upAx_5 zi*R&q^{dPh)6VgRk4A>Q(BieAXq)(i2DPzTwO%G4Sg2xKvlRZiSBlnDWEaQY9MF_O zZRf$KY&x&RSPel<5Y=RIqN)GkUJ2J@ZiVm8m;RlvSH6iYrUi*m9}&*a%z$kKnn>^& z>Xido1Z0IB9isB%GPlF*3lRS#wp^{jQlCbaV5$fN&>&uDpPfxBuz;85xyMbO%}{>g z(tr%z-HJA2ftMBz?%W$^PYORgv8x4>DhP)lgD1Mzq!VT78Jj7FV8tPiRLKO5Oi&6@ zmXoW-b{9@jqzK5Ahb5`cMiTHkRcih&g@+rxY|Drti|d z2l5JN0oXDjzQW_>!j7{(!~()1qSpk(FOTDgOe%Mrt=_-KU?BTBKpHNg_?T-wN9dEL zR%k931Rvy9Jv}6{*t(vm>z`8Mjm&%*ol0vY{r`x7-=7L$P%iAkf0FoOT0XIrCloa( zSKObMdk2=#bvoa}iW$)>b;OM$j>00S7(Bncmn4hCoj`dkrnGz~9cBF|?D3hVr<$DN zWyHs;p>*gq>5=7%8VhH$xGl4Ys zK+VWI0m3lgdz6)z8}|lJLUt=qC~irRH`fs3WAEwNJh^>G6GSItV>O9-7s%loWRAvx z$4Tr&oC0r+c&qQI{s^?;b0z&mFG;^7Pa3-_T@ntVU*Eo+9gQn4)W7TB_owJF(-=r`_2IPA}N=mEd;Q zh0*4V{&)jXBS9TSKi7lC!ugd)f8ZRi%8mQ~v+!4yPf3>T z=^D7UUK+3kdoj&d&g?)=SM9tzRH)4A1mX}y&0_qgME}~7nGlxW{{iz`leHPiU^(15 zAorIuY21xRUXky{-25(m=b-ymcA!L+oSyr8`ra=|W*gEzLW@A#0~z)!9UUhRbP5Lh z6-Q@bD_(Y8XWn#}ti&OOiP3;L4EBoLl6$6-e3$Xqo&1;A_GzSXMNdZgzW-WUqP9fl zT^Q+Sah9pAtCRlY-{^GK7d{q#fwT7!?;&Gol?AtC+cPS|-h6V?LKMzSx<&#|`j5emDw)ytN5d?Qa&pVt$bU=LPQtY_{RK)5O{#W75r65w;em!tpMT$k8HK zWMAhCGeerh7y9d<#4AWM#I-+{mvI*?hVb8;no20gLBsz^jW<1_xEN|w5Cy;;*E@%hk!ICg zq}Z*d6GmlOx2TNuvGgAp{iT-SU*3=BCOR0DRYIQ+>wW?EBS;g;E~BVq2g_W_$<#~< zA-bfj?4&|(pc_+H2_uadjWc6A7;Q*vKDLW3Ep&#G_GAYhK;SR9ETWTe?)N}(ohMWPgMBQQBpyV@EPX;m_{IIR^H32rL?47J(ams# z4j_0KXnyxLwQbX8#2>L&Q;q|pMz^T|4HKm!f@@#!kF2$!{19m#VV}(V4^h(`ZKI46 zunw)iJEMs?s`<_@d+AEZo;{FomT?%hk9bvQ1d0;a$wQ`!*m~eSo(ty3RD3;cIb++$ zzmFv#tPa|=RlR%_C?S&NOy9m8oar`}V7V#4#vpje~vz+;_R?B_%3m&L;v`Zx->SZ6sI?(t^u9yvf z!g6GM_y|$eGX6cuqJ1UjUkB6;o$+ex7Y)_b)u0XOUxW9DwNkDR8K1(RKlY4B4*&Ut zh|mLo%#oX$3q~dL0dr1ws62eu+R68OpUSB6_U(&Yd5Hguxx;~jD%6qF-KTChEgct8 ziQ#PrTX1~z5bkasn5n|;3Laoya?XHk=86Z%-rdtM!N2w_92eG#h0TVH3k^G#!anOl zAaCs_>xGP76P%atmsV+1c!%7maDuyxlI4Iu3kJB=Tg2C!a&eJmymPUQWb)N7xP_(y z@c^myqD*Iirvg|kHQKwLrwu{%I9!M*B-Av^U*N`@n3Xa0y~9C_`F}K>Wmr~e+lA?p z?(UWbX({P=XhcF%LXidmX^`%amXcITP$ZO25u_vpq$C8S8|1s3?|A3u9P?u~&)#>e zYprvo9B(f!B9+u$S#-W2Ngjhm1s-dWHpYZ01Hse#RpKWMrO)=5HtrYdK#%2@FHpGS z*HBXCm<_wlc!S?3{6xAB+BSL!_0nf!WpSOqSy9oS(w+&$jZF4RbSZU6@KjqFL2d7- zg9!YPLRnBQB_;-3~C%EjSFv2^#F{7#?qI zlpFKFh{Lvo{B&rG4o6(2v?W6=XlH?!}bz~pAz=Wp(a{d$zK z-rnmU6Yp-q^XIX4JoW01!$_XrQc=x%YZc%$KovNMu&kJy*yea;>;K$Z6-_~8(=UKA?0fTk6ruiT5 zBxv{=zxUB|Pe{Y}M|sJN<8?2|kdw4Cn|d0wTbnvcaWl|(pXu)#aG<*naV?DD|9fZk zDnOQ0D69olnL8bnrz#^;V`Ii#k`hqK~n` zq(zGSwkde!AZ-If z9z8%xM-R%y6h%GWK$B1S`yzP|Z?$>exlZTp_evEN6##>Q`3j0KAHVmPk(H9VO_ zCobk{{^B{ayOeCQj`JnqY#|cLM>=|~CNuiSs}xF6V;xG7ewtM-V%6H#CAC>Tf^+1lX_T9UK z&!?Y8s)9~4`}-)1H`G;<;1vgt(sdws1HoX@2766xkKZTwz}wfS-PFZU7SHRLr;!ss z5VUGKGQ1@TXBJeC~GM~Mr@Ct#pNE7mHR*}OO5ozRxw^{M<4 zx9x6dD0o{0)Zs7hO`gJ`4&K!hZu%ldQuL?KmFmyig-boWf z&Rm?fcNUB6U&86%>|kqZv?_XL!J0NIba8KNsY#OsL)#4Tezc<`8JS8q!5YW<=6l+LK^Xq7fp;hDIK_@ z3LbF3B6%qJ?%^12mcqPa{|jeNSJ7pP->f99IW35H4)Lz;mlFI%uEVk@k&=;FWapMXnxt5YW#>{(H-qvCvCR zNTWJdY`))x$u=m-j9cvr<067KR%eYotC5OxlfVUS4h2G`Bl0M>5CePp&zr&sN7WhW5vsy zQYtJokUnL={HfpuWf!l<9SsfVjmQ#AyS*Yl(%cDP$5UcpZ3k?>@d_M+;p1^{ii^_0 z=I&07{jOoMkE`@Y<;Me)!1I}W)51(7r(R=mT8w6kgj%^GPmsacf04Nk6GeuWYYec0 zV+*EUf_TB#03uj@m9ke0Ja@(zO1`^8R9ymKJ%!xHv{c266R~7W0@w{YxCTTzoD1Tn zv(x=FS1i=J5Y)r*mil!1wV}!`X`nrH34j;Y(;W6MxC-kUrVCbYvGe-4rBznK8wkFC zn3yfJ`XDYVL-A1E;{=JuXpsqw^wSjOAIkHP-z&i-7Lbsd7`0DU#Y49|d2-ZNpWlTY ztmLd>MUlZGj{Z>csOX+DG7||KWt296-a$bZxSdC|7ePs`Bi2siyK9My1f^_HKNs0$ zCGxSrs=QMDC8{9iY09}GhgN9^7rl@*o~DZaWTw9}il951pjNMDwIh>ZfRrN_Extut z;NAR@t>#y%tU4T>BEF5qG|h9X-|ME|h`pK-8?zWK=d{W>%VH$_z|-H+AJK!?`hbT0@Vb)KO1(tRnZL=F`jtA?7z-EliVIwKnMu zzi2zQkD};?E4d#tQvN$l01YbI&i)+$YmbWiZLRyFYYK($iDjv+QzbO`2C;?aI%XSe zeIeR|BNZ%<8D9XlCJvBF*dQI1nLW&+-jgttTmw8|Hu=Hy*AHCO!b|TuODZa7og4Wj z-+WnLhm|Ht36s8;^4`Z92MK%f1!w~X&1+ob$Brmt{sB(Ek=hZ0*`pSXF%QG7NxZ&# z3w~c%7(b=df&n?J2f>wr)Or)W82P7Ez7}GU2vNcw=+d; zlv8{a*6;G2HK- zDYiGg5ZZ)WPGs+BoTF^a$!P^L2R&U=WhF8%A+GZBem4}0zAq_hdg0HVj)f-Dn5-ej zP54b=DalHxOQ1)`X)Uoq&7vuU)^#|3s4)9cl@V(Hl1u#z&{C~6B&3G%E`yZ|CRmE; zI;Pw2sTTZ+q+UQ%mytc4tO-OnfSG4Zc)DhWrse96_a$_;4hdt8FqyKozx}cE!Gt%L zWSj6T(#6m)zQ?-4K)vB1yh||4-Si|p(m4Dk5La=0fNPumWr>}IT0n5VhqpnKywSDe z`MQ16-C0ZjLA7ZC37a@!w)B49n(DVT78WMCw^S5RHxuZ}ap%hopUgXd|8_5;b4Lmn z-K$8|bi0rfg0!0W;&6-#_SpEB<>^YiY>rs=s22NkG@39Y^l94l5?8StPVei_W2-~qYoHfut{JXsZwgqF-RF5)o5{Xn26UA}k3)zAjjVaV%>wii z1>`>bMO67%LFlYJX})VdMP^vHyNm2K#L%Br%9SjUc{}7Ebue#TX9oPGEdLF@W7K~p zc(2TLjVnvJ1z}e)js4R3j{Mq;&tmSZ9Z`08K5|on=8hzwBe}T`M;i4Kh4bVDOXWL* z@@sH%DQCi|p^Oj&P)cH74;%krF=K}uu4>1?$V#u71RPrDZC#%jsSr)Rxt<=a{kl_r zsH8r=n#;9mU}xoK<>tmDo0FfVrcz{euvecf{nTuUUHJL6%#809EnDC(JomMI9a5rI zaIQyrIB>~8$Q@DU=8iq>@Id%)n7Bs6A&c!V)Xk=XbBtf^=acs>z+j*njhc!&95Oa~ zfF1;}BXCqaa7K>bt6maz@0I^ji|!C9`Pjg~$%vf*M9S*!Mh&WK} z_dZn25|wmKZ7g&)Mh9gLyrC=iQ-_u!#&g*38}d?X4ESm680~~0hibixZCtc?B%(6h1FRF|U*MJqdd-vUmzwO!I~;e#b(PdD;;+(1 zGJj+t3AdN6GWeJksTPzz_+~?=a>y3AAlNTj{8g5la61yzh1W>{r4cGQ-R+Tm>=V* zjXwXvx{_b)TB=+$>dD?xTkCmw@^xm$Nt~9LU;gcOpA*?AMkx6-+MsS_gybIQ8o??f zmJ)3E#GSf?u-<^bLmp5gw(>IhW!Xq4k`s$}%e>112BKZ1HmQha3+XFL9n;l)?d*E@ z!%yPa`rg-nBDMPT&_jAj&1xNRpq!)%t!&8>#6sp{J)z%LjFSY^Q|@_7JeB|6l;(&@ z0jIlRyLtn+2tfdVj`D*gzJ6ugojS?ASLnT!WQ?7~-o+?a(aQ|mPNuJY=38DbTZD^s56k}5*7(}5+~Yjl%#-W$eIyNnB;hK%Ovs6vRLvHG zKF|>R6=@1?KtfpthIcZ)=sM94GrEfp9QrY9yd;X6&s32hmWttLLtX3W2rLmYx@^v( zk~r(+4OvpGh01M!gs?>Ams9#~MpZ{^|C8nLK$HLYm?thtG?J1ec_G=C#MHGDjspG| zyMW2+S8ku{s>0Z&ibx(=kNg;>Pi0yvs;NSC`^DVO?G{neJHrmm2a0KL36_Qo?A{%Y z$XpIx!~(s8swku=5?;@>@=G3v#8@A?O8!k;|GSbpl zYhrEZ+h5DtngV3=5HJ*TxYx{g!Gi+vaEA5hTx8fB9=x-{m!edu<1gtmGkb2i4#s83 zVR)TQj%|vN)sia}t-(?PM`NV=z%~Xfw5XH9I@lFu%9$LOJ70*KJOhN@`>ofOw#J|zY*4`ByyPP4y+$OcpnbuajdP`71xzl4pJvA zSj7m|XRH${HPb9(@pmed#Qk<2V&DG>dlES#k{EGW3ioI`rPqet==w%p z6hsGFh$JGQf28& zl%8RC#I76e?Z2bL&$UP2ah#y%g5N?;g94@>+A@N|sou00FU!5v*VjJK_8%G;ZXPf6N=iwT`t7M6^ziGg z7`Wnia%8xe^jg+v{xWAF_~A_rl0mDJP#fx1sBh&=q1w&)f+$BJDrON z?ax)al=t6T%CxzjR!C{f_zIJI@V&9=lhD1om-mqfL-SLO{x@nHO*B!d52NZy$F5LV*4O&EH$jr~@;OpnXKz;wB%^+8&VN;EDECJ`_hxbt zKD$Enga$v7$el}1(tSeL%9*%#8azqt+}{80hXbHfj7oj2e4}tlSX=e>E#vb4 zJtG@WA%8mW43@{y$1$%;+VoT4G2jTuI;9SB?iDp77kxM z1O5@ly_w<|BUsivgfI7=Q(X$Mg^Gs4J@=YmsIB&Bun-z3~Ue0 z+kWO>iL8Tl-bqx~7fr$g&Aof7Nef;!uu~^-U{)&|qx-4&dXdJc9YMHk>B>M8Gc*H5 zB-x3^7PbAclfrMohg#dYFm?TCvq$!++dI zQCZm_kk%YTnp<0?LEFK5B@=Xznr|C-{b%xWSSHhRi9!A&&8?P()UXtMV&4$E;1k1YB+&=5Nz_uXZfB8rw2s&$jt*af3!r*%6)m@KYgDpRbQc|JV69vAcB_+PFd&in zJBE8W{B*L)bj((61bMQhxw_HhSh>Dp>!-3J@$Yh~cx7-uu%|}%tvFAy%_MKXtF7Ie zfym$adcHP$s%EgHt2lS2wFy_drltFKs80T9_tzy!CJ;*CDo(_q7QaF3mO!_w2y}0H z*}yn-xb%<=|Au4;d4|vXO9cSM8MOw)A$uT>oXlU&ar0xT} z&J4p~z89Uk(y!Vh2Uhcnaz(xU>Li_yJ-ta?Cgs#U#*T{NKPT@0x|;vdYioeRMO~a@ zmIj#yL@bnixcn$|$axBMU|xYJ0iEQa(2Z|O?{|~H4W7|En;h#!DE!-%{obvhQ@<*s z%SB9C)v#{N%JS6!8tmLw2Ap+&pyp%*Ze|27Q3T4fBKPj^Y1pkqrQmtIrjbo2dZBkq zp-_jFxUd&JLA(qLWt{EN6p`0G;=;UzQNIN4LV;e*@z#J#QMuRzC=3EoCGx(Dl@)Rw z?~Zp)=}(Ab0-&<(xoCRhuh*gad@)~*h>2uWv`I=0Wj9r#d|Tg{ zf;oH{5Lm9oXWbtr2$$@KrRDR#knPn`nQVMHvR+zqb!>OS*zWeaG+sMRUxdW)x!d_7 zCkbaBVjafFb;fam5!u6N?%pjW4xen}fM5!98EK>E50RfTS;2mI(qia0YxbA`` zOeIF8ANmXev>xk5(ton=Tlz&5A?-MxN>HL2?*2)_TR)3PUZiM+hL$@IF0 zWVh_EjDRSb6k<|^F~01efQ&*PdU-?lbDwTR_U4M=dXzzlnF#HRn<<0GzC@>L5Bqe) zW_((ptEUiNCS0B9^g+4mcDL}(wdvE!Xs(9y`Lbh574clcM&8C7l6!)6iA9{?+A*!?*4*=m5|-W zOgF#vdUf{V*g$g0$EZ`HQHNAqAZhSB6v5Tw`B#rRI5{~T`F-k3WcUfc8wL#vd|Y`?i%(uf+z;sEX8^Tg}vM6kA(1znVV3;t+4p(KL7^r4n?H>fXp zoZUM?A#iX&lj-O5^f!^GR#x&uS%-@rr1a+i7EYzW(>DnhOI{$F)#XO1atE79$arU0 zB-J~JvpR^lV!g^Jb<9IV5{l8{=qq$e{QO*@NwUpDC5V!0+Hs%4K@<}uMAPqNK&sL1 zP&h#YG0rtTM*m#F7C*@9)7z;nQ1LM_F~K(if-Fh?S-0?rG-?t{OHq0Hh6C;!C9gGiC;bN&? zl6Ls#T7*pWsi)<@hR1029c3eO`+`TLN^0FJFr9p0mYb+ZYh6I+mhdgXS{sa>?x} ztgIPfVe`o93rQ^?2P5rO=as(p&xD>L?;X2l=n?^J+^aZeahS$K(~+_ojEMd-xBu8F za>aJa0pOFkku>l1OY^(vC@%Jl>0fN_XdJG*DHdGpf^qlA)zApx?5KrQzgpvyT(?&# zXUM#L0ehtEhec5cDkJJ>y7f#i?1Hr)KYlpy<8N5U&2-=z`Vdy5T*3OYH_#CbCDshM z7#+VUZNT*SW)e`ml-x^2#lN}xr%^G&P?#Cd{axk_R4u!tcXe~?X~UWe#pOkF9`XGx zBtrO97ICU`r#FMC!`-FCMd<=YqN~BhoQj9 zo|#{WrK{&f)gZb#2$BOgn$}-kPL#<^ZhBg-Ff)H~@=YZ<5V_mELLc)iupD(nm;SDw zVy`Vb>y5;Lgxz`YmtR6v_bmmI+T#W-QQ{5W$kVyT3ceg!AMjHX_Jv!a0r#fSy!}xa zNb+(rcJI4w;tJ^`25U4Q5x>?Ek5MkZpF17z+)nE-%#2w45OzACLZ4|4emoeuL)3TP zS1y>SiZVPX_s~@wx=7Uh@k32`SL-l3p#*fu6p8gk1d3Z3Q6>r7xH9HX zqig5vFJISeXm?ju4&@RVB-uj3u7P{ty{6b~>N$GscURFsubONqvpESD5-U*fQ%*1K zTiyjngI7U+efM+M+>#9hoRzAK!d6xn7u)xVuaJVdy4$AP4#YWR{w?2_L3^0TE7(D1 zs^x8jqMA3>`Pec~+w8eu!#($3ApK1l>{ED(E|svAh9@K@hB;>)msJ}5f|@Mi5-dw%kZN z_prQAKfhuAO5I<$`K$!2t&p9DWtbwv(PW=XCvEJUzd1@9xw_`><}-5!t$)UWl_E!E zYvW@LriNVgvT;~WwAfS8?js31`|kD|aeesR(_r!NA);)&U$z4YkO^=t@901dB#p*& zn8EqCRWl}t5Pxe?E_Qn*=FHuPK7et5CgR+CC*KXhM8~HW;)Zz6+$k~g%N@D@bf?0v9%{g9vVRFfz$6hQY+WRdC8>wc1^ z><;-`_$yH~=L&PQl-PFVhGXvNE2@7D*N9avPR+Uxy!y|(momX;fyVfb)z(>$f|mQW zeGUdVVB}lgO))Bkv(nu_u@Cbn9+%e!1BUEW>A^K&2+YAP!r9$-iwrtyIpcdTJ(DCQ?ncv zW?jxiH?e@u>AC>VGqCXmojwZ!#kM&QSb0i$r3xW&1;L?yA&Dr?$L&BMfCVEH=6HQNMn;e2s8n!kGK0;o zuCfxkS%Ui6g|==@p_;*{chS(&$JV4QvV=CnR>&9VE-%gm8)kI_-IAeE zQ@`9ID`;yyF&!-78&QtJ>#-esF|8fA^f8b1<7(>QvaG!PA)R#E>T@J@^i}GcIIR)O>hn zfEPfCYb`eUJ3@Z4WMl^)(WAs(KiWan@9Lr3;Hi7FRoG3iL+SH&BoFc_$$9a-3*;J7 zLrKTPv5%rBnKEdvcIL__`?ViBaYq3BrgxLp8#{QD2@ zIAsxJ{%o^G%iG{eW62|~PLa8ga}dxV5S1lA@Fdyp)!YoHP}op+V`^BGBYLZ*S_OmG z0(}t!>eXBPZN?S%Wv=E<@7BAYf`usEPe6EJnz6@tv<}kXAWnnB%bHuE?WcvYalc*t zm5;Sr{mk6#tPpP4?@C5gM=8Ce+aBvvz-bnRde9@SZesnVq8Szt3;WgP_Hp`F^;oIedg}`>9Py>{S}g% z&BE1<4b?iy<0yQCedLPD3m1#C`p=G6-2~S@qPYv;%|nPa-Dfh`jz_#8h*n&N1HrEU zy41fyCg^TE3U~o-g)L%314^5;`@u&t>Tw;K0D&26Week2_ZQM9tNHl<-XL1rs3uN6 z-mJyNLb~nj;m;=wPijToq&U{4QwIuMlLa-gWb)#FBgcf}@;(RktH)k{?E!nLm64j9 zo?t1jkdlGvQLK&}OIpV&%DtYt=4v1U`OCl)gHJo@!7sNg?>p)akz@g^#PS|O)EqrB zD9Zy>~4Vbg4tn9+3`!TniGVjMv<27!lxFT%H#$F|G?+&Fgu1kaO)Ew#QK7mid z=D zStn`RoTa_9A}>NG1N}Bf>&5YBTTvc=F=cfX(DhM5NUFDfE5IRPgMc%$6H;{?%q7GF}Q z-hc9xd%JR0;94QC8x>*(8bps4)e!1T7ZU#%{`1=H0ph=1?e3N5XzC!h=s;8QVaHGe&nI)eE~Vcanr zu#_Mrfj9X&=2m(GCrQ?@hpwgeL}#u?yPOC$kmCyY1#A-Hy#MpW1rxwpucYhVM7w+p zXGboCp1*5M(e|G|+#e#|3_;)uoZl7!YeAQVr$MU>Hyhey>MCuA>o@j%8PG$PnY4^1 zP}Pmpr7sTPkM#Qc`k|pDAv3=gsWj=?*WZyZdawbAa+zWETiM z==8u+@`PlD)hJ@>*si|9(F6*pNzpkVmJVJ?kd)Yw*l~F?1z1NVIIMF5($y4B&+CJ( z;cdR^x;aXfy)2f!{?c~yX2%9jWbjm|Fc(xS4m=nG~N@$G&% zzKq(nSK;d7q-3AG$+2w;71eBOp4^}JAXTwh!!lJSSJH)FhKKx+WW%bBJi0V0b{Ty8 z%|pzEL|;=195&)D%RGr2Nz$_`D;+3B*4}Ffve4h(iSS^)i1DF<(yvdr`}o$xC>}Tn zs>6_1#Cqz*%L-^bdL5ih*&csk(NZs2UT*;J5#DpF1IS1y`G0#-^XX(`l*QIRr!;!H z=L1`9qz#@tvQEQWuv0ud9;t27@oadAqmdHpXkr|>VwfWoG{qZ> z_rFc=Wte1!b}!Cqje{z@4-Rw3Z095Vej=O~kau(o`X{S?v^AMuT+}Aptf4gFnjv^? z_)lL>@Wyg`bu$ulwl(j3${};|Q@YN7MNH;Ws?HeqX>faEw;`PF-FDIuWj1xw5X>AN zPN0lI@hdY9u!fl!S-f)3=KStyvZ}L*BS!Aw{w*Y}UukgNH8E-SE{U4|_F2-~%tD-R z>8|ITucmI4S*Jf*>qKts#QZI8d)-QU?kP-AS;QGqmUFtJ8iW3yNB0gzXz4eULP*4?lE-@nu$ z?$(#7MQjQZq{*3?((>w z;dlz~nTwQ}kbvXtC_+-Wk-OtCRkexDX#KEs#tr`>BhJToBGs2XMU@p;Ki3cIV#B;+ z(iv653=GNGG$HZd_ESaibCh>n9q4$7vXHy*TRYt2KcJNI$t;atzhzJm+K=Yg@VY&z zcB4a_t2>O{@V&_|qkH1X!dZxjuR5GZFI)mP^RK6FR%fI}0x!h=HvQ+}Dot8PQdaHN z4D^3r=19FU4~SQJ0=Gwx*s?UUODndT@9xD|ex7j}yX`7E;>q1LXjpGG*gTm`;z|R_ z8&gD7!QXe{cDruPJ6sj+&Ln93N#E{<+>}17>9ZE(8b`8j%hz$8aXJBJ;A{&tc@b4p zw;|f@5Sp__h1x}STMK^NQhmhYZl2~Noe_=U<$=wEi7q2-^1m18K$=Q=YnqOT*^!(%fOS)Uh+(-lXt`xul+-NOc`v&MG67&yS->LpI&&oX@_#z0`h- zP!J-n^zKibnrB?h=kzJoDCX@hidtZ5!B@n4M8D+wD2~VmW}jG=Rz_uFxr&~@h*K&m zaVVU?Te2EUH{Jc5ZC=EgO+6+G#T(+?p61#mz)l3}OvhcGX5yaP*I{i}ZC>U#_OgEy zgSc-hEY62&PA>zxqdMcbis+~)H&}B$w8p`c3io(8J`@^e-_QJk)ub53weIicwvWc} z(*SV~d?PZl&lZZsS;S#UjXy(l96wzWuOz(1kU=&UFK>P|GaIsIY?`>eX^O;QE zV0q}B?N}q$XQXCW3t*E+tI`?gWiJd(!yr0JMWXfE02|T-zH-S&y%&n zg)NCX9)eKt#7?46NEN6mNj7}QA9xyck#)1oaD5eYUT<;MYjM-|^Xd(J&@guVu3;uV zta*!KUyF&{fgzELa!2=%+0-#yl-Ihl2OYnpgKNYU-}gq1Wwm2rj-f7hdOs*U()C7Or|rmkjlS4Wze0FTUXdd>h5VOApy&2Gi<%AX%;7U2qcrstz5W=~OWDa!Wn=xx<)B~h>l2)Xjz=kNyo6X_ zXNDjKZB*RLxwWI?&rHj<HMs@0nUbxGv@vtiCP}5vg^1>m!A?Bc?x~SW zY3JoIy{Sd|cQbd)CI;t4~&`35&u1pGO4R62mO2roF$8NMNiwZ{KWt_N;01a7xo zpWpn>5B%fucfOJSGwejkqWBGAT*M&~V^kY=GCnX)zcZ;Sie3Q==Kmf|L=2aO>f_$) zuDin?)HuU8F7f{HGV%$YY!yrn_gXf`h>WOgf+czCRN4-#m<5!s5wXuyCG+?)z7*9A zOXULd0f_pQSJ)O@=G&PshluR(DLAu(n0~P2g`28jSw}u3J%|)U#d!F;gD$a8eocq= zfr5B*ye;YRLkdaf|bPOp3wnugJFS65>nBGWeFeX4_}(&M|J~)Rr+C)Xs4YoAWd=D|L$WJYMfN8)WzG zubo|W3jZyq&ivSPS6CIv?k5&&3JmPvZarHm0sGJuxr_|u`a5}_d`ns7TP!GZ-^dBo9xo--u$Z*j-b^( z+3VD{J(rt<`cnc1P|`!i^!KpF$oY)*n+%z#AITCC&2Am9t*>AI+=Drd84*pXG6tdA zt>^bpK5MZOLvZ3`YSOeV%H?h3Ak70Nb={~0>o!TEAKO@ESjnfX%v1{H^z+KUSGaX0 ztxk@o&bhVGAF4Ai=2kM-g9{V0dX)YwtHEAcRCng$yg zth+nP40{Dyx20%Kk;TZF5`5A=jszn=Zf8BP#9r#ouf1ue`+@QBmUv13jsUBlYx^Q_ z2z-2^Z1pb(f>sg(p~XPv=C173(M8_R>)4+_IfLHW2jtLwX!O9Ac8u*CmE0!OZ=&#z z5(Lb8Uv&h)J*by-s5^vvoR6|>Vu`lN~9yYi?f#McKK;%MKjx>}x z6w^yyr9CDznt!2^^mvztWYUlx0DloqT-f3MbUa0h^7eyIxUh z?YRu9ES5s8E|Gix^nQ$+UMn^Uicrs%mu+uC2LD}B$E&#>A*RsVQFITHhkZqJ3=PxW z=evue7OwYi1zl7y0E_ZiJLs6;Z=3AjoGA;%l|sO07~_e#@-Rt;+@5Rrub@oZ+>_#d zbtvB95B>ZF!BLd8QbyGvwPvq^%9446_E+TJ%4gWR^-x7u+@-|Z?_zi~c(ZVmRWm?+7YIAxx6TfU=jk4f zB=%cQ>Chs2Y;?XqEg$S%{>Oz9BfY(^Q9NB$YZ&+Bs+ca^NT9$^PD-#h{T91A(GS+h z+!fV1F#jeBlzre-F>;LF=^J5DY|Q`Q zLn+V{d)@ar=XGo5ub2$Q2~R*;aGz%z5tiC)+F$ZR%rfma78=UR(SDtZrJq^F$JwMK zEP?b#_wnJpSj+ladOiD{X5RU}SUh06)JuS0USUx51kIH@ z#{D19qs!Xa%F0T8ldn1ITp>dbb{{K4B*n*R9vGQoI=o}QpDzw4)UKENZ{E#lpiv;+ zsDCZRiLAj*u-5BK7JTs%Fd-Lb?qX}3i1)$2zcUXN#OO-+HAK=9!#Wn$~j z#oVT+Mc_HxoYj7FQW?*3A61SBX0=!M-I@KRXk_*w>>spk$J5I}Lt{zXETnyEf_Ih= zA@ZGF`d`3low5W48zT(wWzUIJ=y?Ag5Nm%}2H9!xbBv5xuD`*COC#c_q|Ry;ok#eC zAX)TPTGHhGRX;`p2ZSe46 zqo~zuv`BnjU?~@t6+z5)@XoZao4jHW(-`w^xvzW_%NaBF_SYohNqpsiV_!ne zpw_seYGW}V1S7xK`g)xRdwj7~d5e$iLvK&`6YuglabjQe$;|EiJeFDC*p;VCqNf0f zq-ApT-wzv15Yy4zU3tl-ubT$h_KT9SnyA~aV73ZEWL+Jy-!p3p;dHfh4b5#c8@yg$ z-I0tX-P+uh#)PR~A1TXfR8?>npLKH6_}ok88d7)Suwl<*xJl9Ap~MZm-m+*U=vwD3 zS>?WoRGm_yQ9@m7H)^CO#>HSs#>&oMzQhT8%}L-^ZZGZOk8t1I6#fxT^o&pzm#wtS zCk{_wZ9}8fi?T?gyalC*vxF6=k0vw^@HDr{>=Cl*8Z1A4n6BRq*H7!P@>-bX$hiEl zuRd{1Z!dM%%g)46J5aBGW}o86=a^Geh>#{SXp0g?%J+ar72$?g^b&cexH6jl{yJ~O zH?yxJAu&414_dvGn!vbQEnyd?l~xJ52KZ$j-SO18l76d3*q6olwCwJF$GS8FmYvKC zA(VOLHxELaAHfcC?t!Wj`#T#vbV#s z+9}lr7r5vb;A@d$zdEPbFcK*vFHHD`@PD{+^{pa?rcz=#_f%SzE1X2BL2q`SQk#SX z_Xt1%EQ+est>*uP_j02*z%0wr_j?u%rQYjzWAur>?mRkdV?XwtXwSHLoxnV~DN7dD z6gP+Ok`nc`WmNVBwgU&brWH9p@=3RYJF~aLgLT>PvHjT{iq84GG~z50F?Bh76VZeA zOhak(c{3`T{>EY#vNv*cIRa&QtaFhq3BH{eQd;5}wckmtu`^?CCz%*V|AT#na-nIBHv!z(2a(^lq$-ckr1lt2W(Snzl~(UaAWwte>QRbx!1W~ zbP~mc=bg$S9@08Ja%Nn&%J-b>*!e0^a!2aA*_N>W)-yHMXQq|0k2?|D-NaGOZ}D;R zCM4e*C6gmhpxCYESW;pXS<>#t<*4EiO}((SvO~8*sqJ%WZKu425e6XZhc&2cj@XXb z>?m{mIF0o|Czd1Y%yVo~D9RdL0+x9QR$_RCI1?)>D|dZIp6L~$a$On^RfVgHj(nkB zc_y*?j(u9ffzM?5+}Gv$c_$VU!Zav^`Ah^gW{r->=e0koRHwu{$#KW6wy@+H^YZbqSbMy>!e51$orL;a{#JV8PWEdPC= zAj?C}iF?JbgSz5kVzro#rq_DYXPauVD8O%Axa65=mL`~0OsdvARDGI0G>QSkv`&c` z`2<&b;CiJE2iZ0z&7La*GOZt9bm-R1n!_1YIBJ67|>Jvufj|IGMlUo7NTQ5R>y zyJ!U;?mlZV zAN9okRb%LDYp`#CflF~qqr!?3JYW`A%M@Pb;6nT8wi5B|poVUKpTd_k z4(@osXkLjI>+X!^U>>lAJI>hIj)Tgrr~$Nv?G={RBu4+F;I~T)pYzd zFj(T`xwrGsntr)2MS`?M%J3`OJIO|dWkfPpLPOlSfMtqKBx&-bu5C=F0zy<%ZH6fK ztVvdmh3)jJ;D26#PVC=_9$=Gx_G!x;+AbU&9j&Z7p6@X4>$*va-!8!KF~h)jL{lr# zOQzqqm#D$RJkBD+7}w$b`eFf^mKPF49bBZ; z>cJcz{)QBMsW*ts7|(Ad@2p@a_O;-rXHEtJVLojShPcLziDvgeqE#+t$_rtiM9b#m zkIWUUic>BQba*716L7Kl{HtAsm0mUNAU zwB+Y*Y`&v(8bbu58l^z8om>Z(-oB=_mvcJCr=7Ypr%2zMO8wTyc2plNoBg2};-ANV zJWR6JSTYZ|W~wg5Z|JQEqc#AV6P|Kaatd)lj~Y-sq_=`}Z6 z#o_n=Il=LLd)J)EjdGuwLIMlyzy`r$jyd6~bVv9IIxwIhNG;jqb%GJo1Pzwh!YFrp95dXQI`o!x_zG$oEC!pf}0TS5a#yXRiSH#QBb`FQ+&<(T5Wm9O;GkE8AIN!CwM36mv|P%MR<3; zqN+%hlOhf4^`8DgK|xWL_UG|n7J3={4(%_BsT%OcQsMht*l3)M+gg=4P|2aiJKR(Cu)4Q$zAvDwS8m_frJ2|bGYYp z??fdh)gR)92S&XeUy#zCSk@-K+~bsCqs6x|Nht}4@woqhPq7$PR`q-*G=3<+Yh-Sd zZu`uBCHL^~_O!~Wlj0v0R%?~eRu`#$w>O1}^L_(psO%xM0qll|zVBOJoEIVYch1tS zyzJixZFzI|VYj1?vwZ{qaxUJRFc7(qREs03C(aX{XZs63p!Pg1dPS8_Bc5{8_DSry zJ8e^SBnd0lAKR)=L#3l}$`_<0?|6bf-1*QIc-Xc%^|QXqCD1jTh4fvZQMm19sz^$2 zirpra)-%JY&gFe)hto;hQma5F~s14knpzSbOVgAFQ zPF7pH5xC|P)0Ly1<1DVdE5UT>n|M%*Y2fD_5O%9YTCgub&*$y`(e%|}O}+o$bazX` zhZfxL#aG18*!u3mwvG7E`5Iv*#ha+G@p;8XdX>#<=fyRh z=IG!f4 z)fQ_;rwIrU_e8W%gqMz6b!;zRSbQqVFv7%}qx^|2qWTLj3G#OEtLj^a$4yqwsxw~6 z;V2OjXIXi))*mrY^$@d?dNBrrFI}vpSN`kMnchce8dGn;)Ydj zqlEKeJZYj1w?eyKPu&cRY+w3;c~3{w6=J$wTK%l)<&Wzfb8T$B`f&wWss;A9zl?TBew)SH+K*+Dk*&_055`R*EQUY+RiALf>rG z@ond^$Ll6sSQE=njlKP1>U&cY&Og7G-J50D$%Q60C7e(!lwa2Vj11(Lwra|#MoWgn_$GevM(i`PWx+y-I3O1)R;cT&A>&$ zy0TbJWYN54baq&CDFLTvyL&H=J-O3r{#c{ySlvsgOEF?4>CWJekrHVV(GTeKgiZ1AtUo!C{H@zr3UA8<2I16**QruwWAKP%nG7vssh2Ufi+E#>Hc?jMGRhWh&Y0E!Ve z094jP=bhF!(#ghi@i*T`zdpC&SdtN%bQa^AGj{EYfKT>l-76US4&7+lW?vPbX?m?t zcBNHQDy*5)sm1c94y$lXx-&7`pRNwz>&1+I8*vZ6s(=L_%wK+n znAe;YzlIG$&2;DUZK=HB0C}i?=KjpkOItbfpfGv6G~_=W3bG)>HXoO+%K6oE6=s7y z_*&}AP`>az_`m=NTW}UezB^(*zW&&U*T-iTIwv0+h+gdp`@g@}N z6{!OPc+`dW`=q2Kl>#9o6nypL+sq@zWGTqyZ(Bwv6*Z*j*)#h*lwfw^g@~JP7S%z9 zuD*b@eP$uj5q2~fV$dqNYF*|(t5BQ0k)K$0!2BI05?<^&P7YP|%eE&I@j1evC%MP`{E)HP}i|=NLdEA+u{;2poBJOZY3yE zEh=TBe@}&bcqnb?H+#yxFKgpl5IlFkh<>~`U*ns8eKum>R;B30Ypggf?b78uw9W2( zG`ABHMdgs@uBnsDC0V<88EuOf$4T|QoCwnGyD3%9mSdzS2_!inj(N`HKOrn-)7F70 z4qo*|gVKrl|K(i&o|~hjH{ha^cC$#hDc?-vktS@{sVw5SYijj#q`D%;e%(7dt~jo< z;WNR^t#rwbeoa#Ow&Wd^PQtxL}zQf;~hCZ#D2AfY#s=tZ9siWs|-9}54{8-mT{t0K( z>^1*XhY~cI&OiV{%;*PTG!_;X566EWj)8+ZJTUMf3o!RSHLY>~SQ_jzBV`^VOpl9; zO-T^;CF%hc+m(*Pi=V3oAdBZANzshyn5HRQV$~3^-mI8IZ21D7X`R=|c{>PaF}JkT z8x=Ik`%&oj`a=1ovI*{&iuV7+Wy{IwM3N+U$4OtCHc>>&&^yG}>`dvVc6I@^y8271 zjOR;X;YrT#E?V_8p}O_T-<|>h zmW%&$o`eRv?|+UbiXW|OxTkH#ggW4s$OiP`AizoGEUnwh)!p6Q(=*wyGk@bpKGus7 zJujj<9EfwIqhxk$rj+HdS}Ykyl6u(<*_xc~ODlx(IC*qp?9bBX>fv#@W!1J-Z*w1z z{tA3P?L^M7SjU$!mS4Ybec>OFs`z)x#K>mz2{{ArJ`_LJ<}}eYVp3d8hzmHn(Gp%; zSaX^YC$c$;0gLrr@m|69mX`fXzvK3Et)X_siz-L!o&+$j>RJqbp`I{Z=i4d-;uq@RFEdtPIIbh{k#jZ=@V z(#7rlv)SRzH54&`E^7&e`*A-oW@lyT%*TC3@1>O1=Jdj4iKO`IjB%coogH{FrP_Ji z%V<=wQ*LA%WS26z#pB^+Bss3k*u=#B3H622BE>I^IKp2@uO6Au@s_v2%N}_l}O-TUo&|R%5=ESHJw41%olks*_uD1ahflzdjF9mTeES$bdIcanR-XXUYf8drU{zUtklqfLLg) z=g${(i~9olM$*!l{7;U0$-LQj4+y#hzUf?fUVsBk(!Y=`flp)MD z`t~{)*n5%4PY212L44*}du zrEyLCvFh#Bs?Xnp>W71WleRn%AI8gaSHmSA9nNcJv;EF3)54iL@|K-IUj}bbHbs0r$2r;$S|Hu{U9_2f`%~;))>iH z^np}1blO#9rY)Q>7zlAKYog?)YmdVrb7`ji;%oGsg86y8Q0u zb5oX{JP3Qdv^=gDG6pOAwIgO5LvTomWq5i)TfsYVzCUjL&C}MExoBG}Z}Y@}0TjxF zO`>ye3PmU!?=4y#l>p{_5wkk>VbP9c^rl4RQ|uNGuQ1&nt?dkldH0k}c2~oPkgO0J zJ=;H)=)JZs(e&BmitqN4pNgor8fE+d5stuFkaHJr=F@(Y0NzE!FluXdFRF&WmIo3W z_3J}6lmplhri;(bJlhTy|5jyE5VD_0>_?#>J)l-&DvA4$lQx%&9)_qWKO0NuSDGVOR2yls&c-+jXL2%low#2#cp9J z#8cCawnsV_M(AA!%Bi@CQg#x{-g2Nc{wF|LV}ANU-t>vFUgTLJ{g~&_NTCk(qKgdU zvNINyt=p4vNHOm=9)UUMt~^C}ZL-2So+UmvU4f@2Lz$=)v%2BWgel$AuCEF3a-!d? z-61gU$`AQ92zr>@)8H37(iNZ3Uiikopn>LGR|8QBF`95-R(mk4^~7Z%;R~-XlT28H<*gqxF)^*Lu0G=+ zANPJj+G#9(Zx|RV?6fJk{>Y`3f|vF1^764t1r=41>ys#=SFIX@^~WAI!_wi~-s9eL z`V0&F!Spo>|s(ZcNX47I)BUr=4<^f7+j0|xuQ ztb!PonSF{fL8~FNVQ;5_Z;?^MrQvCD2~?wvNkd=zJL*d#lxksA=8BycgtNqhdcMlP zDO&O$$vnQ%O~*d|Qy)1<2DcWn(qNp}5V*)Bqa#@82~6IHTUc~a6pat7eta!VAopYc zq}ls449YSaj@l6JyhP#--ku9Y zcNnp)L|7d4Kh0J3b3ku86K7OrUS#(Z&rqH6y`L z+gbeIR8tueS_9*^bk2Pe1P>ztBI~D7M#!r9X=I{ZemSiaertZD9nOi{<0n^*A zkSb@nX^u|LV;BI{*(eEE8jhmsT}Zo6@2nvs`BG+eHIG@Tn0^ElN#!|8S2p#Ey4E*oAmOlD^?B`y1_-FNQX>e-Roogu6RNsc-aeR_+V#C2 zQ3Al@b zSH_Din(IcrgF+srkO8wwGtUsCH{y7BEK2lyj3Q^{2}7^Hx;B^)-l;lM4+vR$<<>By zZjw$&#wvyF6?Ud=$Fxuk>#0$Zf2Wj8`#U$0=-gVR7Vr!E0Tj{okrBdK)e^mX@Lq*1nMokD)Ji4~7wj zk;cN5)&=Mn_rt11up8BB4|gFGyn}r>V;DmmVgE6E-FUtHlsiRwoWL3_G2#G-w*9@m zcUyn?%0x64$Q-9PiNu``hj7*Plqv>A@BbIoA`z=vpwqV6Sj9!&8S`|LsVIPD6j%>&k= zUx$|SfKuv@BVIkGDJXoel|KZwtCMvDp#V*_y!1{hufU;_ju()X6(6}Ynf_Z(34Ln% z4_<;@jQWBauAJ*Z_eGR1?>rz#7zvvB5f7@2USI#Frk-Dy*68O>EuJ-!^h8CCnIofc zq|8U3{dPQ>am?`air3BgXF}>^orrfL^kq`D5+y~x`3KICv58d9ZU)o?ZUs{(G5`YI z?!y{!!h2vIeVrjb>Gl~quWLOaW`^)4O7yG;a;(`yF9+%HqaSI-Gj#S0ot8|Te2-1I zBizE*Fy6+}2nKJ|SZo4wB?K$DF1kLjJgusTk7h5|qG)_XTc~95#`NjkPJn?;yBYET zCES`t>GHxWb}I~q)3cR0KE(>dn;*}dzTR|WWF(mJvfVn#H!>gR_8RoBY}ezc<5ogz ze2XK?k>-iv0H_7hQku?6hNyFI{=t%yT6xnw=F zXiv8rQm(fqtMsPyO?01NI#vYF`%Mfawy@QUQfP4vxp69#%zM$*Jnc?!pMoLe6i?ZB zTaze^1Q77eeBp3M2gIl;4?9;~S`Vt9L)sXMdIdY{YMqqTcdnA z#&j9ug*w{Jqy3u<8OBMyq!oqNZ<}M(E?u-t=G>~Kq0Ws6L9nzCgwKl_8tm(y%Lrcn z;Tkx}G9!PR9VuuhChA(f;9-8XyNg9?;JYnI+kCP`xwgr5X6LqGrsX~?c0j`0b9HY~ zZ4V!}&RCo%;>8c{?%z7SylmjDqls>814q8+pNMV0_1Ue zzi)5bI36-cff(p}ut>1ikW#E(kd~LHRx|^%>Ym3(vd~Plqp`j7Q{T$Jcd3fTX+BG8l$gk98O+08A|zV#F}t4^Y-by!*JKPR^2U60$sc7 z1YhdZxb7&jI#p>&^3HtNZ{^RaM?a79HyALdOHO!8KhXT+pHu#E(BE66LD9P@U;nDZ z)~XCPVS+7yljhw2v*_?1DGl#}Mr}4nMk=bm1j^B_YH`eHm6+$r z5)%nn$OyR8P_clM?#fRV{D}DcKd`4({nrQdR4A%g56t+99%U*2<&eYZ_SmZux|RjT z8esU16fAmG8~Kiq{M`E%-LR%9`pokDzhp*v;$a?>ZHk=>-2wX^W3#Szc`gl!=V1)% z&k3-tx9p&1$@6rB^ah+%y`iT#FthxfIZNT&61AfkfYGXN+x+~y|2*i=*qgvm>vi~E zR-K4fW|2=;i0lBkaQVB98Q_O~?ktr(R^sE+v~{{VQ}TScaT|u3_zSFG+`6|ifPdmr z=9rxzCCHLhDIgti>#$H2*^C4xO+ed7g0`j-fp(b(D`u)bt3&l)&zaa8v(pO#?)Ij= z|9==YUr-DmnZxqAn9d^*yL=t+r2I{CqODyzKFcyuz6T|*_R>pc89emcZRbyi;5O!eL`IM%fX%p=;J@TAqKbz8y3Mx^6w3cnX(5Iro-}#Tu4=B| z;N@GY+fz5gM?Y;_e>hG1a{cKTyOT)`+3aFI48*9|-7+6S1PREfyrk60Amr;;Ir2er zjB(hZ-JxW7lx$gOCu7cH=tvz!IRDQYQVazcK< zibqEhfR06Du|;DdqfkhJU~hYh=2QASv6z|^CT3YF4z(_^MWLKs`rbivjZ*bfS=uk9 z07pucg!K?(vbzc$ND%F;>AkY3n3I|KFCacUdu7Gq{wnCNC4?OOO*(eI!e&R2Y{B33 zT)zTgzR}bI0FF?~I5SqBhB!1WE4~M0wTKM>PVDbP(9oEJWdDIt&=tl1B;{lj<^0yK zgwTO8YsT2FtAv^6Yd+rj*Z@t>6Ag^_^oQ(uIO1H#C7*Nf622AcV6vwTWercGL1zs; zdPdtdc2QQ;3rbOJO7e$f%;z9@aVvX+A8^-ogspnkfS$vAfqiU%520a1H9)GX5_mF9 zg|Hw?DXh=jPWc7C(zoJPunT+PQjdbJg(x?b#Sfn0>EMji>G0?N2Lw4a7iTz~cirUQq6XzUB zxb5!wTs_|eSTHOzyN-)`dRw0x6%@{(sy<6l*=q&na{}ZOw8Z@CWP(ftDY08$-Bu1S zo%zwCmA-=00(i8~KP(Cw7?cRc2wSp$&=;jA8p}7Jb}e$4v%>7VPHX$Ghn(!r4GzEg zxm1@XU@7N!@#Fq#U4ZO3RAXGk98vjxqQ3iHa7$F7VpQm5zSGtNWKl`C87SRucG4zM zxd_P>{S^~@6Vl1p*(5jK41aVz78hgLC#Z=~jm8eSC-uhH9IWMu8_h%mYvkiLD~3PQ zH(!Rcs%^CBJ-vi+Ei5gAK%6C0(4CO;I~Wb97VCU;s|a$&>Jl&t`Dg{tz>CYtqn!uf zv6*_nBAFlW7eyJ`JI^{JYBBD8US38HkjG%>z0HlO>2rX+9Z=!(M>*Hjm=HVO0t2?P zx@r#A^JZorRQ_|ODYP#x9lI;Vk?!+E7H)JW-;Qld4yB@m)Hn_$LpjfDP?oI87e5!) z)%Y?mRy@UiW$7&ES5~f9nQJG-YvjCk3P9Bu))uSziiN}fWyk|UIPc}-y~}x6dI+pU8;}Po)BihsHo}PT#->Qpz7+71&D`t+rUY)aXiIZ8I!O(`T>Q{ z8C(bltr7JCzAsm6Z~twb_s-$}R*uzZ9yA9i<-Kzv8#aAkUOe>dLc(xDa_3$pC~>@C zFj8-lTr>v%V+ZV#(<{24{) z!QwtQT>!5p{6Wt#mlUEJ=5IVvNvx@2J@{ROC`5!-;r0Yn|!>_a1*WC@pIYHMXCugb$k9 zJ;}Yqf@`HCvmxzSrwdp2HbNQ1uT9Z+Ijq>T!uz~yh=)w-46?el(j#T8XR^#vtz%TN z6yC+NA{$H)(41r_9mk$zJd;n)cEg;AYw%u+6BKNy)q#yyM zUxg$7kaB9d2bD*YyDYY7#fCNlfjr!pJKl|w)yn4vKom;fYPlwd5k2c2n_-Ooz z?;VdLH1XXB>_1SLLJ!X(#UDR(3}}M$T(EDJ_+zCtcHRvML$B}qABBwh?>+GzA~baV zi=+0FFC|SJHxc0DZH(J+ZPqL&?@RTK?iq&+8_s^=$xNmg^tGTu+=#}$3`^7JFC@?2 zNbQEP<}gFZjEdX!Wb`ncsAw%tUU?Wq;(3I^rxK z=oTU+$@fVy=8m&d=~ex|PcmHmL?~i92YVi~lM6u`vX(r{Qao2Ormp4VBdN25M-7gg z)jp8E-!j5I&D=+_Qbp<6%iS5=36jp8YtPRa6!*&vak-ZYO?-USS@rJ;lKu+jP@cVH zX9N3Pl+epWRyk@?rS|yG<}Di(s}aWD9SUx_M7gXJ=ey^U+YIRwu;^h&lHBu53y!Pa z4_nRpKgN4|zH!nL{wmwsi<|s@+;Q~~tR-qPCL=dDz zyn3pGVNfzL@n~GwKf@Ss626KN6Bif$S3+ohps+agA*-fhP|tZ5Th-`FA3yq;b)?wn zLNp;kd93vO`8}4L-UC9IDAg7w`0@a2bTdqqtGPaQSupBcxHoiP*aV`Xp$<9Rl<3dz z*raS^o}_zBPsA`_{IPm3xUIy*$eFgy#L8aR3mIl_#G=KCSFGqs#@z!?iPA2klZ$|* zw*0ti2t}In*{lxR=qrN;W(s7{+wZ^1sHigfL4ia-Y^Avh4-f?a zBhRVeDLtq7NMU?Y_U^parecuiC1geo;~M_jM`}T5MWq+{!!LiR2sq z&H&=k+hA~}29ZOIj*)N6!4dL~8D{#h-wIzz9n!UnF8Vx1NpPfNRG#cNQjwU)Wd`t$ zy6?&&)fJUqP7NlV$M%0L?4)1_-tkbWugQ!)n!)y<&2wxp@l@7)TU0|FoA&ls_A6K| z!_Th~`v#A0 zg8zeLKu_|1@!K$25(}yvW>ApBYA?xK!&WAx#RvDQ!s@$9T;}uQges2?i5fHb^2fe} z{@k!~?*W!O*?UpS=4y0zj00P>R$+cVKB_+lqvT`3^6jNF&xH$=FuCv9{QdiYW-qgUy}HMCr%Xy`=$VcVUqr9ihEb`dPEJL^%i-Y{p^rSuP?OX^BXz=5 z#^choO$VbPTb?`g((}ecDw$C~6}Am5-V76t zMDtjWDD=34Rh_kED(*dx#?z{tQo*r#iRVu4kkk%-jK0 zv8A!5!bOc${IOerSZEPs`_>~lRXpjVx}&l$e;w+LG2ZzGPi28>3K;7>rBe1u6f_D_A%r^h=LTLC$bG@iebtHqI|v@c-w z8X+K1nG-3EDZ2G~SKH-r0%g%~5v8p|oowS>GVZUoq zX&G^C;VzCyEY1R%-GV=y>eCLQAPFv!Z5#7EQvMG0lb@d-kjlEc0+Qce5h+z9{$aKW z>T*4-^OqdKpv+87j!(hecbP;H*R=!Y@(TYHnYA%JZUpMgdHaw}$bI%6cmYbGm9=^w zqs?wwj%^d!N=izUU(o&_w8)%B)EQ!C^K9~Gz^ei^IL_6JPm*Imb_#7w@~*6`ET!Uw zpX^D_;_+NC)fK_KY)Z}Jz2pTfD3QqABdmShVPO0UqzLuY1zQX3s|1LzKZA`6%aJkh zaFpLKr-{DBS~8G6=bAx#P#l^w8uxK&dU||!v9A}%7uY(OFRGM5F=Z6-pXSkQbrn;W zPVn7(4X0@M8vGqi_?-jt4(!*gKBch@-P}k*a}{xTQ2BmiTrJC{pgnS-;4t8<-={}g zFYwpgo3H=Tk3_#SSk!uTAb3tAY?a!Y8tA&)0zdb5ZCHrg_inp}4e58l?j`(SqB=AY z{#C!;{QdoANKYqE^qXF0m$1&s#j9<@uTLwPj87fKP?G@rN4jLbj~#DiYzKaJ&^UdwByf@lw=_@)8KuSqyoi%`TM*h z0>z#$o}v#bmGqxogI^KKt0S_hDDkeH5cUO{xD7Cn-H(;;b+LwqF{cfOvvr{_ep7_o z5{{~R6j>)Qhm(MXhwkv8{eX!IkJdM!qC?HGrZsp;#p

!$})d^G+&5a4z> zmGr*}x|O!+Ux|Nv0M;cQM6y^}U^aul(mXO8y{qMN|-_&x7J#OOiK z21zUp)^EjHCm%KSL<|BJsx5&5H~N1)J6OKC(Kjt01%7#+s#M21+O8TPorG1R@M-u< zpN4cHH8%Ug^apvi{Y{C74dNi9yl16on()%^Q5E#5@chpu-*<+y4TA4RM^&ZG7mDsR z!dwCzeBIm>Mz4Og9pkZpG`rEqO*OvS-viU>&bp1Yy)qZiO8F=Ho=84aw+v+&J*a?uQI^a-y5Ah_jtLEE^Tkh^a|?ve_4Hd4FIC7%YzfBGZei~;KKNSP&-~o9x2AM$hpgGchmMkW3s!^f4+5 zBv$&25gQ;ovK&cKP8BT$l@vZl`C0goP)rP$9ij`bOcT}gad+1(mZhp`Z5liXr>kmy z{K)XAl6CICvn1WK9acEqA5pO`$qBOTzR+@hNb7Dgw8WHDgt{m&*eK>T22GL zsM+%J{(9t@lYH6pcrqh+*>keHUd}|)a7xxrt$0rOb{X%3^wa8oEtx@!} z_Wu;XW_VJZzR~45eBV`yB0I&0ikK>c+`IavD~)L8nc~42zL2;4?a#V(kn7RAaRR)b z?v|m(FZ@M6DM7S_$+<2chd4rJH^8w2Sm`la%|QO~^K)S9jk_jKr9Q#o!P&1_7&2fI zx0qDmrCKZNs-MD-(9qYvgl09C^n#PhvkZTH$rUb^+W2TW|B^l3uVCD_EYx+_V zlhZx({C<}9Y)=wxH~#w!P6JyQP22!@#E{8nK3{GbQg9YNK(9pdS~78p-<&n83Peij zWfe8248cY)Hs*caP7j*a1E-G{T%uiOwNYg+s(2tX@q~(idO4<7R4K<|6v&&(dL{>kgCpZdgTz2YB zRJ6<5wY!RqDleU6DsZlkHTY@QPTx!ZO&AI;FKl=6p8yQ7i2wMOjb%uj$0hsj0hFL| z%(xLsESH{2PTcUhp$58&ZqU>G}$sGE(1ncU5p;OXHhs zFFnLYoy$!b?qZ5$k;pSTA&i^S-TU&kH5}kQOl@Nl0RWTUkfiVKbdSM(tqh1 z5cIy@cCz|(NO6;xE}{Prw4ErP*mO>YFLK9ZO7>5~`QuJ}S-rxKPI&T}C>KGYg0P_S z@%~~Gv*}&InX`4L?D%VUTWE8BzF3fBaDt%6p-J_r%3CZN;FV3}E}kX%l?G6WM1I63 z7XqN$$-KJ>IuHDPRK89z=iwO~Uy5E9Q&MI_IX4JoN=$ca8^e}mC}c}Gm%;Ecku6JZ zf?|dr{T2%Y*Ys`eFGRH;M+kGSZ?mTKSW(fZ#R@4$_fP{o(rEvJ8IgbtNzOtwsc%t? ze6btxWcOq1cZ6?*(3c@wC`t`J?p@qu<-#|}$9_A1C|dYy%{eCa&xq*i(-Rr_iN`sq z{?X!IH2}IG{=Z+c6Y%q@RwG&<=p4E26~l(eP|JrSS|`OJs;BCKJCCcic}%)W>KNFv z{2Su`d*}yARM)33`S+iqHuA7vBXW6bQ*?dl=3FTQ1uHbAECzM6^|rI1)Efy?rqmw8 z5qog(rY-%;qNvn;jUO#Lz~P)uS145^zZp;uo2~Z2)4VMxA}B0vWepp6dlF&}XFS|i zml|*jbDe&SH3b|{0f~uy$;HuNK}plYOp#57P;Vr zsw#S*qPmT0SQZo)HsqwjeZm>qzVOyu40`XreU!SClq&3Krzj-j?rNYaljiMFt@~(j zc3+F4xPH^wP~+tzQ8VM^;)hS4?WNJGdX%F><&8hu@r-yx41E3KPLB;M;7l5#ebe1z zOxjVVPqLLA=h z$=#jw0-UoY{kO86t%BYI)p}XU^O5!hU2UyuF@s{Olt22N4z^sjh&$BC&OD~d%zS-Q zji_zhszb4iF=iaGxhGUqZ{Go}k#yELEjfd;O%)L8{r2k^UC3Z=F!uX4zEYltiDu4V$nrXC=SG%A^aPhZFzDRfb;I5NI>PHu!%<5U)U+4`d z@GJ^aGQUAtX;Z%8pX>5QEM&Up_pS&q46wN2J`dwUbl$H1-TV2|xope!H0y9vS%iM9 z(NWIO@#I;O_O9{*{?6}9x>@*mm$%>0UG<}A$Rwl)- zcRkGV@PcO*b-m2_z3pl$l0t!7heF3BG$hP^-UH6|%)i(y-0S^G2SKl(zORc18o!5q z!j>|;WEjZMV#8jr=&%cQ9N(7ic}n$7KGj&-_bg0yeFydxcfLC%DXPwj|H-#F_g8^u zh?Y>Bp+AAn)(t;8pvx~b?DeZ!pcnvwRL6WAi0dZgf5sg?-`97|OLNb9D~-ZE#+*6b z(X6;5G+0|Ms!0^XVAre1f#L#Uh?^}cee>*Dk@WrR`;T<5J-E(Zg1T0>^dUAA$`SqC zeq(7!*+c4c*o>N%#?q1Zmc(xv;x9s@rTbrt)5^~2gTBx1ogV*Oxhv|?DP zx%e7!;Ns!}oPq4yps+ODB9CsdP3VjCCZ+0RbuUG7i`HBr=V&lc#l{(?YrN3U^50=& zofse2($exRDKoU1@ZoPTMa*49u6@&tGe!*AxO;hp+Xq`xuzeJxhiADO)U>^z1;Z;w za59-o{i1x%L{t&ik}MnZ1TODq6<>|lS+j>^@!ve(g@ad_HmSiya7BblRh%}4EnJS` zu}Ds`u!q;{ZoHG%2OkKq>2{M47cGigSPNb(Xc8M6#UWOKo}N#8&+lQ?!_o^xg}(|a z#jR_XC|k@(U^HthEAQUmFw0;nw%K;Vt~GcYiFKH1bTlMs2-JS-^j$E~Jt&SQ6yo=~ zUXu7wQALJ4{w65?Im`^_e`*{@K&fpofrq-C2O{6B2~ig@eUPP~tMuJVAM$qB@#LQq zZ7l}^R-B%_i5zf?C(ge+)dbj?Xgb1Uw4aSmrN5N>m!@>%PZaUJF9gX|!6$^;P=ltC zN*A#KEWHV{|NG@>H2-9qaPJ0V^sC)Nsq=Re9g3KNE#N~KlliCGusA>d7m>Ic(i~vi zAftq*z9zg61~VUr(CK*jdZ`wWrqXL$CSAFB?0%OqJjmDFkG~DCN;rB-M1JyGc@9&7 zB%&KQzEUUy3>$b0Z9AtKet$U0L@uf!aQPgMbSGz+;s%n2*!13e7#GN^j$1Gb+GvnD z%J6A9dZI)o(T+L_S#t!RezE(uRjgwr!$=&d#JP^ECpg1SuS-9mp2&uS*5}jT1FACL zkcl`Ax(8SXT3Uhz9=K;Z59GCPz*(O!m7*ZarHblhdh~1%f%~j)QP#%(=`%m%4_*am z9|MzbaCx0Kc1_Ee%TaS`rDx;112&B`)YZwh8TJM2R6?TCVj!pL_*R*t=!`MUW#{~# z7$EewuzIZArUel%E6#SI z;0*7!0<<2=@+8S}wPJ5NlgfVkXD@-U{pklfnZjVA8DpjY z#cufmmt!|D4uN~J@Twu%{X(-^&2aAhYsXuj*FOr&RLc$rP(^uE==@mJ6G!*Xxe#P6 z-y7QSWG0-NmVwVLK93jeC|T_PaRD9;G`ZDD&7Gz^U}ckk{7_(8u%|T)-IIKm!B1)w zd3IG*@r;+32-B$+=8}34MgqntpoIg7#&A9KERN;??{{{Zu*gT^)EmelL72UjbReRK z(=m2Z%KjfvL0q<(wgKod@&V%)YDlI?Keqk3Pw)&%&Wsb1DgTJS?e> zA;$ULzURShpsr@&f68oO6xI!Hv4_BLx(S3i_{nOJ8qRz#e1yfUPB^&T!Zt=%6)pKE z>)mZQ365v}34M^l;7+2Z)Ln!+y{VB5NhSMKcg4`1*DNPB7#fd(PH$@1PEWl}6Y#YEI9^ zw|*G8W{0zQUt#M}h(Th2F!&=!v2@#l}47!h2Wl zQ{o*jv`-2Plff^IU`474S*Gk+VR0sc#{ux0e_|k=v2V*2z3SWJ8jF&RXvVSaGq;7= z85k?9)cHtsa8^|0wM=7HBgS@bQ;Gyp6DGiie zIuu&=Gp9z6&AENJ_3-C_^J+d9K`#IRXs@fY_EPh$>`e-vxWutHe&4eu9(=-~%Te(_ zaeA?V0QwXVAS^~vo-~?>wC@NLc@JF`-lr)dk*l9(XE-3oKxK5}iz!`t)o}8@Y*7k5 zh#`?Jn7(kha`7oB2;r2VA9`7()m@Sk=;!72kDeqE>*4Ruo6dr>q#o8t5+jA?VUVOO z@c4rHdmUS9FjMTn*1wRltvLB#mwxLv<34Lae*^GP#u5g+;nHxza1vbvTf$b`DbSzy zSV?68&X0zm-FGb3&?RRqo~6@Mp}j3ixK`z*u>iZITwpn0gtQKYSjT6<*G#7 zO-am#%AE}>=^=Vu?O=)67WEkdS}R+3#!e8V2^s-Uo4Tc^D5$O0pI#Kyj1O-nJ&k5H z%`*n)3N_C+!6s3|62O`V>y|Hm=VRgm0s=X50a7#sgUJ{(@xA`Kh^z5!0TeYzLUV~l zwQ4PWoIUU=+W|Vi0&rYNjQh*^m|uaJGT!+2D710LM>h_-Y-k8RrW$~Gss+o;CnYnu zXh9Tj=r1zPZ|Lg-JUzGJj73c4T^{}`H5Od-&JmkV1?R{uiJET-Lt4CzbvByAAVd3v zo?aIvaZn>T`Fqr|px0yzuBk~C7y$^_8)WKv-xJEK4Y+~Rscz}(3DLQ$e|{`6g*HcN zT5;_sQ$$~A4+8(Ew~|*E&`L&qIsWoDux<$;G+@6s`|PC1F@IDrRd3-?OJkV%eUBi0 z+(3N5%)Tc`s>-%)wq{%g2v)%Ti!qW6-j6udXsI67jt( z7PWmlW1W?jV~^rfzhga&Dx4LfOQZ)~`ufOo#2-r&R-jU#ps(uTwU|rRBgiJE*u1yo z%u(cNPF??0&l7KfKU~qb8PB;6kWbgRbcaFsIY37Aae7Jkl=M7h{D}H7YFf{`%}&L8 zKt0X*R$~M!MeZ$;;~*jz`*@sZlRR5M}#J|9M$)g8X1J@*Xb&m_u83FJ>)UG za6>AKD0L<)GZWaQZn)w&2^RS$>9VTu=m^iFhruL}P#aqBcXn`?(Qd0q9F9V2CZO+S zK{*5sS-q@5ZlmCZ32o~Kf8Fb!3>QFAQB%z=v<>(+L|{=^qiHZ81UN7=`*U;P1!p9{ zI{EMY*7eVyAJz`I5cE`*8rc0KFq*`SuEQvPh;3qKcILyS<=;!t<8)XA{ked+|ENKu zA22@AI0*>yeCs1l`>(z6+`J7wz6F^kIk&pQL!QEnas69uH~zcW9un=7AA>;>XG0hd zL9cf4j2c46>9VH@#r$N>2&(^W$T9Tq7@JHtm6J!Kl8XyLMosE3@awMntb-pLf>!*_ z@dvMV^p?a^O&E>hqB5i=hCMFc(luanlepOKYj<~22t{u^4pV{!Wir*kX%gr2CtfE8 zEC#B(&mMB&D@Y~yFDgKA=N21%{nm=VZ!af^KTJYi{@A+>AR&OgXJiur;UQQ{Q69j6 z{+^xwam?!VlS+|I-CvqE;NiwlYfM&7id=EDc;h(eN>yRCbPJHU7e1}W`~p()(G0Mh z`Q-v=iHTp;6K7vB_ef}Mg4@?c+{=t$l|QlnhbS?`s0diJ8%^oGUymip2MVS(^D}|< zq=CYM+2>q(%lmacSP5wkQ;&NEox!HsyKViLII?%c^}3(#jsJPZx{Q>RAFzG`Q$sW1 z7|!27&Y1X+oS2^OT%m2&pdAKo0O_dhAXso5h;1TJO1^sJsz7#*j`&K64$L%+s~6(p z;%cW+iuIxTNn7kgX744(U6T)BayeJG_lV~sJ)43$OOm#h`dJ}>(i%G1Ed*9i6f+Ilyt{HNIxr*t_$X%zv1@5(>$@_YB*bI*Cs6Uhi3Fx*P{-uwmO*HW2Z>X|?@(Y}UJ;zkb)F1kUf-61|V^ z(`u#c$oP`d(edV(0H9iNyN!kL&ICjX0+d}$Or0%2K(SmK(#;awG~mfv$P5 zd7tOZ^xflti6EvYyFjV{G7?I_%LL{I0Cj!$b9WNhp}G6`kS+=F(e}@>6e2kNcA^cT z@aHBRP8XsYi9At(KOGqR!FB*1%lwX3aM?kGH~-_^!7Wa}TzAl>mHYd6{ck6owE=HZwB=oV!<>qLW|zd*M?n(i5K_ zNbQRxa~}n9oW86zX8{%$mUqNYQr*p1eu+IJ7gtXexZ5e8M zB6U>ymmRWM@VcDvMU?uzK%Tgl+B7i|Kn_>E_+v6xxds(v7{>Uh7}pFv46{86yf6lC zB5HbSsP(S3AIv|VsKq`~(o#~v zgZ@BpT&|fMg^Rdg(^j!8duP4SwW+1WGTF&nvSgNiNL?oosWc80a^MwbX*_w?Th^%_ z=jEi_XLA#CBiH=go%r=q8_-xpqmYV3-2#MnVABT7VpwW~q0w5&O1apX>Lx`UaPlxFa$m#FDRK~%gdAD(TTDONBt^L*Ayn-q73!mKU9wk;= zYpGU(*oWK&%a6Sd01yVUZHGnLfkT1rCxnLU6z%(1**t6VxSsIl@LCHKNhEk?8?k~y z%&RAQ)$*OrrdUjCR$k@_T~ljGRRwa=Ny8F(P^@t)#UbF00`|k3MPczS6pnAj)2V7h zp|R9Xa^x)}1(Xr1NvveXg?j3X@+|IV^T3?DLgun9>iEez50V9qt*g3ap!GHCLRu&@ z?nC+Q|Oe|X{3lV8ZH(2C=A+@@? z8gODZkh@-xfNwpnKd#sG0>TujsMHw0yoT}lXxVKi+VmTu|V)#miybweG>5Gu&O~5 zOWVGtwJ)PLYb)2n=H&wcc6D18i3kZOA5205Oejb;|2%mSfV6sw^(m^WoVFz*B_0Q>NXbMoq>C&9Qn9&84M&)M}qX+d@x21Iz3tV6-MZ%!U z?n#_$sL{IM?tMHNU=K-Rkr-mzv>aVQtyG88&2kU0$0fjV*rPhZb!e4_Hwlp5es90| z-hO-4F06J1Cd!L`prv%miUpL}`MRvm=px|I@u7cf(aFQh3qE<*@5B>iJ#gFA2(@7+ zpGcy&_q$5%txgcyJd>SIvzxl#9b{t|KK3DD+Z)AD1#c+KafoI-<0j1KUk@@ zj6f^medvFKm#%B3>Z4vhOIv}iKYWjTfE^KGS_X2$&m^NZ30<&fAqkE1tH9ZJee7E0 zMbJW_(Q~Z3>Ph_EhKw5+>DvpCde{D`WN9iTwVlt?azF(yyRW#sPiQn;@8>ktQ#RMU z=nR#Urqid!MvOdQ^myAUSNz#bm1(o-(4yC}!G4)z%6Zs*#jZMR0`kfJ8@a7QJ3n3C zE_Ay)m$l3gYN@37iv!MN%tL zvRicsH9OaThmC>ug{b-JJkrQ0Ue5KwH(oeFN$G@O8Rhw}*8If=@UV`a?XmYR`&h1%p?LI>*~T&2r+eLu`NKc0Bi~WU`zM;l@|} zEL-g@(E>jCcQT!L9$?sE)33KJX!ENBv;epv7ifsg^)_b>h0<36@}f*2YA|wpqpZlCtn$LQ~kZ=JjrYtJ>O+au84Uc-Zph^zg78@oDZ4bG%1Z6p9FWh7gd+ z(u_rIzYg0|I?N1s4h+-gRsXT~JYbCFrKjN0p{(H1FPi5ax_w~a3%U8X-C_6$1`g+X zK^4O$&DBLXU&^Kx<*Ik#&S_iM3kKGHHWbT%{Ws_)}stB^bPvHW;h%diuQ zG2AF?O`i;Y9|eA$6=#VTnmC(c7VyKk04Q07#gNG3MhaGVto?WfVBvkh`6ns}XA?I< zs&y(Y)^6VoU&0zg0@>5r(X?4amwhtZ8_S)Q{Vg^wG30}JH?2V(RT~TR9#t`B3#JqR z*%cE)fEshd`+gArZdTc2iS`ME05IBU(EaN)aITOi>=rM~6z4%WDM#%-Gw|~hx)-N^ zN`Zi%&eIhv53m+XnFW?P-l(dO#q^qndkg=d3E@6cwc;i@6$6sM9=8mPKMtY>^!Wl< z!B)HAmqhgT0huR2sawlZdHn(S>0A#wc0paNx?EeFq@0Ht)GvgX$IqgZW&h(_i3$a@ zS6=I4GB+zCF&pA8?pM_bS9h&pKih$eiL0=2$?B;`I8OW<$FziQK3=xMRQ?U&9WMw+ zQ*>w6qyw(3--IQF2Zh%!?{`AUz+9zvMyt4tOMq4Ifzw7rL{ONDK^p{T|Uo^xv}R~H0irKx5p1biTpMZk_WbYp!h z60gOJ&3?U%c%;zR>`{2UnGIP7PmsT0kaN+tFUv0hW4uXxpA%sA3*;&N|M{v%My%t+ z*y*6ZfMkkk>DJqut4yC$qH;nBPX24KO4ky?PPwbgJiUz!o)bV?*j*v6uR!erk$N;P zG_TwBUf+|M^&be^_Z%c_?0TpH&{*{(z(y5G~N z6DO$e+kaJ$R8;+5`3S4IM@Mm_ z9LZtMm?OSDY&qq7leY{18~akI?TDU~Gfh4;S!YOq+@t$CaT|1{O3G5%rZa3F#LipkD zv2E3lWUdd_%2J)&;|XW|ELUBWOS}NY6{TU^;$F#8Lhi=RcCANH+PbePUwVs z;}t8M4Tdv`Qpg~iekv{P`}@az?K@rw>l}zMKY@b#?#iwQ*J!=5=0T4^qv{J*%l%h( z6rGZ^M5ix+3#B%74u=9)T7y~dAN_dVE`cEvJSboMrak15ow?iPo}|LH|)ckP~Z}wc)x~;!WR+A6S;Na0|}`F ztNE!g#>zuWjc-R2>k;Gm5POAPxyzVs*$RUl%GPw$#=q%7vk<@>$NB(nRq)J{s1!K7 zyz~a(4Z}xCxUwNlF?YkH!=JYCd!I11erPlvi!fMxE#j@6H~On|N|8vQWSpDCRm0+H zID6_?;Dx)5d;=1ANo@i1O05>RXPLJ@#(iQ4V+fT$o5blzH?{1@SiG&)gsjiW< zgy;8@%S*cjL%Z(>vzV!;j&dYa##MqB^IVBpcZ!H*-GzIaouDR%|Br6nM(!T`(5Lxh0eIQ`gqCqKpz5;#TJI^)7Z%%UA8Go=H6Sdx|IjJ4# zxrobm({Vm9A&s0}%fM_Do&vJxpiPd^l0oA{dB-p6xsFLgk)p0tIUTZVxs*WdiGnNgvLl0(xWh-7s>qepj$mxdsJMmC)arWLe3XdtrfZ zcJdDcQ~1DsL+ME;U6Ko@`*dni*1Fs2Q?1bL3KVIb#M-&6d+wx44C*{rckT#OH$vH$ z&n6z+`GSzWlg3%emS*2Q0gn`QV5NAdMrdnd{OC~aEzkrJsOI1pssa~+&FwKe;VHKy zHkm{5ocr>~B@I8N39NdZ)5G`A7Hl!GnpUv%B_VDt*>7j=I&SRm@#P4AcCj8bgOkr) zFyDF<6B>UYgpDc*D9?Kv^PhPFLLX|lho^6 z=F+;gu_!%k35m7SpY=^gFMVPpj|M-EiW>%nEa1GbB8i~QhE}}&MU|13)Fvmx-~3-tHng-2m7_t;Z%MJO9Hpp4dHIQNeUf7c#tsTD=e_mDYiL$^GN-b z3bw{>LU9bYXi_}Yd-j?`^eV&3Xwd0F7-~bIekq^m_4}}^m9BpPHxlNf;Vu`-`7Gm~ zwAVSMKZMOZcT&cOprzH*eO}FdS2jT?Z2>RWN!Rm}{)^fD&x~Ji-qpn6OZCO7@+%b8 z=cfZ>o0uCkg*FX$tMD33YvNgaTAb@vsJ+H_GRA%&Hl5z<02Eb1DD_ZxP9H9tTl=B2CI+ejQwm!4y{b~&F&gs#h@s38$^@wPV&Jv;w(&& zd`>#NrW@T0gHr`@Oy1Y@4TaeX&@;n?bVeyhnrv>HU(`@Fv;z(6^LUCo{`U%|xfS?Y z+!1^E9izWQmSCBs(+{!}I6lnETk?28>O8ie97+6rLj zNwKxxOT$C> zD5sr8jLhM$*OaVehw6X(N3$>K9py!`X6`GQ8_8BUMfUk*iQsnREn#D3jTT1KiKB7| z;R}i-+gf*=6D%F5&mF8>+hs-76LWeq)6+X0;3bhu_B1$ll&_R~?Ccs^4n_qnJLN|3 zPI2Z`q-VLxW~_oWtL1!~^!E9lJ~_+L83L7#p2YfrUdVk}YgveX&kte!zxcvOYTuR) zK%OI8dfYemEH8&9uJ(I=Q;|5qwC?#lE>>2ues1AvM;Ti%fQ*57`%Etb0Z#KZjs+wA z#fdeU^bG?{-jHNoXkJT6c-P;Udf(i<+RMEmzKGuPI9F@woy(?@&dXj{N_JC%tRNF?R3 zqL=bI-CnG=H&Ro!CVwD47^c`v{b{)QYZY#SSA~VsYJb($o7W4!BW7ci?IP>6M$9y2 zKUW4C5X!e}*=bRI{iYkJz?t?~v62!?xy%3z27#UHKY!}wk;i$f2w1RR?pB0}>jiV{ zJIu;bde>D{2!PFYn~pnDuG2z2j$%-Dn}`Kfmm5{{+wWXK!Y1*+?49}5lQ&{`y>CZ; zg*TSP2Q?(x{4D!iboj4LSBl%QF)P?V%B1ArFx3B$29s&S}?Vc4OS|MG}_-8XQ zNBQcU`woN-#X#^%3+jq+V(J?ZK#TE8ddvBp9sP799_2HNg`Ix{e{+lVs4x(p6}U{N z&3qKr{eq!w)BcQxUPQ$Onm*!s$v$uMN8%YJE;btLm!u1f5zrnr3j=eGb+ojjKlFH+ z$vn46E5UwVppJRSZfXd0)pT^F>$6Sb3?t%^(=UNr^^4p|ioNxC!e=q48iG28F}G6G zxmQS;(U0=qE*G!EH|CecQi!7NS=*)59#4d`Lly1V2ZM~BdxdegG*oF~8L0AvZ$kXe zc5~yJMB;WbEo0t}9(y8~>r5ErNRJ-AQ>;fUjsc-RonHDs-B+9E2`j1mm|GJs@j2Xe z%Jcrr#UbtD_Q9j!V%yTyxW+ys>{b7eKN z6WS;1Ds*ig-~_nX)RkY-a7Rl~UE6pJ5!2Ak+%k-nsmGN@^O}42Qo)KEX#^8r)axnm zQN|)Z5cXt;XIj&@cx?Q=VeG6Se=vG56FB5K(?+4#=&w_=Kp zGsyTYr&Z?%LfMV+>67tSj~Wa(GZ##0ReaRpNCn8+!4K^KO?KgEMdAB-Hoa3u16( zEAZ$4OAyiqh}(ruNK2`ak&#|UKnV_hAmH<{TR6?T?~xUH5kk9qP01eGzY-iYldXF@ zkf*<|ki2F1f}?KpZ@t@R&E$ONXcf&1(>#X1lSqa*r>C%b-|bJ-{goImn_D(V&YNsv;G@2g@5q(SUx?d@%DnQL3hHVyGq&adp`YcvI$8k%8lTW;(jY-QCYEI z-w-d#;6%{-4HgLzMqfTolbiNTZqleQOm}RP-$-Jz2o2N=Fbd9}`UvTt%2Dlf8eFb2 zGbQGHJix1q%HM}O^}mR6dVpeSRsR~8#NT}(NZ`Y}lla8qjm;ND&8FG3HF z^FgO!S1ksKa;#+_(;KA48;ZIclxXs3)T!R%u-IpFj^75B#hhl{H<pKWUQ<1^xb4gu{#NlVom5I-10kT^#_AtMBjecyh2 zx}&D!gjCFCCik2!s;0mbN5#Bse%k?eY|ja(BCXfkcqO}}-`o?9YqlT$82?@5qE_;V zto$mzVRZOK%n+wD&U#$eJ$l%l+^KrO@^Vpc*Cu4^ml;#y8zfe4EiSWIEJ}{h1Yu!N zyZdlAJ#NuijYH3_fpyK<|A5f6S*ugivcPeL#wETgjb%|z9z+B*6*-5*>)^|3J#QqB zW!r!|fcI?ogP0%U3p{z%R=`Q?4jN9puf@!|3tUA7(9HRHY#ef-AV8~$-!>a6_qv5d zOVT(zzQRq&bSwYU0hu79d-D>Tn6hwMcx+Hjw#i;8Ns+6=5B2|lr}+3 zh%B3hh`4#wkQjDS9v`C@ZI7+q=|G|3 z8^4;w2bXn%C|WIm7dRmcV}G`>I5)dzcxA+;1R50P1H9gh8YTl{F-88e@uG)u1PI~WXxcQpVYzW&4S9yPMZ4UkF_+|nfT6Y=bN zHtOwYMOAwUnR<6H3_2AD=YAm$Z)~9oPup~$wv*6v*9#fK7?ie{p%k+%KU;Y2oRY;x z{#PJOmw7LXy~O8I25N_-y@*=nhtyYah0cH9N8QFVkkpblhi}1HPD;`5S z>OT=Zs5EFa*k4Ykfanie;Ww3;n7Ms-%%3w m5HXhFJRDc}jPCW{yd{|PBQ0vAykm$3{@{w53gvQ$fd2t1ogYg8 diff --git a/src/bio_ik-kinetic-support/doc/solvers.pdf b/src/bio_ik-kinetic-support/doc/solvers.pdf deleted file mode 100644 index 80f2c6520198dfcdac57bfe340cf4ddffd73f402..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19457 zcmcJ%1zZ(f_cp93-Hqg-LAnlymJaD|q`SMjL%NY}>F$yS=}t+J?uHli_POJE{_+0a z?>i$iv#(iu&+c(uYp+czEg(e0K+6I{TC-nz3&RAU2UzNuz;JQ`=*0Cc4DE~n%ukRk z3;+P26EZfl)3gmI9bHmu$+30JV!#JhxtGu^d1R}NWDjmrisRezr z3Mj)5hu~exUjTOzv9(&apYL>76IKJXwGuR_28Ygmbs;5y!wMh?!9_TS=lQDZK8IU4 zNQNyaAjBa&AUw_x^`q~oaoZ%kjfLW1x^3I1&3R@M58GXHAK&7Jv_hkLA%s@8kt5%kg( zwPv?>>o7B}>Yl6jpW%R|T)l%cPRA};$o|-Z$bkH+(&}hMyzMgAIllMdO_GX81Ab#7gtVDo`ltT)2x8iK z8&23P9in*f^4EqY%LaO9-&gmUE!ZR2+q1j+2pg{(k`5h+BqN1Mh6km$OCVN_;p~y) z_2`1=r6MvvzUgrVx{uz|0yqqZhO3#)L4$YOX@TQXx8vT9f{s_MV(!3q z&XKMP+uLD*)Fb?`Z3y$-cO2KWQ=qJ$ExS6kU9?hD&G&U5S5+k=6LINlCWbuQGi*qn zj;&a*B3df(%gPgle%i9iK=m0A`KBgCgQ^^3=#t)vhn$|0JLO~s##?bs1<7-REM(d7 z)b}H7(9{K{zG;b3VzmDJ@9=KLS2>1ok&(D0;O>r4b;L8&%_|}(#(CBiJwJ;yAY7s+ zjpXs##Cv@ielOhXB$R};o;F26bZYrlDl+0@QlQ1q9=z#$3v4_tNiMeIc%;oGI6J?^ zPtMz3@68_OOf)53$meZRd-|izdS07w+n!vUEms*S9WH66p5@>{cFYLL_oKOKZgES} zV(Kj+)gf10hK;|1Gpi0~65QdX{L$?8l}v^nE1y*P;$SC^8M;XoZWG8%~Bski?Fw6P&xwocCQveX@!M}I#8Y_xLA9N$~@;qhfCYfY?P*U1)H&jOUw+_H)}nv6m%+- zRY#%pOwXFDuTT2y7F-j&mlx@U(YMh1V`_W4d!7`ZXTs;}(_F~F$i(<8@Em0P*PQut zmK1cd6PB}knjrz4oX^11k4~HZDFC43=LIkT=ybK8&QJ5^(}Q1UIw4C7yT3>q(9qM+ z16Y2~yUz(2&@enfz`sL`PY~;G=qahEe}7~N^sg)d=;WQP^Z|6z+J^c+vnQ=>qyO|a zhTrk%B=q%+wfQWa0BTPVJpjbaKnnt~u>zR^3=C}av@9$P3?K%8#?Q$7PZ@oRZ3}q* zPW<{P2-{fNTm2>Tzh3tf{at#wr%c#rTi9AXzgpM%H!KRUv$6k~spk^%KS>GbI~eQg z%L?;7i~LbHvii1`_BOiuwt(k)miu#B+U{B5k}Dw5kdO{f~r-KF8e%j)!(N3K7V%1%_~16*H< z3%)Qb&#bA~oG@&~%6X||G>IC1S;Lz8Xzl#625};BF)ufiIPZ|F2D!c2TaQ$Nq+Amy z-F9ui#+0n8SOV>l!h-y0yEbv4rqYH5s7Ssl{&K5|QD%&8S{?i%z8Y)grhR-m(6gZ} z<;wVl4V#sb^ime@0l6uBBzA3()Vo{1H=CTa0L!DHzTpoXOg082JRJvhfp-p`zFx)V z-&=2Br_9SW8?LKr$bhGkGB*AQOoA-tgWeft#}K7v-e$!^)9`plq@BV9{I z!MwbGL`O!Rc4XVwHGv8dtKZbipH?p{#(3TeU#S;Jy|a#{3l7>*P9 zioxEN)X8?Z<|-LKm}4g9gg`Q3UloI#UOz5>d_`hH(9S$_7uvcRx0+u!jZJMW(qVLX zvm|vK%{)w(7=N)*+%6o>)q2H_z(;F78l(?>cVuAjz1C_yGnErch$rDD$>R&USP>or z8rf{w?Me{`8EL&(pDF{7n0@fvM$u@iIAk?|fmBoaIRPC}$v@uOzB9ZNDq*S_m`u{C z>rM%SknlyT6R81>Mkfaqev&`1xrXjn76*y#M{JB+zZ0gFxu?lGse`KYiC)VZ!VQ3C z#UAQHNsAm#Tp>qE!^QxXCbM4Nzj@DUrdDa^M9b5x*DzlKAv9A3X!GBppAP3p!4OZ& zgb-esE-9_osIgU=pP*`$*6Pb_S((41^~ZeO8_->@{63!+@_|YHP*ln`U##rC>QFy5 zr9afGul89G#53aKW(*5!4NGD^AmfU}L2>MQuztl*gMfn#4b^O}dg7y;ItJUOmal7K z!BvU&s8}Wa#))`eUZ_^3HPkZs68m0^<2ulxR z^J(={7=H)<8|w-qD5K!7Ih{VyWfe#_p_ z<_(`3l+A83R+b`6?H1yjxq5ghlvDB`_pwixx>;I_?v~Bm&j_M6$+B49nBy!|5#VTe z0&EUL)jZf!4Ql$-3?-Al>>2L_Y9b6}Q^BSB-K%zbfbAhB^r+NNQ8(jf2jkd%J9Zhc ztLmjnv=!Eo63fPOD90beB2up6Y@FEx(pzn-!n1-`ni;;mIlD%8hd#xWKTF!8EAW5^ z(-qF2lupFs5;Ky`O!mo=iy9#?dig+yhxYH0E^Yf1{&@Unqc)rxe2^!DfX zpWmw{wahoLk0JTOkR!7*)HkntQ`~tEK||w%kz;&vHbV)0sT4^vjX~#HR@*?+{5>8~ zQ}(c6WS;wwLdDHt>K@ULxDIz|WN~Qp!cwR4MuB&1gHI}8g3-A5W}QG^k6y`GMN~#e zF^kmnaQ&5~Ga*L0ZBIaM0=*b{tOcT>w;^5C9{nX2&idS^u%6icV|$hM#Vu(3MjdK{ zwyC{%MPc>${c$M&q?Xv!P+oe3_CDv7M!OD&Oy=}AcVBlGxZkiKpXsAv#);GRQ&0!t z6^|-$(1phr3p**z+&|u5ZP2nbiV990`844qU2bX{0E;n^b`R8RZNX;5YV=ArTThaH zC|x^ajyQNAtDGKn%c-zXtuI=~h0d!I)-FI*l%;^bZu2dGG$X0#B&P2vn$@WdDAAQV zX1(fd71wGl8*-c_z`_Z2clooAI`P5%}ltgwAs?x zyhH0AlfSNY!wL8hq#zq$kA=U+9$sQlRt|83)?%cy?ru(On#=!; z51MD4M4Y5QzCc$b-4S_W_3{lMSma)ICPJKbvXyw(#7KTF%|#cwvR)pM>QJftq__!t zt-1_rIQM=}gnHjQe2=t5NxsWJvDeCFabdvPGTn>Z4887pdIq3Xb>R(@UN=_r*k3y{GkXO;=ybespS5Snl`@9ol?2g5F5iEVQ znEr#4m7y-Ha(?FWRAqC;^ z2R@+&=j?=E$rF#qq*;8MXS%=CpU^8)F!$B$T>?R4C7TBtQRI=Uw@C(t>8ecM7|f0} zZ-kjw1_(iN4$QW6H+EuzKjt3t4R1pW)C4+G@aE}yG1d{nfZX&b&A`qBoZK=zQQU1m z%-9G=mu@-*r)lrUn4@gO!3|*;9_lVcqN}QxZd%`@xIi17Ofa5VN4+poRBlq=nhupW zn_yNd))p-7`|8kJr@SpcEfTHBUv0@)p9q9=Hh@bxgrHjL4WP0(5m7JqeeqNPVRf&= zJd%d=QagQ?!&SH%k-5u9xEq6W)A$xOW3!3uU$&{!)=ALHuJ%y=N-Hg#Cur>w-9zdd z70jdci(9nTy^aJlkIQn}w&J7mh`_G3QAa!Hf){Ps>vh>qhs5oJif6!Ok+@ zJp5N=ulUf~8t4Q)n0#(!kFLo+x^erA`1gK-@88b4GK0E|K3S$X*s&kr5}j>YAahNt zUkKnDN!0&{n9iVkIYBsw*PSnVyn}QwzZ2WUXuU0m&@d4!#byQB@0hb5Be~dW{?0fpkX1>T`Ih;pMXc8VHx$YXWa$XVVo!phCrsC7MTP0s=TuX=224Hm9f8BKKZl6rZ(ZsdgdDTji)5I7kvE*P_dnKbIdYi&jd0 zYl*Q!hF9O{P{mf>?efP^f9C^^?t~cSiZ&g2%Oa|5-qpl^RR?n@3)0e#>*PTHDyMuM z9{G8%$I|Zz0p}sSvjK(*?Q8JbOtw=Kv?XHbf&pG+9c8Nhi$TLieP=YoMr#A*MeGi> zopcGv0CW#?q0-c-(1~o2#5Z|=eVF&5)RomQ@+nf)y-2$osi8koQcqWas6$}vRrEUe z@L!3_1I{egZm`V6VvqL<|q?ccoS3< zQ!{k);Q8MuzIodYM+NZ;_{L7BJ>cOg@=SQF*KUXEoh6J*S7%J=dnwMm-9&@V>T)ZN zft)5BIZ+lCA6kD(WO2pB*q~s^V)5$ii4f?Vn9*>AaqHMjjm+@9a>_7r6aqdfWK-~- z+zi1?l}wBA+~sgGJPB>>pezht(YKXwf*|ygNula-|DyMwe8hz@Fe1Y~rwalt^w~Uf z!8}X+AIsje-y!DZp3eTes`pco_(RuYq6f15R;(DF^`?I+9lsQlXXWC5V#qY4B`muD za4n})ZhHvmqKyv8enb*Wq=6O_mG>{e$OKhiAlvk{rN2@j-+$rp;B--S-Qc>PD$&l9 zrNl)>KfM!FrIp2pWmO$91#eV5K0Mf#toI?c@1C7JmEDjN73_+fbQ?`+w#A5N_0_8v z5BMxXyF#<>>9;695|BKs_!2U7%we(NYnu9w$jD!R>c5qgTh6nWs%{G@V*xPFm4%XQ zvV7%z8#W%^C!kOR2s3`eeoyE4HmswdI82gLs?clmT{Uk0uG_5g^*vaOt;@`RsN>&?^Ha?+vM~Lt zivP+%{?aC&^E85FFshKoa~!zxv`lufZ~@J;8~9HS3+jmhEMt|N<=b}AzBz;D}vxfUujrW-n_}%&bteIcQ+3VQ-RP^Pa zbn544KT$qyTm7Hp!vCh0jBRY~_>Ht}epiOL_8(yehF_)ldoutS{v-heEuP3=V+%t7 zoszKyuZ6AgZ{TMxl#KQ4jBNkl7oIir|CfUm_}p#&JeXLY!>p{&L9B927JT8k|5D zgn)p+u>Bl;A{H^7jGc;KJ#Bd?^)Yf7wbHA-wVc@ruGO_=%ftHvN6*E>raD(ig|wmr zxo!8NKxw|9D0w8nCpP^mj5Ot=yIa3RDkx-&^wZT>pGwc!nFyz2f*`kO8mr72tu86~ z&{Byx4w|>4oGmRHITi3wb=ZNP2cKn~PIfGmFn3%FeAm=S%Fne<0Cj}UyH|2-Qm+y!OcDbkVf@GRQR*IdrLrE&JP>8gS*wfC- zR+i|Nf)0_yLi*j;h}Tqgh>Qs#=kzkd&0-LS_?ksf&X0xqsR;&mBQ}iBs(HQMYUV2) zQ!bm85mk>nUKN-(d)0=RI(InOm#5VdeY8X-!Vp2w0boSG37KF;J>^~(suV^^7ECnC zLP-%J!?IV*nSYbZlfg0jW+#3Gyg+H`WudEa{Dd&{Jr_;r;`_lX<75R5oiIRF7TN$Wy z#)6M@3N~wiLAF!_aNTadCD;SkEOuP{dh=t`GMM3(b^opNw)u0_LzoIZwf@wJ}~vfc5*qQkRFu@w_mPn`bbsmaFjX znKMoI)UD-@b>zweHD<@Lu{z7t+1oGE<66yi4f;~!C&CbjF!{L>Nhs<#=e%vPoy|Z}OxG0Rw%1agN zi`u^a#{E^{>(`ZN<|<*ecF_W|SU>o6ys?Yni*siU&eUMt=}O7+hGo-V^JSR@#r4$- zG;so<_px@C126bW_3XY$P6gSFuvHVjZQp4zAtYDr1K)2faQ?E^m*luYVL{hq zdT96Xibs-bo2La0Bi*COB#?U1x~Y11HF8m7XNNhVN16%#m}h_G&P482rX9y4LcEAW zHNRLN8B1iiVrYby29ZU!iSLKHJR&xq>vt%r2u*`C!hQI9Bi^J6ad8UayE7$fbaNcn z<#cVU{B4}VmmVG=2fISo3s)oEg%oXJCx-qgZ>+z}DC4o*08P1;U|plpnvBC>&1u3E za}i9msT>^~iXw`W7anz+=h0Q(RZSRSnPO)k?k}gQXt{hZF5^j6k{T&1P;E7tIw?Xg zcIAl-s}Za_esxFv2>ZASbon;1PR<^Y##wretKrou6U<4u5icEjM??q^X1sO3kU5m8*!aRfC~FBw{`gsd;7xINQ|MJ>Ha15g zL-DbB`2tM)0q(&xF3}HIlg?%`&x;IhW2JKA$-Ht#jKlJm?o#IxLvv)+5&*PAAWlY)L4o!3!aNGF2f4{T zY=K(zct*wG-Q~xC>tMXnDyd{t|G>ultXij{OYz6>Q|`OQU1?)?0}wS_y$RH}Zf;7Dqvc%`sLF@=fx;b*7Bns$GD~iXb5sVb5>qd1Vgmx~7LGV|J60D>^V- zD;j)|vJUHRGvY^fIteW?FuBv+BzhNjK{!F=jLf1mI~Do03nntZhqW@yAAp37iu8&a z0B-lvy&Z`jLj1boy420lL*2v8gU>_9L)OCvTs{c>GhflD!gne-^d1;FbF-4-X$>4D z9IX`T6y+58l)Y>wmQ3CC@Am$i=~Q9mr01o_R2UY6_*~AjwTs8=Hd@Zz9ORyB>LAwI zBPgD!+sLCYb)g9!V@={tYj;F7nD50-PK2|~DKI#`!E&M(u<#J-Ap>LZ)(nEjNTR+^ z4d6B>ml?%3rmbabAUA(u_VC+2=^9&X#N6K%cJGOLndxL(ldC~CWxZd)0+_*?&*0|n zu^=1&?lk1pVfwY(^=m)oSMa+T#!yU+*(7HT1*H4+^z=^RyVvJ($J5(n;c6VG=XGih zv1V9Np~m`9_N!buT921PaQAnPfl`P#r6*(FZX$ic*eWP6DnmX~8)BlkI z87b$?nMw15BxNNQ1chs=mNc@1SRR16t(MVavC$dCbtEFw*6jUJs`f+i#@;>{8_ z#4)mtu@jksSGSZpznLMQncKC;7*F^?a+G|oBiT5!#M-fiu6yRbzx=vIP_nO@Be0MB zIJH8G8;MJGwq~_*yXZz~lWo-bcrm}NJGX#$vb^al*-WER_|%?myEGsF>dpw^j(RKe z`6%xw8QTo?DI(^i%8r6CuB7d20(0@*S9XI}&5gHd5~Z2#$Sznf1TywpBPLb-pT!jiUf&DMW}R?h&_~B zoFIP$3eqp;6Za=C!ve!V*7E`M^V=66&E<@sOB1v-K6ach63jfF3tE4SIb)lDr`6tc z{KNglp|AY?Nx9bL=NoVBb|%;{r@NVieIAYb9rRNz?hffe#*3Tc`ivSB6L4uMA>5R5 zgV33l%v;O7$Hz^W^*JT~{WIgeZDS@XKmCGXQ%9_}EClT`Zp^&E?_xK=h`MN~+PDo4 z6TeGigoyd0C7rR*s+t^L*D8!wqW2kbxTFM;sGysWEwI-2A5rMI{3jJ3bTN(i&FE_1 zXTpaZkID$uj~b1WNdbC5 z;}3XdzD0<89Q1}39djg<)NFR#v3yetit}QSI^y`?PlCwZHU~Cz7`!D+b%G^UXIO(hR(3pVoY=D; z{rbB<(c7bAsFSKG%X-BKc7x;@QhTmD!OSI=YJ#tiSxa;JnG9PE`qGf8q=`BdC7$>(8V4`uuAj`;_E z%}~M6Az=z3Cs=JdK2kn(KH^S7zuYzCM!Z|#9_EKOly6*qj9;2cTPHGIm^wZD)Kk-~ z;~oTFpRt$-Il+D7gt3W6!txspv5Y19FhWM`79m=O#e4sl57~|7Ga3QyEQ}V0FYfoT z@R7?{_6v+bwB$h#C*_BW&q}!lbu!>ph4pF$8DHzBhxLe23S%ab+iUr5AA{CGLZadMl^r>4UsfpX|0JmObUj9wvmFK}o>`YN*gUrjy^_ROg?WgygZCQyY)an^T*d zW(-5k!@*0ex3ita^ZRun;?hEXv!pqiv8lJK2+&xwD%5Cp(#CF5euYW(1(wTw*5BBQ zkmTab`>+kCy|oQ)1>VbZ+v-!7mXFofmn+k}td+CFnH+ALBHu%l1BkQa3cz3~F1Z=6C-}K1YQms0V*{`|` zB`+Wm(o299BdJHXHdl5Oc7SmPvh+6=20+mgw+*ZU{^7k0|?n8R9FOoV5%e2 zgooqHH+}?y*e+g#Lt9Y0_M!AT-8Q0mSlG%@7Y2)M@=<>}T zfdkZR5V()4xn!VU2BDpTt~pm!u5{C>2(Gyt-JwYvNb}|hC2$xQXl>-i5Wy1P8A{i; z*9~&w4cHDJZPzBYsEq2e#VA;b?Ek@L*5`L#|9Hb*;9BcHW$lFFzB`BERLiGc*E2Fb z>}OACO%rGc;taoHuu3!Eui!_kyouDP^2u6?vle*v{Kg0 zE#Jf~LUW4dCk@nkOFTyxx(bbej@VV%J2=OP@G*hknUS+cyAU0J^9U&rl8xHAeJFnR zgJ@#(sP*wnCeBe>+S;zQNGG1aTM+eH`Oz{_Z8^dL&Fgo?c>ire7Bw=;=T4!AsiyNxAgv0C8cx##a`sFJ0^^ zURkUk*&^_~57OmMcB4Nt+blZ?^~z;Ob53a84B~2#-H%j z`dlP*WS6#8fG_-3B-cv_@U^)N=tzGJj6eW4)&@ON0%Q+A03Tx2$P0%NzJj_6objf& z91c>?58Qs?p|bO)_nl|l5<5dndU~$y_duzO)2{GNX$RuKR@`yWS%o)`1Ia07Cp_@T zTBK%9`7LA_xv%qJN6bq~RKOcY$*ssQgvjpUB2P*8ea(uN3;t6#lV}li^=W zh5jQY^|ajPUkf=kq9rVR>5=>|x&0G%h`_`NgZ(0l?7Fge-tNB)-02*oQaIXaU_jEDXFD*?yOvl)dfd8scg&(ca_mahQe65YFTl|oD5GOVEDfF zGrj%taF(&&g?GvzHfw`x<>?DVKCz@0L0IcE%!XU`OX~9(B##eZZ3F7nCDp6+2ZDi0 zT;4;nr>#}>Jf#cx3lF&IYA*`M8&`>w<~mWTy$kNa1!A^K%u-Fkoo+3qmQ*1OTkX-p zu}71)l?CL|QFM7B4Bl~blJ)qwbhz-ECBi#1yE5;f@1j?-+dM>ZvboymbbszUoIMOu zDT}d4qOs|})*ql#p`#4Ee)k!qCF9Ypj$E94o}Ac*3=6*v1jj~Q+ID}~!X}R3)^JH? zlW;w=dZg>bEui9RQ}IjR`VWotT=Bnc2@Ifrwb8#X()v5W`jb0-hJNwf0?NYDeByuM zxg{(uEdM3*ANXy+v%vpAa?E%c&vTjPsI_luBC20Vn&|`O7YPn!8VnWvLP(IEV(dkx zGBY+fwh`3pg%TN6WWgW`QGrFGo_EB2lU_2&yczKGz>Hx#Gjp0qYotsOi>+oQKs6KB z${o{++lreU>*ccJrR~TG$2J7k3#?2CB%cYYd__ECmn~ec7UXx0&NpS-;DK#(T8NPX zD#=(S$tupFcDIx-F40t{K&KIcW2Cm+wZ$vd5C+4Es@lpYPE1~bTU}l%liKbQmrymt zlLrjNHp`vp##_Xbr5P7^j*BerZ`*rO3*95<72@PD6)88xv|g04^C;VogcXs>;k4#C zuz4MTM*`kX_sO(I6-@DFfa}EzqN2ywgzsD*lDxU8sp-A(ZQ+{69~8Cm!~GUXj24$^ zOPlB5K|1~Yv!Q3b@Q$FV7ccCRF&2-w>$&qQ2hpKd6bl@oDQ2jlDSN1vl@nHB1m+LC(Ffv#vx&J+k^;axgQG|B^mc8=e~xnoLs-YZ#X2Ef`6+9_<^ZbHT; z*e`#QEoBzqk$pQY6-#z1dvAVYLpeZot}xEms@x{t=2JG=mL>I8_pLRx-Uq_GF^Y48 zL%Va~^dm=e#hv{dSr~ixT~5mI-T9&uudBIxCDH*YZ|LSo98p;yhjx{7i>-NOW|=wQ z!v0c{><5QJyhCcloYav-@T(LUP%OUo7J= zWB77AB1IbO@DWt;&hIij%Is-w!Rm7I!#xC&3GRe5;$Dphz|*xTD_s)xv!lOYJuxJNeoIWLSX!su;%h!O<11sy zC;~w=a8NC2r;#VrErC#DWdftGfNo->wmn)4G|VhQGOw7(L5=9_#KjQBe@ProqVr;Y z4=5Zg??ZZEQ-}+%FWocQ1XBM*cor3;gd^b8 z*>2&~l7^?<9-uX8dkxwlC$C}@Eh(2Zz<({jmu6mM;TV})kYWl`b$o~Hadr3FT0HI* zbXQ?Lv)V3t#oOKwp?9fYk^~`|mX}5CisP-><53x6u?FB305A6h0r zE5>}tBa>(rZ{=FDqXmPuriHdW*6*Z|6}?uC%Y6HO=SY7oZTj8D-K%u*Nd6yFMa?BA zVk;(kiy8ckdbXXB3n+DYu9j75V$=#4&34A&;gu!3_{Ih^jjx%y8IEZ-6M}Q?gkE^v zZrDh##)wFXCp1v!G6ff-i@^}8uzQOt^Ib(#;ga^~kIpUBS!DRjs-!3>Bq>{HOCE>k zvUBicR_)U1dFa?cgi;eCymt`er4$YkiODIDC*~rX5|zM|rONe^#G2qSK z3m}OPptQj_`_NL6!1>}tg%D$a?PiQVIlQ~Vrokig1mFrquz^Tv`T<@oaCd{;x#WcC z$q=i9I6h7S@51*ILtk@Co2eVC;vMZ_Shh_otv72f$>S%zg}2LRq|Jqnd@aspA*{sa z>sC3geRM|#3+*|`tqh_Ce8Z&cEx0B*8n8vhm_u5Xdfa&K;j@&jXj(z6b6DzkV(#}; zCI*0uAgo+2GbCFX?AXXl())|Hmm|oS?UGmNUaqpnnqP~Z;M+3Bli8_CZ7aUavo{9v z*_=hAVDTP#;Vp~e5|N}HPd$Kzsn|Mg za@S3xtGY4<^Pn^BixM50Ky?4Rq~oB|4A(OE&^Tl~U0jUN;nGT=whTW9t;|~SF*50E zD)5NA&!6VRS&GxWC%V#fO1ckPiqg#Z!Ai+z!%pdxNbY;ikWx6-pcyceqsIzayu5BusCYK_5rb8Q z424_AaW|viJZ=B2b%bAKdW3O_-+-DFsM#O0hU@)_=1#--`-ABpoBf()tZc6so8R<< zORv(1;6n?#RXf^MN(f3#(a%}Mzk^TnEyajkq7@{Uh*V7TRDub|Yq5D4PJh_aJjO9F zXo)}2FI=OFABgy<&Tir`d}X~izwEA?ezz4fGsWRhh9*N}LEoVeMH2J6QLzzKvHrAVs~R2Nee6+Fxp7}-_!rB`K#%P8_9<`soG|CVO2DwCcI!<%Qy!`W=;{X&oH=?|t9%D{v7& zbECMBU5S66S}o43^Re__YNDbioR~r=Kvl zLyyr~Fbsap#pnFk?pB!j<3O!6y~1Z0LrSB`{(5U9DO}&}r2TNa)pKIea;w5)sPk(C z6*h~9f_@ofQ@mwJy!+(Q@EKs`i0ZRL77aOP*>{}B9ql48d%F9T_UXAx%7>A= zEnh;=mPBCd10tboH#J|QS>NQ`3)_&?<Ns^-o-#BOWU18byLK%^OFgY8#YYK#O?6R#M22Zw{nicBU~ck;Mp z78mAXL#A1?i@gl*wQPoL!OKf>zw$X1h#1-rkUc>k5!SMMwOJ_G+Z88Jr&|JO*t2zB zwk#DE*+rXVxD3Bz8Cl7U-XAT{Yx9Gr>$!9zThDqUJbj-=wsytZc zyUHZzqZg?5>$cS5ReCJ&N04{wV>|HbKm@4QciP$g z1Q{ayH3~D_j;<#Vfz5>CwJ!cEMld-)R(mFKKGSN5_FPfPFPeO`M-qmGg{4zH{8)6( z-tzbzuG+*#sA^dNGi1?(9xDtYrr)d*%Z!t$J5@)tf^6aDdlmd#kC|L8+rl`Nash!3 zmJY5Cwi&(|B@pdRsJP&kDGlrpEH%d&2OLKjhd9R&2eo#w&XmfzqVJRu1AKMZcYC!m z0SXUfW-#%KdJRVK4K6%r$WQ50AI&lWo-Zh~_R?;5%*S@R<%G*~>bg~s+ja%c_j|X8 zKQYzXe7P*i=g5CdD<ol+@%sG>ajuI*6lDV`56;;_k6O1znwj$Pu~FFBYWi04}=(kk;Z zM(WL%o^m36l$6i0i3XJ_VhzTS3wg@!vLP1rvH3Vu8svg@tfH*DS>tADqOPhYwRtVn ztnXhcQd)dxr&T=dN(1mWvvO4udV33HIUzx z&dFZXpS9XCC!!BQBOm|+7#o|W9!cNfHF?KPIGy_+lVH=`Ip3u}Vx?KfPL0E!tBBcS zj^D;!lyg094!q}C@YIb+*nmANm6ysXh}VRA_X*w;U+Oxa3QzA^(#m(6Pc1*rU_dcM zMz2T%?W1*hLG<J+CsObDP>|twBpgLsQ5?*R*ekm2%C}Z~bH& zWbw&Z2M0F|k{Prj1_m>O2l#d(n>9752>_fl3Y5C*lb>4#OjVEv{ZtF%!*#fUV)KYi z%s0Z3@d)1tjz6x_;~J1MS0Q7F2wl9q^#&`Fofa?t+WrP3L(yBlU9z$)c&)|?>U{&U z$v2s;97WhY^2RnFf)BJ@#92oaJk;l8rXH9PU51m0^uq8s0KD6iu2{*AQ2IcSZR)3r zK=cG!65-q`B;B?+ps1lrUWnpG1=yEiHS0EYT2H`ZgitVtcqb>^s>s{?0;Vf%#rN{K zN?B&e!*UZk?BrxEROa~{t?<3yHSHRi1y!l1IvHw{_5mR;4-8W_ZI+nX&`si08_*<+ zn!WkQJ=TPSdp-t!)6V5djY0?cuPZm)WeqSuooc`-izwxQc39MMZfTZYTLB6*30Cz zpy8{Hp*6HJ*BjatSf*<+`#NZpLyIL-hx#luHN6+xl;>^;&S%k9ULgr&`uJGYvF?70 zmJkb3S4CC&#$J2DbU;{UJ3RNSu(lxOrtD2_cws*9nEpPT#u7#{9bH{pD~ip@-@RwG z6}w0*W@br~RkL9jcqR<>oyDHZZbj;(`zGI&ops>?$6!?5K9UqJ@I}6C&Z}8|o+Vq_ znc7Fb$6T$qD#G$1{GrjpNv+oiMqi`=akm+QE;!gzK&=-vZh+TUj1*Hh$(a#0#9H9+ zvdylfAEQf};d2Mx2CX5^CsQ^@&*NkYdSGr*3|D(Rj6ktQoS+J*LvJE9D#MEc(@*eX z)po+!{TZiHEBS2W0>5MQ$L_v3yOiOE{Xp`XMRtKX>Vz9?Qso84INYEM*!0BfA2?Hw z&Zcz|-O!^r7${p}ljt%HT~Z>1MVrM3F(%3wQ139va)hRR{XfuKO}|>393Yt#3b{aW ze>2Z{j-w)tgV-+l_FkG-zY&<6=K>xtrU|VQGQB=ApjUEck}FKLG$e@c#Z)&f+ZIJ? zydqgr%|InTQ7>UyxFRw_%f!j&j!`lKRsxs^oqZisD22!S*0f;cOIW0!`wuADH<{cu zkV(91g)?G!)&ewB=whdGb9Y4~c}ACOs+6Wx(oIo0=kz6hB91H$h{A;Br)~txlt*p^ zVX&Nz4sbk5;fB4dQr4WrN5bCu;13Iucf6}@Fr1`Et~{U6oQMoI$Y1w_VTJasxSMXQ zY_8P_-MQknsce2*fx2Ul@C;p7n!FPG5ZK-ZbBF2O3;rOungG@ov|f?jf2RzNFVRD6 z@8OXH_IT%<_{bKK9(XJj*m);qeYYov`8ed8j$eB!rQtLiv7eskj;MU_6U*Jv@uS1k-)4 zL`wvI*Pj>{QKNXE)^EX9irZ^@_nIYEn~dw#R%yLU4&k;+tlhS!6JZiDPNK`W4Thbs~Uc=!y*QBCmZPY{hCFNdCaPs_3ou8t8g#PXKg2VwvBr z0a@gk*y3vCUYF(R5G#aFU4^tKR2`9e)4{m$u9A6=;8vI|$gHAB)6h5?p5`c z(qG3`t?wb&=^;1XiQ#qY3@a`)wP)a#oLUD{LMBjjK4ReD#_m~xRi`{c_C|zNCp>Zl zBXFS~B0#zNfJK1h)Yg7$b6jV7r0G2fK7D1Cgq#5L8KYicbq(}D77_ZI_UfFh_1$V2 z>$f}SLN^cG?cB9R(6U^(BkbqKV#Ky$#N{`=1E6K(-UaZl$RM@V&pQU(RG5Y^VOwiq zFvRJE<()z3$j&#+#OW42#zLv82U~J8AJ}Y;C2se^LH;b+c@GO7t#3{Wz##wXXf>%H6ir#f=CW)AlusYgx19f<> z--AkLyL%fd>iarEHI$jB2SRZ{(;+iG)87GQj5f5aKeJ({BNzw+xajNP(bw1kZ3taFSV)r9FKj_ zsEy)I!!bS`cHUvL(aj8O$Ng1JIBcZPGdAS_RORN;lYtq>+-$Z$mDF<_^EMa@YZL5LGTWHajjlKAF6@zd7& zj<$Fe=`k}wo;mrDGHJdleYet(T+x_t;7tgFO2&*gKKn4LFzb0k*Y~1!5=eG%_JVyR zqq27?Q(w(RjZEW89IB`LFt97WjHQN>H757_=3&nn@#+cyp(bG`q2=qNsK6SKodx38 zgU~m=QaOCY;kjBNTlG9jKzKwT)EUD5cLT*wlJ*Y+1q(Cq zUsfc>|KRMv_{?(uBF6s{q5F3$(x62b07>xD4SdiJ`TC7C>7ZPebF%Dxpy;cZ?3L~C+(_2Ls4GMDRVFrDY?_pT+(2?eVNc?3-5bEK^oG5Ri?t3-IIU6KeIBUo~~V)aF9Q+z6&6b1tsPD}yzRU&5x{e}45J zn(w(ue%qIxIP~A!68vBKQOLj!zVy?N;=eO6{SUUKCkF~P01flgQg{Y7)~6H0 zlbPw)HUj@|4iv1+|9k7wKRqaZo8q3A>i=I_m;Sr@`LE4OPxiiNBgfw@djQ7&UtA}E zPwoo8O*a4a6~y#p<^ert0Q7U`3 zg7vA1S=gRZc)I@SP{G9fE7q?o>yz6A5XAaZh91Do#`F~9$?53H{o>~nmOplw_^W~G z=k)Ve)8JpHqhF2pysL%n&kBBTtoQ6;^3!bhM=#^Iw13{{;x9W!F#gq3ga2vQq9<=6 z0EMo$v5h5wftG=p76_m)va_>dr+eC@;|Vmh(Y7)&*0rUzv@xXo)!U_QEcNVlpLRj| z-$(n?LdN*pT>oeHf^ABM02S(4p#QfBlf5+(QpPJ|&7z5LDwftRIHw?tU z@=uJOftl^!(qmwK?(u(1kDeLy)E)kbu|0d&{9TTPk@?@^vM@gRX8lu+jpd(Zdzyhj z|CD2(XMJ|=``bGh7@p97$DRhtKjlDA9rYg=D>K7WNB?_ztjteC=${zq-|C8i74$r` z{vHpTrQMS!-P5T2^???(Ft7yt d?DIcIk*%G!jor`Q!~l94-Y}%3f>J^-{|7#h;QRmp diff --git a/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h b/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h deleted file mode 100644 index 6303fd2..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h +++ /dev/null @@ -1,50 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#include - -#include "goal.h" -#include "goal_types.h" - -namespace bio_ik -{ - -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/frame.h b/src/bio_ik-kinetic-support/include/bio_ik/frame.h deleted file mode 100644 index eadafc4..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/frame.h +++ /dev/null @@ -1,258 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include - -#include -#include - -namespace bio_ik -{ - -typedef tf::Quaternion Quaternion; -typedef tf::Vector3 Vector3; - -struct Frame -{ - Vector3 pos; - double __padding[4 - (sizeof(Vector3) / sizeof(double))]; - Quaternion rot; - inline Frame() {} - inline Frame(const tf::Vector3& pos, const tf::Quaternion& rot) - : pos(pos) - , rot(rot) - { - } - explicit inline Frame(const KDL::Frame& kdl) - { - pos = tf::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z()); - double qx, qy, qz, qw; - kdl.M.GetQuaternion(qx, qy, qz, qw); - rot = tf::Quaternion(qx, qy, qz, qw); - } - explicit inline Frame(const geometry_msgs::Pose& msg) - { - tf::quaternionMsgToTF(msg.orientation, rot); - pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z); - } - explicit inline Frame(const Eigen::Affine3d& f) - { - pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z()); - Eigen::Quaterniond q(f.rotation()); - rot = tf::Quaternion(q.x(), q.y(), q.z(), q.w()); - } - - inline const Vector3& getPosition() const { return pos; } - inline const Quaternion& getOrientation() const { return rot; } - inline void setPosition(const Vector3& p) { pos = p; } - inline void setOrientation(const Quaternion& q) { rot = q; } - -private: - template struct IdentityFrameTemplate - { - static const Frame identity_frame; - }; - -public: - static inline const Frame& identity() { return IdentityFrameTemplate<0>::identity_frame; } -}; - -inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame) -{ - KDL::Rotation kdl_rot; - KDL::Vector kdl_pos; - tf::quaternionTFToKDL(frame.rot, kdl_rot); - tf::vectorTFToKDL(frame.pos, kdl_pos); - kdl_frame = KDL::Frame(kdl_rot, kdl_pos); -} - -template const Frame Frame::IdentityFrameTemplate::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1)); - -static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; } - -__attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, const tf::Vector3& v, tf::Vector3& r) -{ - double v_x = v.x(); - double v_y = v.y(); - double v_z = v.z(); - - // if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf::Vector3(0, 0, 0); return; } - // if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf::Vector3(0, 0, 0); return; } - - double q_x = q.x(); - double q_y = q.y(); - double q_z = q.z(); - double q_w = q.w(); - - if((v_x == 0 && v_y == 0 && v_z == 0) || (q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1)) - { - r = v; - return; - } - // if((v_x + v_y + v_z == 0 && v_x == 0 && v_y == 0) || (q_x + q_y + q_z == 0 && q_x == 0 && q_y == 0 && q_w == 1)) { r = v; return; } - // if(q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1) { r = v; return; } - - double t_x = q_y * v_z - q_z * v_y; - double t_y = q_z * v_x - q_x * v_z; - double t_z = q_x * v_y - q_y * v_x; - - double r_x = q_w * t_x + q_y * t_z - q_z * t_y; - double r_y = q_w * t_y + q_z * t_x - q_x * t_z; - double r_z = q_w * t_z + q_x * t_y - q_y * t_x; - - r_x += r_x; - r_y += r_y; - r_z += r_z; - - r_x += v_x; - r_y += v_y; - r_z += v_z; - - r.setX(r_x); - r.setY(r_y); - r.setZ(r_z); -} - -__attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p, const tf::Quaternion& q, tf::Quaternion& r) -{ - double p_x = p.x(); - double p_y = p.y(); - double p_z = p.z(); - double p_w = p.w(); - - double q_x = q.x(); - double q_y = q.y(); - double q_z = q.z(); - double q_w = q.w(); - - double r_x = (p_w * q_x + p_x * q_w) + (p_y * q_z - p_z * q_y); - double r_y = (p_w * q_y - p_x * q_z) + (p_y * q_w + p_z * q_x); - double r_z = (p_w * q_z + p_x * q_y) - (p_y * q_x - p_z * q_w); - double r_w = (p_w * q_w - p_x * q_x) - (p_y * q_y + p_z * q_z); - - r.setX(r_x); - r.setY(r_y); - r.setZ(r_z); - r.setW(r_w); -} - -__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, Frame& r) -{ - tf::Vector3 d; - quat_mul_vec(a.rot, b.pos, d); - r.pos = a.pos + d; - quat_mul_quat(a.rot, b.rot, r.rot); -} - -__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, const Frame& c, Frame& r) -{ - Frame tmp; - concat(a, b, tmp); - concat(tmp, c, r); -} - -__attribute__((always_inline)) inline void quat_inv(const tf::Quaternion& q, tf::Quaternion& r) -{ - r.setX(-q.x()); - r.setY(-q.y()); - r.setZ(-q.z()); - r.setW(q.w()); -} - -__attribute__((always_inline)) inline void invert(const Frame& a, Frame& r) -{ - Frame tmp; - quat_inv(a.rot, r.rot); - quat_mul_vec(r.rot, -a.pos, r.pos); -} - -__attribute__((always_inline)) inline void change(const Frame& a, const Frame& b, const Frame& c, Frame& r) -{ - Frame tmp; - invert(b, tmp); - concat(a, tmp, c, r); -} - -__attribute__((always_inline)) inline Frame inverse(const Frame& f) -{ - Frame r; - invert(f, r); - return r; -} - -__attribute__((always_inline)) inline Frame operator*(const Frame& a, const Frame& b) -{ - Frame r; - concat(a, b, r); - return r; -} - -__attribute__((always_inline)) inline Frame& operator*=(Frame& a, const Frame& b) -{ - a = a * b; - return a; -} - -__attribute__((always_inline)) inline void normalizeFast(Quaternion& q) -{ - double f = (3.0 - q.length2()) * 0.5; - q.setX(q.x() * f); - q.setY(q.y() * f); - q.setZ(q.z() * f); - q.setW(q.w() * f); -} - -__attribute__((always_inline)) inline KDL::Twist frameTwist(const Frame& a, const Frame& b) -{ - auto frame = inverse(a) * b; - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - - double ra = frame.rot.getAngle(); - // double ra = frame.rot.getAngleShortestPath(); - if(ra > +M_PI) ra -= 2 * M_PI; - // if(ra < -M_PI) ra += 2 * M_PI; - - auto r = frame.rot.getAxis() * ra; - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - - return t; -} -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal.h b/src/bio_ik-kinetic-support/include/bio_ik/goal.h deleted file mode 100644 index 480ac6d..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/goal.h +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include "frame.h" - -#include - -#include -#include - -namespace bio_ik -{ - -class RobotInfo; - -class GoalContext -{ -protected: - const double* active_variable_positions_; - const Frame* tip_link_frames_; - std::vector goal_variable_indices_; - std::vector goal_link_indices_; - bool goal_secondary_; - std::vector goal_link_names_, goal_variable_names_; - double goal_weight_; - const moveit::core::JointModelGroup* joint_model_group_; - std::vector problem_active_variables_; - std::vector problem_tip_link_indices_; - std::vector initial_guess_; - std::vector velocity_weights_; - const RobotInfo* robot_info_; - mutable std::vector temp_vector_; - -public: - GoalContext() {} - inline const Frame& getLinkFrame(size_t i = 0) const { return tip_link_frames_[goal_link_indices_[i]]; } - inline const double getVariablePosition(size_t i = 0) const - { - auto j = goal_variable_indices_[i]; - if(j >= 0) - return active_variable_positions_[j]; - else - return initial_guess_[-1 - j]; - } - inline const Frame& getProblemLinkFrame(size_t i) const { return tip_link_frames_[i]; } - inline size_t getProblemLinkCount() const { return problem_tip_link_indices_.size(); } - inline size_t getProblemLinkIndex(size_t i) const { return problem_tip_link_indices_[i]; } - inline double getProblemVariablePosition(size_t i) const { return active_variable_positions_[i]; } - inline size_t getProblemVariableCount() const { return problem_active_variables_.size(); } - inline size_t getProblemVariableIndex(size_t i) const { return problem_active_variables_[i]; } - inline double getProblemVariableInitialGuess(size_t i) const { return initial_guess_[problem_active_variables_[i]]; } - inline double getProblemVariableWeight(size_t i) const { return velocity_weights_[i]; } - inline const RobotInfo& getRobotInfo() const { return *robot_info_; } - void addLink(const std::string& name) { goal_link_names_.push_back(name); } - void addVariable(const std::string& name) { goal_variable_names_.push_back(name); } - void setSecondary(bool secondary) { goal_secondary_ = secondary; } - void setWeight(double weight) { goal_weight_ = weight; } - const moveit::core::JointModelGroup& getJointModelGroup() const { return *joint_model_group_; } - const moveit::core::RobotModel& getRobotModel() const { return joint_model_group_->getParentModel(); } - std::vector& getTempVector() const { return temp_vector_; } - friend class Problem; -}; - -class Goal -{ -protected: - bool secondary_; - double weight_; - -public: - Goal() - : weight_(1) - , secondary_(false) - { - } - virtual ~Goal() {} - bool isSecondary() const { return secondary_; } - double getWeight() const { return weight_; } - void setWeight(double w) { weight_ = w; } - virtual void describe(GoalContext& context) const - { - context.setSecondary(secondary_); - context.setWeight(weight_); - } - virtual double evaluate(const GoalContext& context) const { return 0; } -}; - -struct BioIKKinematicsQueryOptions : kinematics::KinematicsQueryOptions -{ - std::vector> goals; - std::vector fixed_joints; - bool replace; - mutable double solution_fitness; - BioIKKinematicsQueryOptions(); - ~BioIKKinematicsQueryOptions(); -}; -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h b/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h deleted file mode 100644 index a6957a0..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h +++ /dev/null @@ -1,661 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include "goal.h" - -#include "robot_info.h" - -#include - -#include -#include - -#include -#include - -#include -#include - -namespace bio_ik -{ - -class LinkGoalBase : public Goal -{ - std::string link_name_; - -public: - LinkGoalBase() - { - weight_ = 1; - link_name_ = ""; - } - LinkGoalBase(const std::string& link_name, double weight) - { - weight_ = weight; - link_name_ = link_name; - } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - context.addLink(link_name_); - } - void setLinkName(const std::string& link_name) { link_name_ = link_name; } - const std::string& getLinkName() const { return link_name_; } -}; - -class PositionGoal : public LinkGoalBase -{ - tf::Vector3 position_; - -public: - PositionGoal() - : position_(0, 0, 0) - { - } - PositionGoal(const std::string& link_name, const tf::Vector3& position, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position_(position) - { - } - inline const tf::Vector3& getPosition() const { return position_; } - inline void setPosition(const tf::Vector3& position) { position_ = position; } - virtual double evaluate(const GoalContext& context) const { return context.getLinkFrame().getPosition().distance2(getPosition()); } -}; - -class OrientationGoal : public LinkGoalBase -{ - tf::Quaternion orientation_; - -public: - OrientationGoal() - : orientation_(0, 0, 0, 1) - { - } - OrientationGoal(const std::string& link_name, const tf::Quaternion& orientation, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , orientation_(orientation.normalized()) - { - } - inline const tf::Quaternion& getOrientation() const { return orientation_; } - inline void setOrientation(const tf::Quaternion& orientation) { orientation_ = orientation.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - // return getOrientation().distance2(context.getLinkFrame().getOrientation()); - // return (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2(); - return fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2()); - /*return - (getOrientation() - context.getLinkFrame().getOrientation()).length2() * - (getOrientation() + context.getLinkFrame().getOrientation()).length2() * 0.5;*/ - } -}; - -class PoseGoal : public LinkGoalBase -{ - Frame frame_; - double rotation_scale_; - -public: - PoseGoal() - : rotation_scale_(0.5) - , frame_(Frame::identity()) - { - } - PoseGoal(const std::string& link_name, const tf::Vector3& position, const tf::Quaternion& orientation, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , frame_(position, orientation.normalized()) - , rotation_scale_(0.5) - { - } - inline const tf::Vector3& getPosition() const { return frame_.getPosition(); } - inline void setPosition(const tf::Vector3& position) { frame_.setPosition(position); } - inline const tf::Quaternion& getOrientation() const { return frame_.getOrientation(); } - inline void setOrientation(const tf::Quaternion& orientation) { frame_.setOrientation(orientation.normalized()); } - inline double getRotationScale() const { return rotation_scale_; } - inline void setRotationScale(double rotation_scale) { rotation_scale_ = rotation_scale; } - virtual double evaluate(const GoalContext& context) const - { - double e = 0.0; - e += context.getLinkFrame().getPosition().distance2(getPosition()); - - /*e += - (getOrientation() - context.getLinkFrame().getOrientation()).length2() * - (getOrientation() + context.getLinkFrame().getOrientation()).length2() * - (rotation_scale_ * rotation_scale_) * 0.5;*/ - - /*double a = getOrientation().angleShortestPath(context.getLinkFrame().getOrientation()); - e += a * a; - return e;*/ - - /*e += 1 - getOrientation().dot(context.getLinkFrame().getOrientation()); - return e;*/ - - /*double l = getOrientation().length2() * context.getLinkFrame().getOrientation().length2(); - //double x = _mm_rsqrt_ss(_mm_set_ss((float)l))[0]; - double x = 1.0 / l; - e += (1 - getOrientation().dot(context.getLinkFrame().getOrientation()) * x) * (rotation_scale_ * rotation_scale_); - return e;*/ - - e += fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2()) * (rotation_scale_ * rotation_scale_); - - // e += (1.0 - getOrientation().dot(context.getLinkFrame().getOrientation())) * (rotation_scale_ * rotation_scale_); - - // e += (getOrientation() - context.getLinkFrame().getOrientation()).length2() * (rotation_scale_ * rotation_scale_); - // ROS_ERROR("r %f", (getOrientation() - context.getLinkFrame().getOrientation()).length2()); - // e += (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2() * (rotation_scale_ * rotation_scale_); - return e; - } -}; - -class LookAtGoal : public LinkGoalBase -{ - tf::Vector3 axis_; - tf::Vector3 target_; - -public: - LookAtGoal() - : axis_(1, 0, 0) - , target_(0, 0, 0) - { - } - LookAtGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& target, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis_(axis) - , target_(target) - { - } - const tf::Vector3& getAxis() const { return axis_; } - const tf::Vector3& getTarget() const { return target_; } - void setAxis(const tf::Vector3& axis) { axis_ = axis.normalized(); } - void setTarget(const tf::Vector3& target) { target_ = target; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - tf::Vector3 axis; - quat_mul_vec(fb.getOrientation(), axis_, axis); - return (target_ - fb.getPosition()).normalized().distance2(axis.normalized()); - // return (target_ - axis * axis.dot(target_ - fb.getPosition())).distance2(fb.getPosition()); - } -}; - -class MaxDistanceGoal : public LinkGoalBase -{ - tf::Vector3 target; - double distance; - -public: - MaxDistanceGoal() - : target(0, 0, 0) - , distance(1) - { - } - MaxDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) - { - } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - double d = fmax(0.0, fb.getPosition().distance(target) - distance); - return d * d; - } -}; - -class MinDistanceGoal : public LinkGoalBase -{ - tf::Vector3 target; - double distance; - -public: - MinDistanceGoal() - : target(0, 0, 0) - , distance(1) - { - } - MinDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) - { - } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - double d = fmax(0.0, distance - fb.getPosition().distance(target)); - return d * d; - } -}; - -class LineGoal : public LinkGoalBase -{ - tf::Vector3 position; - tf::Vector3 direction; - -public: - LineGoal() - : position(0, 0, 0) - , direction(0, 0, 0) - { - } - LineGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , direction(direction.normalized()) - { - } - const tf::Vector3& getPosition() const { return position; } - void setPosition(const tf::Vector3& p) { position = p; } - const tf::Vector3& getDirection() const { return direction; } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - return position.distance2(fb.getPosition() - direction * direction.dot(fb.getPosition() - position)); - } -}; - -class TouchGoal : public LinkGoalBase -{ - tf::Vector3 position; - tf::Vector3 normal; - struct CollisionShape - { - std::vector vertices; - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - collision_detection::FCLGeometryConstPtr geometry; - Frame frame; - std::vector> edges; - }; - struct CollisionLink - { - bool initialized; - std::vector> shapes; - CollisionLink() - : initialized(false) - { - } - }; - struct CollisionModel - { - std::vector collision_links; - }; - mutable CollisionModel* collision_model; - mutable const moveit::core::LinkModel* link_model; - -public: - TouchGoal() - : position(0, 0, 0) - , normal(0, 0, 0) - { - } - TouchGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& normal, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , normal(normal.normalized()) - { - } - virtual void describe(GoalContext& context) const; - virtual double evaluate(const GoalContext& context) const; -}; - -class AvoidJointLimitsGoal : public Goal -{ -public: - AvoidJointLimitsGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - auto& info = context.getRobotInfo(); - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - size_t ivar = context.getProblemVariableIndex(i); - if(info.getClipMax(ivar) == DBL_MAX) continue; - double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5; - d = fmax(0.0, fabs(d) * 2.0 - info.getSpan(ivar) * 0.5); - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class CenterJointsGoal : public Goal -{ -public: - CenterJointsGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - auto& info = context.getRobotInfo(); - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - size_t ivar = context.getProblemVariableIndex(i); - if(info.getClipMax(ivar) == DBL_MAX) continue; - double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5; - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class MinimalDisplacementGoal : public Goal -{ -public: - MinimalDisplacementGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - double d = context.getProblemVariablePosition(i) - context.getProblemVariableInitialGuess(i); - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class JointVariableGoal : public Goal -{ - std::string variable_name; - double variable_position; - -public: - JointVariableGoal() - : variable_position(0) - { - } - JointVariableGoal(const std::string& variable_name, double variable_position, double weight = 1.0, bool secondary = false) - : variable_name(variable_name) - , variable_position(variable_position) - { - weight_ = weight; - secondary_ = secondary; - } - double getVariablePosition() const { return variable_position; } - void setVariablePosition(double p) { variable_position = p; } - const std::string& getVariableName() const { return variable_name; } - void setVariableName(const std::string& n) { variable_name = n; } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - context.addVariable(variable_name); - } - virtual double evaluate(const GoalContext& context) const - { - double d = variable_position - context.getVariablePosition(); - return d * d; - } -}; - -class JointFunctionGoal : public Goal -{ - std::vector variable_names; - std::function&)> function; - -public: - JointFunctionGoal() {} - JointFunctionGoal(const std::vector& variable_names, const std::function&)>& function, double weight = 1.0, bool secondary = false) - : variable_names(variable_names) - , function(function) - { - weight_ = weight; - secondary_ = secondary; - } - void setJointVariableNames(const std::vector& n) { variable_names = n; } - void setJointVariableFunction(const std::function&)>& f) { function = f; } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - for(auto& variable_name : variable_names) - context.addVariable(variable_name); - } - virtual double evaluate(const GoalContext& context) const - { - auto& temp_vector = context.getTempVector(); - temp_vector.resize(variable_names.size()); - for(size_t i = 0; i < variable_names.size(); i++) - temp_vector[i] = context.getVariablePosition(i); - function(temp_vector); - double sum = 0.0; - for(size_t i = 0; i < variable_names.size(); i++) - { - double d = temp_vector[i] - context.getVariablePosition(i); - sum += d * d; - } - return sum; - } -}; - -class BalanceGoal : public Goal -{ - tf::Vector3 target_, axis_; - struct BalanceInfo - { - tf::Vector3 center; - double weight; - }; - mutable std::vector balance_infos; - -public: - BalanceGoal() - : target_(0, 0, 0) - , axis_(0, 0, 1) - { - } - BalanceGoal(const tf::Vector3& target, double weight = 1.0) - : target_(target) - , axis_(0, 0, 1) - { - weight_ = weight; - } - const tf::Vector3& getTarget() const { return target_; } - const tf::Vector3& getAxis() const { return axis_; } - void setTarget(const tf::Vector3& target) { target_ = target; } - void setAxis(const tf::Vector3& axis) { axis_ = axis; } - virtual void describe(GoalContext& context) const; - virtual double evaluate(const GoalContext& context) const; -}; - -class LinkFunctionGoal : public LinkGoalBase -{ - std::function function; - -public: - LinkFunctionGoal() {} - LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , function(function) - { - } - void setLinkFunction(const std::function& f) { function = f; } - virtual double evaluate(const GoalContext& context) const { return function(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); } -}; - -class SideGoal : public LinkGoalBase -{ - tf::Vector3 axis; - tf::Vector3 direction; - -public: - SideGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) - { - } - SideGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) - { - } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double f = fmax(0.0, v.dot(direction)); - return f * f; - } -}; - -class DirectionGoal : public LinkGoalBase -{ - tf::Vector3 axis; - tf::Vector3 direction; - -public: - DirectionGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) - { - } - DirectionGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) - { - } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - return v.distance2(direction); - } -}; - -class ConeGoal : public LinkGoalBase -{ - tf::Vector3 position; - double position_weight; - tf::Vector3 axis; - tf::Vector3 direction; - double angle; - -public: - const tf::Vector3& getPosition() const { return position; } - double getPositionWeight() const { return position_weight; } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - double getAngle() const { return angle; } - void setPosition(const tf::Vector3& p) { position = p; } - void setPositionWeight(double w) { position_weight = w; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - void setAngle(double a) { angle = a; } - ConeGoal() - : position(0, 0, 0) - , position_weight(0) - , axis(0, 0, 1) - , direction(0, 0, 1) - , angle(0) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(0, 0, 0) - , position_weight(0) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(1) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& position, double position_weight, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(position_weight) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - virtual double evaluate(const GoalContext& context) const - { - double sum = 0.0; - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double d = fmax(0.0, v.angle(direction) - angle); - sum += d * d; - double w = position_weight; - sum += w * w * (position - fb.getPosition()).length2(); - return sum; - } -}; -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h b/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h deleted file mode 100644 index 70e6676..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h +++ /dev/null @@ -1,126 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include -#include -#include - -#include - -namespace bio_ik -{ - -class RobotInfo -{ - struct VariableInfo - { - double clip_min, clip_max; - double span; - double min; - double max; - double max_velocity, max_velocity_rcp; - }; - std::vector variables; - std::vector activeVariables; - std::vector variable_joint_types; - moveit::core::RobotModelConstPtr robot_model; - - __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi) - { - if(__builtin_expect(v < lo, 0)) v = lo; - if(__builtin_expect(v > hi, 0)) v = hi; - return v; - } - -public: - RobotInfo() {} - RobotInfo(moveit::core::RobotModelConstPtr model) - : robot_model(model) - { - for(auto& name : model->getVariableNames()) - { - auto& bounds = model->getVariableBounds(name); - - VariableInfo info; - - bool bounded = bounds.position_bounded_; - - auto* joint_model = model->getJointOfVariable(variables.size()); - if(auto* revolute = dynamic_cast(joint_model)) - - if(bounds.max_position_ - bounds.min_position_ >= 2 * M_PI * 0.9999) bounded = false; - - info.min = bounds.min_position_; - info.max = bounds.max_position_; - - info.clip_min = bounded ? info.min : -DBL_MAX; - info.clip_max = bounded ? info.max : +DBL_MAX; - - info.span = info.max - info.min; - - if(!(info.span >= 0 && info.span < FLT_MAX)) info.span = 1; - - info.max_velocity = bounds.max_velocity_; - info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; - - variables.push_back(info); - } - - for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) - { - variable_joint_types.push_back(model->getJointOfVariable(ivar)->getType()); - } - } - -public: - __attribute__((always_inline)) inline double clip(double p, size_t i) const - { - auto& info = variables[i]; - return clamp2(p, info.clip_min, info.clip_max); - } - - inline double getSpan(size_t i) const { return variables[i].span; } - inline double getClipMin(size_t i) const { return variables[i].clip_min; } - inline double getClipMax(size_t i) const { return variables[i].clip_max; } - inline double getMin(size_t i) const { return variables[i].min; } - inline double getMax(size_t i) const { return variables[i].max; } - - inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; } - inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; } - inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; } - inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; } -}; -} diff --git a/src/bio_ik-kinetic-support/package.xml b/src/bio_ik-kinetic-support/package.xml deleted file mode 100644 index 719ca77..0000000 --- a/src/bio_ik-kinetic-support/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - bio_ik - 1.0.0 - BioIK for ROS - BSD - - Philipp Sebastian Ruppel - - Philipp Sebastian Ruppel - Michael Goerner - - - catkin - - moveit_core - moveit_core - - pluginlib - pluginlib - - eigen - eigen - - moveit_ros_planning - moveit_ros_planning - - roscpp - roscpp - - tf - tf - - tf_conversions - tf_conversions - - eigen_conversions - eigen_conversions - - rosunit - - - - - - diff --git a/src/bio_ik-kinetic-support/src/forward_kinematics.h b/src/bio_ik-kinetic-support/src/forward_kinematics.h deleted file mode 100644 index 904a1f7..0000000 --- a/src/bio_ik-kinetic-support/src/forward_kinematics.h +++ /dev/null @@ -1,1503 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include "utils.h" -#include -#include -#include -#include - -#if defined(__x86_64__) || defined(__i386__) - -#include -#include -#include - -#if(__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 9)) -#define FUNCTION_MULTIVERSIONING 1 -#else -#define FUNCTION_MULTIVERSIONING 0 -#endif - -#else - -#define FUNCTION_MULTIVERSIONING 0 - -#endif - -namespace bio_ik -{ - -// computes and caches local joint frames -class RobotJointEvaluator -{ -private: - std::vector joint_cache_variables; - std::vector joint_cache_frames; - std::vector link_frames; - -protected: - std::vector joint_axis_list; - moveit::core::RobotModelConstPtr robot_model; - std::vector variables; - -public: - inline void getJointFrame(const moveit::core::JointModel* joint_model, const double* variables, Frame& frame) - { - auto joint_type = joint_model->getType(); - if(joint_type == moveit::core::JointModel::FIXED) - { - frame = Frame::identity(); - return; - } - size_t joint_index = joint_model->getJointIndex(); - switch(joint_type) - { - case moveit::core::JointModel::REVOLUTE: - { - auto axis = joint_axis_list[joint_index]; - - auto v = variables[joint_model->getFirstVariableIndex()]; - - /*v *= 1.0 / (2 * M_PI); - v -= std::floor(v); - v *= (2 * M_PI);*/ - - auto half_angle = v * 0.5; - - // TODO: optimize sin cos - - auto fcos = cos(half_angle); - auto fsin = sin(half_angle); - - // auto fsin = half_angle; - // auto fcos = 1.0 - half_angle * half_angle * 0.5; - - frame = Frame(Vector3(0.0, 0.0, 0.0), Quaternion(axis.x() * fsin, axis.y() * fsin, axis.z() * fsin, fcos)); - - break; - } - case moveit::core::JointModel::PRISMATIC: - { - auto axis = joint_axis_list[joint_index]; - auto v = variables[joint_model->getFirstVariableIndex()]; - frame = Frame(axis * v, Quaternion(0.0, 0.0, 0.0, 1.0)); - break; - } - case moveit::core::JointModel::FLOATING: - { - auto* vv = variables + joint_model->getFirstVariableIndex(); - frame.pos = Vector3(vv[0], vv[1], vv[2]); - frame.rot = Quaternion(vv[3], vv[4], vv[5], vv[6]).normalized(); - // LOG("floating", joint_model->getFirstVariableIndex(), vv[0], vv[1], vv[2], vv[3], vv[4], vv[5], vv[6]); - break; - } - default: - { - auto* joint_variables = variables + joint_model->getFirstVariableIndex(); - Eigen::Affine3d joint_transform; - joint_model->computeTransform(joint_variables, joint_transform); - frame = Frame(joint_transform); - break; - } - } - - // LOG("local joint frame", joint_model->getType(), joint_model->getName(), frame); - } - inline void getJointFrame(const moveit::core::JointModel* joint_model, const std::vector& variables, Frame& frame) { getJointFrame(joint_model, variables.data(), frame); } - -protected: - inline const Frame& getLinkFrame(const moveit::core::LinkModel* link_model) { return link_frames[link_model->getLinkIndex()]; } - - inline bool checkJointMoved(const moveit::core::JointModel* joint_model) - { - size_t i0 = joint_model->getFirstVariableIndex(); - size_t cnt = joint_model->getVariableCount(); - if(cnt == 0) return true; - if(cnt == 1) return !(variables[i0] == joint_cache_variables[i0]); - for(size_t i = i0; i < i0 + cnt; i++) - if(!(variables[i] == joint_cache_variables[i])) return true; - return false; - } - inline void putJointCache(const moveit::core::JointModel* joint_model, const Frame& frame) - { - joint_cache_frames[joint_model->getJointIndex()] = frame; - size_t i0 = joint_model->getFirstVariableIndex(); - size_t cnt = joint_model->getVariableCount(); - for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; - } - inline const Frame& getJointFrame(const moveit::core::JointModel* joint_model) - { - size_t joint_index = joint_model->getJointIndex(); - - if(!checkJointMoved(joint_model)) return joint_cache_frames[joint_index]; - - getJointFrame(joint_model, variables, joint_cache_frames[joint_index]); - - size_t cnt = joint_model->getVariableCount(); - if(cnt) - { - size_t i0 = joint_model->getFirstVariableIndex(); - if(cnt == 1) - joint_cache_variables[i0] = variables[i0]; - else - for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; - } - - // TODO: optimize copy - - /*size_t cnt = joint_model->getVariableCount(); - size_t i0 = joint_model->getFirstVariableIndex(); - memcpy(joint_cache_variables.data() + i0, variables.data() + i0, cnt * 8);*/ - - return joint_cache_frames[joint_index]; - } - -public: - RobotJointEvaluator(moveit::core::RobotModelConstPtr model) - : robot_model(model) - { - joint_cache_variables.clear(); - joint_cache_variables.resize(model->getVariableCount(), DBL_MAX); - - joint_cache_frames.clear(); - joint_cache_frames.resize(model->getJointModelCount()); - - link_frames.clear(); - for(auto* link_model : model->getLinkModels()) - link_frames.push_back(Frame(link_model->getJointOriginTransform())); - - joint_axis_list.clear(); - joint_axis_list.resize(robot_model->getJointModelCount()); - for(size_t i = 0; i < joint_axis_list.size(); i++) - { - auto* joint_model = robot_model->getJointModel(i); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); - } - } -}; - -// fast tree fk -class RobotFK_Fast_Base : protected RobotJointEvaluator -{ -protected: - std::vector tip_names; - std::vector tip_frames; - std::vector tip_links; - std::vector link_schedule; - std::vector global_frames; - std::vector> link_chains; - std::vector active_variables; - std::vector> variable_to_link_map; - std::vector variable_to_link_map_2; - std::vector> variable_to_link_chain_map; - inline void updateMimic(std::vector& values) - { - for(auto* joint : robot_model->getMimicJointModels()) - { - auto src = joint->getMimic()->getFirstVariableIndex(); - auto dest = joint->getFirstVariableIndex(); - - // TODO: more dimensions ??? - values[dest] = values[src] * joint->getMimicFactor() + joint->getMimicOffset(); - - // moveit doesn't seem to support multiple mimic dimensions either... ??? - - // TODO: ok??? - // for(size_t base = 0; base < joint->getVariableCount(); base++) - // values[base + dest] = values[base + src] * joint->getMimicFactor() + joint->getMimicOffset(); - } - } - -public: - RobotFK_Fast_Base(moveit::core::RobotModelConstPtr model) - : RobotJointEvaluator(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - - tip_names.resize(tip_link_indices.size()); - for(size_t i = 0; i < tip_link_indices.size(); i++) - tip_names[i] = robot_model->getLinkModelNames()[tip_link_indices[i]]; - - tip_frames.resize(tip_names.size()); - - tip_links.clear(); - for(const auto& n : tip_names) - tip_links.push_back(robot_model->getLinkModel(n)); - - global_frames.resize(robot_model->getLinkModelCount()); - - link_chains.clear(); - link_schedule.clear(); - for(auto* tip_link : tip_links) - { - std::vector chain; - for(auto* link = tip_link; link; link = link->getParentLinkModel()) - chain.push_back(link); - reverse(chain.begin(), chain.end()); - link_chains.push_back(chain); - for(auto* link : chain) - { - if(find(link_schedule.begin(), link_schedule.end(), link) != link_schedule.end()) continue; - link_schedule.push_back(link); - } - } - - active_variables.clear(); - for(auto* link_model : link_schedule) - { - auto link_index = link_model->getLinkIndex(); - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_map.push_back(std::make_pair(vi, link_index)); - - if(find(active_variables.begin(), active_variables.end(), vi) != active_variables.end()) continue; - active_variables.push_back(vi); - } - } - - variable_to_link_map_2.resize(robot_model->getVariableCount()); - for(auto* link_model : link_schedule) - { - auto link_index = link_model->getLinkIndex(); - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_map_2[vi] = link_index; - } - } - - variable_to_link_chain_map.resize(robot_model->getVariableCount()); - for(size_t chain_index = 0; chain_index < link_chains.size(); chain_index++) - { - auto& link_chain = link_chains[chain_index]; - for(size_t ipos = 0; ipos < link_chain.size(); ipos++) - { - auto* link_model = link_chain[ipos]; - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_chain_map[vi] = std::make_pair(chain_index, ipos); - } - } - } - } - void applyConfiguration(const std::vector& jj0) - { - FNPROFILER(); - variables = jj0; - updateMimic(variables); - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - auto* parent_link_model = joint_model->getParentLinkModel(); - if(parent_link_model) - { - concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); - } - // if(linkModel->getParentLinkModel() && linkModel->getParentLinkModel()->getLinkIndex() > linkModel->getLinkIndex()) { LOG("wrong link order"); throw runtime_error("wrong link order"); } - } - for(size_t itip = 0; itip < tip_links.size(); itip++) - { - tip_frames[itip] = global_frames[tip_links[itip]->getLinkIndex()]; - } - } - inline const Frame& getTipFrame(size_t fi) const { return tip_frames[fi]; } - inline const std::vector& getTipFrames() const { return tip_frames; } - inline void incrementalBegin(const std::vector& jj) { applyConfiguration(jj); } - inline void incrementalEnd() {} - inline const Frame& getJointVariableFrame(size_t variable_index) { return global_frames[variable_to_link_map_2[variable_index]]; } -}; - -// incremental tip frame updates without full recomputation -class RobotFK_Fast : public RobotFK_Fast_Base -{ -protected: - std::vector> chain_cache; - std::vector vars, variables0; - std::vector test_tips; - std::vector links_changed; - bool use_incremental; - bool last_use_incremental; - void updateIncremental(const std::vector& vars0) - { - FNPROFILER(); - - // PROFILER_COUNTER("incremental update count"); - - // init variable lists - vars = vars0; - updateMimic(vars); - for(auto& vi : active_variables) - { - // vars[vi] = vars0[vi]; - variables0[vi] = variables[vi]; - } - - // find moved links - for(auto* link_model : link_schedule) - links_changed[link_model->getLinkIndex()] = false; - for(auto& x : variable_to_link_map) - { - auto variable_index = x.first; - auto link_index = x.second; - if(vars[variable_index] != variables[variable_index]) links_changed[link_index] = true; - } - - // update - for(size_t ichain = 0; ichain < link_chains.size(); ichain++) - { - auto& link_chain = link_chains[ichain]; - auto& cache_chain = chain_cache[ichain]; - - size_t iposmax = 0; - for(size_t ipos = 0; ipos < link_chain.size(); ipos++) - { - auto* link_model = link_chain[ipos]; - bool changed = links_changed[link_model->getLinkIndex()]; - if(changed) iposmax = ipos; - } - - for(size_t ipos = 0; ipos <= iposmax; ipos++) - { - auto* link_model = link_chain[ipos]; - auto* joint_model = link_model->getParentJointModel(); - - bool changed = links_changed[link_model->getLinkIndex()]; - - if(cache_chain.size() <= ipos || changed) - { - Frame before, after; - - if(ipos < cache_chain.size()) - { - before = cache_chain[ipos]; - } - else - { - if(ipos > 0) - { - concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), before); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), before); - } - } - - chain_cache[ichain].resize(ipos + 1, Frame::identity()); - - if(changed) - { - if(ichain > 0 && ipos < link_chains[ichain - 1].size() && link_chains[ichain][ipos] == link_chains[ichain - 1][ipos]) - { - after = chain_cache[ichain - 1][ipos]; - } - else - { - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - - if(vcount == 1) - variables[vstart] = vars[vstart]; - else - for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = vars[vi]; - - if(ipos > 0) - { - concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), after); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), after); - } - - if(vcount == 1) - variables[vstart] = variables0[vstart]; - else - for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = variables0[vi]; - - // PROFILER_COUNTER("incremental update transforms"); - } - chain_cache[ichain][ipos] = after; - - // tipFrames[ichain] = inverse(before) * tipFrames[ichain]; - // tipFrames[ichain] = after * tipFrames[ichain]; - - /*Frame before_inverse; - invert(before, before_inverse); - - //tipFrames[ichain] = after * before_inverse * tipFrames[ichain]; - { - concat(after, before_inverse, tip_frames[ichain], tip_frames[ichain]); - }*/ - - change(after, before, tip_frames[ichain], tip_frames[ichain]); - } - else - { - after = before; - chain_cache[ichain][ipos] = after; - } - } - } - } - - // set new vars - // variables = vars; - for(auto& vi : active_variables) - variables[vi] = vars[vi]; - - // test - if(0) - { - test_tips = tip_frames; - RobotFK_Fast_Base::applyConfiguration(vars); - for(size_t i = 0; i < tip_frames.size(); i++) - { - auto dist = tip_frames[i].pos.distance(test_tips[i].pos); - LOG_VAR(dist); - if(dist > 0.001) - { - LOG(tip_frames[i].pos.x(), tip_frames[i].pos.y(), tip_frames[i].pos.z()); - LOG(test_tips[i].pos.x(), test_tips[i].pos.y(), test_tips[i].pos.z()); - ERROR("incremental update error"); - } - } - } - } - -public: - RobotFK_Fast(moveit::core::RobotModelConstPtr model) - : RobotFK_Fast_Base(model) - , use_incremental(false) - { - } - inline void incrementalBegin(const std::vector& jj) - { - applyConfiguration(jj); - chain_cache.clear(); - chain_cache.resize(tip_frames.size()); - links_changed.resize(robot_model->getLinkModelCount()); - vars.resize(jj.size()); - variables0.resize(jj.size()); - use_incremental = true; - } - inline void incrementalEnd() - { - use_incremental = false; - // applyConfiguration(variables); - } - inline void applyConfiguration(const std::vector& jj) - { - if(use_incremental) - updateIncremental(jj); - else - RobotFK_Fast_Base::applyConfiguration(jj); - } -}; - -// fast tree fk jacobian -class RobotFK_Jacobian : public RobotFK_Fast -{ - typedef RobotFK_Fast Base; - -protected: - std::vector> joint_dependencies; - std::vector tip_dependencies; - -public: - RobotFK_Jacobian(moveit::core::RobotModelConstPtr model) - : RobotFK_Fast(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - Base::initialize(tip_link_indices); - auto tip_count = tip_names.size(); - joint_dependencies.resize(robot_model->getJointModelCount()); - for(auto& x : joint_dependencies) - x.clear(); - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - joint_dependencies[joint_model->getJointIndex()].push_back(joint_model); - } - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - if(auto* mimic = joint_model->getMimic()) - { - while(mimic->getMimic() && mimic->getMimic() != joint_model) - mimic = mimic->getMimic(); - joint_dependencies[mimic->getJointIndex()].push_back(joint_model); - } - } - tip_dependencies.resize(robot_model->getJointModelCount() * tip_count); - for(auto& x : tip_dependencies) - x = 0; - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - for(auto* link_model = tip_links[tip_index]; link_model; link_model = link_model->getParentLinkModel()) - { - auto* joint_model = link_model->getParentJointModel(); - tip_dependencies[joint_model->getJointIndex() * tip_count + tip_index] = 1; - } - } - } - void computeJacobian(const std::vector& variable_indices, Eigen::MatrixXd& jacobian) - { - double step_size = 0.00001; - double half_step_size = step_size * 0.5; - double inv_step_size = 1.0 / step_size; - auto tip_count = tip_frames.size(); - jacobian.resize(tip_count * 6, variable_indices.size()); - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - for(size_t itip = 0; itip < tip_count; itip++) - { - jacobian(itip * 6 + 0, icol) = 0; - jacobian(itip * 6 + 1, icol) = 0; - jacobian(itip * 6 + 2, icol) = 0; - jacobian(itip * 6 + 3, icol) = 0; - jacobian(itip * 6 + 4, icol) = 0; - jacobian(itip * 6 + 5, icol) = 0; - } - } - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - auto ivar = variable_indices[icol]; - auto* var_joint_model = robot_model->getJointOfVariable(ivar); - if(var_joint_model->getMimic()) continue; - for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) - { - double scale = 1; - for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) - { - scale *= m->getMimicFactor(); - } - auto* link_model = joint_model->getChildLinkModel(); - switch(joint_model->getType()) - { - case moveit::core::JointModel::FIXED: - { - // fixed joint: zero gradient (do nothing) - continue; - } - case moveit::core::JointModel::REVOLUTE: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); - - auto rot = joint_axis_list[joint_model->getJointIndex()]; - quat_mul_vec(q, rot, rot); - - auto vel = link_frame.pos - tip_frame.pos; - quat_mul_vec(tip_frame.rot.inverse(), vel, vel); - - vel = vel.cross(rot); - - jacobian(itip * 6 + 0, icol) += vel.x() * scale; - jacobian(itip * 6 + 1, icol) += vel.y() * scale; - jacobian(itip * 6 + 2, icol) += vel.z() * scale; - - jacobian(itip * 6 + 3, icol) += rot.x() * scale; - jacobian(itip * 6 + 4, icol) += rot.y() * scale; - jacobian(itip * 6 + 5, icol) += rot.z() * scale; - - // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z()); - } - continue; - } - case moveit::core::JointModel::PRISMATIC: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); - - auto axis = joint_axis_list[joint_model->getJointIndex()]; - auto v = axis; - quat_mul_vec(q, axis, v); - - jacobian(itip * 6 + 0, icol) += v.x() * scale; - jacobian(itip * 6 + 1, icol) += v.y() * scale; - jacobian(itip * 6 + 2, icol) += v.z() * scale; - - // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0); - } - continue; - } - default: - { - // numeric differentiation for joint types that are not yet implemented / or joint types that might be added to moveit in the future - auto ivar2 = ivar; - if(joint_model->getMimic()) ivar2 = ivar2 - var_joint_model->getFirstVariableIndex() + joint_model->getFirstVariableIndex(); - auto& link_frame_1 = global_frames[link_model->getLinkIndex()]; - auto v0 = variables[ivar2]; - // auto joint_frame_1 = getJointFrame(joint_model); - variables[ivar2] = v0 + step_size; - auto joint_frame_2 = getJointFrame(joint_model); - variables[ivar2] = v0; - Frame link_frame_2; - if(auto* parent_link_model = joint_model->getParentLinkModel()) - concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), joint_frame_2, link_frame_2); - else - concat(getLinkFrame(link_model), joint_frame_2, link_frame_2); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - auto tip_frame_1 = tip_frames[itip]; - Frame tip_frame_2; - change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2); - auto twist = frameTwist(tip_frame_1, tip_frame_2); - jacobian(itip * 6 + 0, icol) += twist.vel.x() * inv_step_size * scale; - jacobian(itip * 6 + 1, icol) += twist.vel.y() * inv_step_size * scale; - jacobian(itip * 6 + 2, icol) += twist.vel.z() * inv_step_size * scale; - jacobian(itip * 6 + 3, icol) += twist.rot.x() * inv_step_size * scale; - jacobian(itip * 6 + 4, icol) += twist.rot.y() * inv_step_size * scale; - jacobian(itip * 6 + 5, icol) += twist.rot.z() * inv_step_size * scale; - } - continue; - } - } - } - } - } -}; - -#if 0 - -struct RobotFK : public RobotFK_Jacobian -{ - std::vector mutation_variable_indices; - std::vector mutation_initial_variable_positions; - std::vector mutation_temp; - RobotFK(moveit::core::RobotModelConstPtr model) - : RobotFK_Jacobian(model) - { - } - void initializeMutationApproximator(const std::vector& variable_indices) - { - mutation_variable_indices = variable_indices; - mutation_initial_variable_positions = variables; - } - void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) - { - mutation_temp = mutation_initial_variable_positions; - mutation_temp[variable_index] += variable_delta; - applyConfiguration(mutation_temp); - output.clear(); - for(auto& f : tip_frames) output.push_back(f); - } - void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) - { - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t mutation_index = 0; mutation_index < mutation_count; mutation_index++) - { - mutation_temp = mutation_initial_variable_positions; - for(size_t i = 0; i < mutation_variable_indices.size(); i++) - { - mutation_temp[mutation_variable_indices[i]] = mutation_values[mutation_index][i]; - } - applyConfiguration(mutation_temp); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - tip_frame_mutations[mutation_index][tip_index] = tip_frames[tip_index]; - } - } - } -}; - -#else - -#define USE_QUADRATIC_EXTRAPOLATION 0 - -class RobotFK_Mutator : public RobotFK_Jacobian -{ - typedef RobotFK_Jacobian Base; - Eigen::MatrixXd mutation_approx_jacobian; - std::vector> mutation_approx_frames; -#if USE_QUADRATIC_EXTRAPOLATION - std::vector> mutation_approx_quadratics; -#endif - std::vector mutation_approx_variable_indices; - std::vector> mutation_approx_mask; - std::vector> mutation_approx_map; - aligned_vector tip_frames_aligned; - -public: - RobotFK_Mutator(moveit::core::RobotModelConstPtr model) - : RobotFK_Jacobian(model) - { - } - - void initializeMutationApproximator(const std::vector& variable_indices) - { - FNPROFILER(); - - mutation_approx_variable_indices = variable_indices; - - auto tip_count = tip_names.size(); - - tip_frames_aligned.resize(tip_frames.size()); - for(size_t i = 0; i < tip_frames.size(); i++) - tip_frames_aligned[i] = tip_frames[i]; - - // init first order approximators - { - if(mutation_approx_frames.size() < tip_count) mutation_approx_frames.resize(tip_count); - - for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_frames[itip].resize(robot_model->getVariableCount()); - - for(size_t itip = 0; itip < tip_count; itip++) - for(auto ivar : variable_indices) - mutation_approx_frames[itip][ivar] = Frame::identity(); - - computeJacobian(variable_indices, mutation_approx_jacobian); - - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - size_t ivar = variable_indices[icol]; - for(size_t itip = 0; itip < tip_count; itip++) - { - { - Vector3 t; - t.setX(mutation_approx_jacobian(itip * 6 + 0, icol)); - t.setY(mutation_approx_jacobian(itip * 6 + 1, icol)); - t.setZ(mutation_approx_jacobian(itip * 6 + 2, icol)); - quat_mul_vec(tip_frames[itip].rot, t, t); - mutation_approx_frames[itip][ivar].pos = t; - } - - { - Quaternion q; - q.setX(mutation_approx_jacobian(itip * 6 + 3, icol) * 0.5); - q.setY(mutation_approx_jacobian(itip * 6 + 4, icol) * 0.5); - q.setZ(mutation_approx_jacobian(itip * 6 + 5, icol) * 0.5); - q.setW(1.0); - quat_mul_quat(tip_frames[itip].rot, q, q); - q -= tip_frames[itip].rot; - mutation_approx_frames[itip][ivar].rot = q; - } - } - } - } - -// init second order approximators -#if USE_QUADRATIC_EXTRAPOLATION - { - if(mutation_approx_quadratics.size() < tip_count) mutation_approx_quadratics.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_quadratics[itip].resize(robot_model->getVariableCount()); - for(size_t itip = 0; itip < tip_count; itip++) - for(auto ivar : variable_indices) - mutation_approx_quadratics[itip][ivar].pos = Vector3(0, 0, 0); - for(auto ivar : variable_indices) - { - auto* var_joint_model = robot_model->getJointOfVariable(ivar); - if(var_joint_model->getMimic()) continue; - for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) - { - double scale = 1; - for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) - { - scale *= m->getMimicFactor(); - } - auto* link_model = joint_model->getChildLinkModel(); - switch(joint_model->getType()) - { - case moveit::core::JointModel::REVOLUTE: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto axis = joint_axis_list[joint_model->getJointIndex()]; - quat_mul_vec(link_frame.rot, axis, axis); - - auto v = link_frame.pos - tip_frame.pos; - v -= axis * v.dot(axis); - - mutation_approx_quadratics[itip][ivar].pos = v * 0.5 * (scale * scale); - } - continue; - } - default: - { - continue; - } - } - } - } - } -#endif - - // init mask - if(mutation_approx_mask.size() < tip_count) mutation_approx_mask.resize(tip_count); - if(mutation_approx_map.size() < tip_count) mutation_approx_map.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip].size() < robot_model->getVariableCount()) mutation_approx_mask[itip].resize(robot_model->getVariableCount()); - mutation_approx_map[itip].clear(); - for(size_t ii = 0; ii < variable_indices.size(); ii++) - // for(size_t ivar : variable_indices) - { - auto ivar = variable_indices[ii]; - auto& frame = mutation_approx_frames[itip][ivar]; - bool b = false; - b |= (frame.pos.x() != 0.0); - b |= (frame.pos.y() != 0.0); - b |= (frame.pos.z() != 0.0); - b |= (frame.rot.x() != 0.0); - b |= (frame.rot.y() != 0.0); - b |= (frame.rot.z() != 0.0); - mutation_approx_mask[itip][ivar] = b; - if(b) mutation_approx_map[itip].push_back(ii); - } - } - } - -#if FUNCTION_MULTIVERSIONING - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; - __m256d p = _mm256_load_pd(tip_frame_ptr + 0); - __m256d r = _mm256_load_pd(tip_frame_ptr + 4); - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); - __m256d ff = _mm256_set1_pd(variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); - } -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - } -#endif - auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm256_store_pd(tip_mutation_ptr + 0, p); - _mm256_store_pd(tip_mutation_ptr + 4, r); - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; - __m128d pxy = _mm_load_pd(tip_frame_ptr + 0); - __m128d pzw = _mm_load_sd(tip_frame_ptr + 2); - __m128d rxy = _mm_load_pd(tip_frame_ptr + 4); - __m128d rzw = _mm_load_pd(tip_frame_ptr + 6); - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); - __m128d ff = _mm_set1_pd(variable_delta); - pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); - pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); - rxy = _mm_add_pd(rxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4))); - rzw = _mm_add_pd(rzw, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6))); - } -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m128d ff = _mm_set1_pd(variable_delta * variable_delta); - pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); - pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); - } -#endif - auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm_store_pd(tip_mutation_ptr + 0, pxy); - _mm_store_sd(tip_mutation_ptr + 2, pzw); - _mm_store_pd(tip_mutation_ptr + 4, rxy); - _mm_store_pd(tip_mutation_ptr + 6, rzw); - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) -#endif - void - computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - - const Frame& tip_frame = input[itip]; - - double px = tip_frame.pos.x(); - double py = tip_frame.pos.y(); - double pz = tip_frame.pos.z(); - - double rx = tip_frame.rot.x(); - double ry = tip_frame.rot.y(); - double rz = tip_frame.rot.z(); - double rw = tip_frame.rot.w(); - - px += joint_delta.pos.x() * variable_delta; - py += joint_delta.pos.y() * variable_delta; - pz += joint_delta.pos.z() * variable_delta; - -#if USE_QUADRATIC_EXTRAPOLATION - double variable_delta_sq = variable_delta * variable_delta; - px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; - py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; - pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; -#endif - - rx += joint_delta.rot.x() * variable_delta; - ry += joint_delta.rot.y() * variable_delta; - rz += joint_delta.rot.z() * variable_delta; - rw += joint_delta.rot.w() * variable_delta; - - auto& tip_mutation = output[itip]; - - tip_mutation.pos.setX(px); - tip_mutation.pos.setY(py); - tip_mutation.pos.setZ(pz); - - tip_mutation.rot.setX(rx); - tip_mutation.rot.setY(ry); - tip_mutation.rot.setZ(rz); - tip_mutation.rot.setW(rw); - } - } - -#if FUNCTION_MULTIVERSIONING - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames_aligned[itip]; - - const double* tip_frame_ptr = (const double*)&tip_frame; - __m256d p0 = _mm256_load_pd(tip_frame_ptr + 0); - __m256d r0 = _mm256_load_pd(tip_frame_ptr + 4); - - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - auto p = p0; - auto r = r0; - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); - } - -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - } -#endif - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm256_store_pd(tip_mutation_ptr + 0, p); - _mm256_store_pd(tip_mutation_ptr + 4, r); - } - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames_aligned[itip]; - - const double* tip_frame_ptr = (const double*)&tip_frame; - __m128d pxy0 = _mm_load_pd(tip_frame_ptr + 0); - __m128d pzw0 = _mm_load_sd(tip_frame_ptr + 2); - __m128d rxy0 = _mm_load_pd(tip_frame_ptr + 4); - __m128d rzw0 = _mm_load_pd(tip_frame_ptr + 6); - - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - auto pxy = pxy0; - auto pzw = pzw0; - auto rxy = rxy0; - auto rzw = rzw0; - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); - __m128d ff = _mm_set1_pd(variable_delta); - pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); - pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); - rxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4)), rxy); - rzw = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6)), rzw); - } - -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m128d ff = _mm_set1_pd(variable_delta * variable_delta); - pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); - pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); - } -#endif - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm_store_pd(tip_mutation_ptr + 0, pxy); - _mm_store_sd(tip_mutation_ptr + 2, pzw); - _mm_store_pd(tip_mutation_ptr + 4, rxy); - _mm_store_pd(tip_mutation_ptr + 6, rzw); - } - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) -#endif - void - computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames[itip]; - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - double px = tip_frame.pos.x(); - double py = tip_frame.pos.y(); - double pz = tip_frame.pos.z(); - - double rx = tip_frame.rot.x(); - double ry = tip_frame.rot.y(); - double rz = tip_frame.rot.z(); - double rw = tip_frame.rot.w(); - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - px += joint_deltas[variable_index].pos.x() * variable_delta; - py += joint_deltas[variable_index].pos.y() * variable_delta; - pz += joint_deltas[variable_index].pos.z() * variable_delta; - -#if USE_QUADRATIC_EXTRAPOLATION - double variable_delta_sq = variable_delta * variable_delta; - px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; - py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; - pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; -#endif - - rx += joint_deltas[variable_index].rot.x() * variable_delta; - ry += joint_deltas[variable_index].rot.y() * variable_delta; - rz += joint_deltas[variable_index].rot.z() * variable_delta; - rw += joint_deltas[variable_index].rot.w() * variable_delta; - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - - tip_mutation.pos.setX(px); - tip_mutation.pos.setY(py); - tip_mutation.pos.setZ(pz); - - tip_mutation.rot.setX(rx); - tip_mutation.rot.setY(ry); - tip_mutation.rot.setZ(rz); - tip_mutation.rot.setW(rw); - } - } - } -}; - -#if 0 -class RobotFK_Mutator_2 : public RobotFK_Mutator -{ - struct JointApprox - { - ssize_t mutation_index; - ssize_t variable_index; - Vector3 delta_position; - Vector3 delta_rotation; - size_t link_index; - size_t parent_link_index; - Vector3 link_position; - }; - std::vector joint_approximators; - - struct LinkApprox - { - Vector3 position; - Vector3 rotation; - }; - std::vector link_approximators; - - std::vector link_approx_mask; - - std::vector> variable_to_approximator_index_map; - -public: - RobotFK_Mutator_2(moveit::core::RobotModelConstPtr model) - : RobotFK_Mutator(model) - { - } - - void initializeMutationApproximator(const std::vector& variable_indices) - { - RobotFK_Mutator::initializeMutationApproximator(variable_indices); - - BLOCKPROFILER("initializeMutationApproximator tree"); - - auto tip_count = tip_names.size(); - - link_approximators.resize(robot_model->getLinkModelCount() + 1); - for(auto& l : link_approximators) - { - l.position = Vector3(0, 0, 0); - l.rotation = Vector3(0, 0, 0); - } - - link_approx_mask.clear(); - link_approx_mask.resize(robot_model->getLinkModelCount(), 0); - - joint_approximators.clear(); - - // build joint approximators - for(size_t imut = 0; imut < variable_indices.size(); imut++) - { - size_t ivar = variable_indices[imut]; - auto* joint_model = robot_model->getJointOfVariable(ivar); - { - auto* link_model = joint_model->getChildLinkModel(); - // if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = imut; - joint.variable_index = ivar; - joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - for(auto* mimic_joint_model : joint_model->getMimicRequests()) - { - // LOG("mimic", mimic_joint_model); - auto* joint_model = mimic_joint_model; - auto* link_model = joint_model->getChildLinkModel(); - // if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = imut; - joint.variable_index = ivar; - joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - } - - for(auto& joint : joint_approximators) - { - // size_t imut = joint.mutation_index; - // size_t ivar = joint.variable_index; - auto* link_model = robot_model->getLinkModel(joint.link_index); - auto* joint_model = link_model->getParentJointModel(); - joint.delta_position = Vector3(0, 0, 0); - joint.delta_rotation = Vector3(0, 0, 0); - auto& link_frame = global_frames[link_model->getLinkIndex()]; - switch(joint_model->getType()) - { - case moveit::core::JointModel::REVOLUTE: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_rotation); - break; - case moveit::core::JointModel::PRISMATIC: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_position); - break; - } - for(auto* j = joint_model; j->getMimic(); j = j->getMimic()) - { - joint.delta_rotation *= j->getMimicFactor(); - joint.delta_position *= j->getMimicFactor(); - break; - } - } - - // continue extrapolation to tip frames, if not already done - for(auto* link_model : tip_links) - { - if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = 0; - joint.variable_index = 0; - joint.link_index = link_model->getLinkIndex(); - joint.delta_position = Vector3(0, 0, 0); - joint.delta_rotation = Vector3(0, 0, 0); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - - std::sort(joint_approximators.begin(), joint_approximators.end(), [](const JointApprox& a, const JointApprox& b) { return a.link_index < b.link_index; }); - - for(auto& joint : joint_approximators) - { - for(auto* link_model = robot_model->getLinkModel(joint.link_index); link_model;) - { - if(link_approx_mask[link_model->getLinkIndex()] >= 2) - { - joint.parent_link_index = link_model->getLinkIndex(); - joint.link_position = global_frames[joint.link_index].pos - global_frames[joint.parent_link_index].pos; - link_approx_mask[joint.link_index] = 2; - break; - } - link_model = link_model->getParentLinkModel(); - if(link_model == 0) - { - joint.parent_link_index = robot_model->getLinkModelCount(); - joint.link_position = global_frames[joint.link_index].pos; - link_approx_mask[joint.link_index] = 2; - break; - } - } - } - - variable_to_approximator_index_map.resize(robot_model->getVariableCount()); - for(auto& l : variable_to_approximator_index_map) - l.clear(); - for(size_t approximator_index = 0; approximator_index < joint_approximators.size(); approximator_index++) - { - if(joint_approximators[approximator_index].variable_index < 0) continue; - variable_to_approximator_index_map[joint_approximators[approximator_index].variable_index].push_back(approximator_index); - } - } - - void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) - { - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - for(auto& joint : joint_approximators) - { - double dvar; - /*if(joint.mutation_index < 0) - dvar = 0; - else*/ - dvar = mutation_values[imutation][joint.mutation_index] - variables[joint.variable_index]; - - Vector3 dpos = joint.delta_position * dvar + joint.link_position; - - auto& parent = link_approximators[joint.parent_link_index]; - - // if(dpos.x() || dpos.y() || dpos.z()) - { - Vector3 dpos1 = Vector3(parent.rotation.y() * dpos.z() - parent.rotation.z() * dpos.y(), parent.rotation.z() * dpos.x() - parent.rotation.x() * dpos.z(), parent.rotation.x() * dpos.y() - parent.rotation.y() * dpos.x()); - - /*double xx = 0.5 * parent.rotation.x() * parent.rotation.x(); - double yy = 0.5 * parent.rotation.y() * parent.rotation.y(); - double zz = 0.5 * parent.rotation.z() * parent.rotation.z(); - - Vector3 dpos2 = Vector3( - dpos.x() * (yy + zz), - dpos.y() * (xx + zz), - dpos.z() * (xx + yy) - );*/ - - dpos += dpos1; - } - - // dpos -= dpos2; - - auto& link = link_approximators[joint.link_index]; - link.position = parent.position + dpos; - link.rotation = parent.rotation + joint.delta_rotation * dvar; - } - - for(size_t itip = 0; itip < tip_count; itip++) - { - const Frame& tip_frame = tip_frames[itip]; - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - auto& link = link_approximators[tip_links[itip]->getLinkIndex()]; - tip_mutation.pos = link.position; - - tip_mutation.rot = tf::Quaternion(link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, 1.0) * tip_frame.rot; - - /*tip_mutation.rot = tf::Quaternion( - link.rotation.x() * 0.5, - link.rotation.y() * 0.5, - link.rotation.z() * 0.5, - 1.0 - - link.rotation.length2() * 0.125 - ) * tip_frame.rot;*/ - } - } - } -}; - -typedef RobotFK_Mutator_2 RobotFK; - -#else - -typedef RobotFK_Mutator RobotFK; - -#endif - -#endif - -// for comparison -class RobotFK_MoveIt -{ - moveit::core::RobotModelConstPtr robot_model; - moveit::core::RobotState robot_state; - std::vector tipFrames; - std::vector tipLinks; - std::vector jj; - -public: - RobotFK_MoveIt(moveit::core::RobotModelConstPtr model) - : robot_model(model) - , robot_state(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - tipFrames.resize(tip_link_indices.size()); - tipLinks.resize(tip_link_indices.size()); - for(size_t i = 0; i < tip_link_indices.size(); i++) - tipLinks[i] = robot_model->getLinkModel(tip_link_indices[i]); - } - void applyConfiguration(const std::vector& jj0) - { - BLOCKPROFILER("RobotFK_MoveIt applyConfiguration"); - jj = jj0; - robot_model->interpolate(jj0.data(), jj0.data(), 0.5, jj.data()); // force mimic update - robot_state.setVariablePositions(jj); - robot_state.update(); - for(size_t i = 0; i < tipFrames.size(); i++) - tipFrames[i] = Frame(robot_state.getGlobalLinkTransform(tipLinks[i])); - } - inline void incrementalBegin(const std::vector& jj) {} - inline void incrementalEnd() {} - const Frame& getTipFrame(size_t fi) const { return tipFrames[fi]; } - const std::vector& getTipFrames() const { return tipFrames; } -}; -} diff --git a/src/bio_ik-kinetic-support/src/goal_types.cpp b/src/bio_ik-kinetic-support/src/goal_types.cpp deleted file mode 100644 index a642a58..0000000 --- a/src/bio_ik-kinetic-support/src/goal_types.cpp +++ /dev/null @@ -1,269 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include - -#include -#include - -#include - -namespace bio_ik -{ - -void TouchGoal::describe(GoalContext& context) const -{ - LinkGoalBase::describe(context); - auto* robot_model = &context.getRobotModel(); - { - static std::map collision_cache; - if(collision_cache.find(robot_model) == collision_cache.end()) collision_cache[&context.getRobotModel()] = new CollisionModel(); - collision_model = collision_cache[robot_model]; - collision_model->collision_links.resize(robot_model->getLinkModelCount()); - } - link_model = robot_model->getLinkModel(this->getLinkName()); - size_t link_index = link_model->getLinkIndex(); - auto touch_goal_normal = normal.normalized(); - // auto fbrot = fb.rot.normalized(); - auto& collision_link = collision_model->collision_links[link_index]; - if(!collision_link.initialized) - { - collision_link.initialized = true; - collision_link.shapes.resize(link_model->getShapes().size()); - for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++) - { - collision_link.shapes[shape_index] = std::make_shared(); - auto& s = *collision_link.shapes[shape_index]; - s.frame = Frame(link_model->getCollisionOriginTransforms()[shape_index]); - auto* shape = link_model->getShapes()[shape_index].get(); - // LOG(link_model->getName(), shape_index, link_model->getShapes().size(), typeid(*shape).name()); - if(auto* mesh = dynamic_cast(shape)) - { - struct : bodies::ConvexMesh - { - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - void init(const shapes::Shape* shape) - { - type_ = shapes::MESH; - scaled_vertices_ = 0; - { - static std::mutex mutex; - std::lock_guard lock(mutex); - setDimensions(shape); - } - for(auto& v : mesh_data_->vertices_) - points.emplace_back(v.x(), v.y(), v.z()); - for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++) - { - polygons.push_back(3); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 0]); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 1]); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 2]); - } - for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++) - { - auto plane_index = mesh_data_->plane_for_triangle_[triangle_index]; - auto plane = mesh_data_->planes_[plane_index]; - plane_normals.emplace_back(plane.x(), plane.y(), plane.z()); - plane_dis.push_back(plane.w()); - } - } - } convex; - convex.init(mesh); - s.points = convex.points; - s.polygons = convex.polygons; - s.plane_normals = convex.plane_normals; - s.plane_dis = convex.plane_dis; - - // auto* fcl = new fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); - - // workaround for fcl::Convex initialization bug - auto* fcl = (fcl::Convex*)::operator new(sizeof(fcl::Convex)); - fcl->num_points = s.points.size(); - fcl = new(fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); - - s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(fcl, link_model, shape_index)); - s.edges.resize(s.points.size()); - std::vector> edge_sets(s.points.size()); - for(size_t edge_index = 0; edge_index < fcl->num_edges; edge_index++) - { - auto edge = fcl->edges[edge_index]; - if(edge_sets[edge.first].find(edge.second) == edge_sets[edge.first].end()) - { - edge_sets[edge.first].insert(edge.second); - s.edges[edge.first].push_back(edge.second); - } - if(edge_sets[edge.second].find(edge.first) == edge_sets[edge.second].end()) - { - edge_sets[edge.second].insert(edge.first); - s.edges[edge.second].push_back(edge.first); - } - } - for(auto& p : s.points) - s.vertices.emplace_back(p[0], p[1], p[2]); - } - else - { - s.geometry = collision_detection::createCollisionGeometry(link_model->getShapes()[shape_index], link_model, shape_index); - } - // LOG("b"); - } - // getchar(); - } -} - -double TouchGoal::evaluate(const GoalContext& context) const -{ - double dmin = DBL_MAX; - context.getTempVector().resize(1); - auto& last_collision_vertex = context.getTempVector()[0]; - auto& fb = context.getLinkFrame(); - size_t link_index = link_model->getLinkIndex(); - auto& collision_link = collision_model->collision_links[link_index]; - for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++) - { - if(!collision_link.shapes[shape_index]->geometry) continue; - auto* shape = link_model->getShapes()[shape_index].get(); - // LOG(shape_index, typeid(*shape).name()); - if(auto* mesh = dynamic_cast(shape)) - { - auto& s = collision_link.shapes[shape_index]; - double d = DBL_MAX; - auto goal_normal = normal; - quat_mul_vec(fb.rot.inverse(), goal_normal, goal_normal); - quat_mul_vec(s->frame.rot.inverse(), goal_normal, goal_normal); - /*{ - size_t array_index = 0; - for(size_t vertex_index = 0; vertex_index < mesh->vertex_count; vertex_index++) - { - double dot_x = mesh->vertices[array_index++] * goal_normal.x(); - double dot_y = mesh->vertices[array_index++] * goal_normal.y(); - double dot_z = mesh->vertices[array_index++] * goal_normal.z(); - double e = dot_x + dot_y + dot_z; - if(e < d) d = e; - } - }*/ - if(mesh->vertex_count > 0) - { - size_t vertex_index = last_collision_vertex; - double vertex_dot_normal = goal_normal.dot(s->vertices[vertex_index]); - // size_t loops = 0; - while(true) - { - bool repeat = false; - for(auto vertex_index_2 : s->edges[vertex_index]) - { - auto vertex_dot_normal_2 = goal_normal.dot(s->vertices[vertex_index_2]); - if(vertex_dot_normal_2 < vertex_dot_normal) - { - vertex_index = vertex_index_2; - vertex_dot_normal = vertex_dot_normal_2; - repeat = true; - break; - } - } - if(!repeat) break; - // loops++; - } - // LOG_VAR(loops); - d = vertex_dot_normal; - last_collision_vertex = vertex_index; - } - d -= normal.dot(position - fb.pos); - // ROS_INFO("touch goal"); - if(d < dmin) dmin = d; - } - else - { - double offset = 10000; - static fcl::Sphere shape1(offset); - fcl::DistanceRequest request; - fcl::DistanceResult result; - auto pos1 = position - normal * offset * 2; - auto* shape2 = collision_link.shapes[shape_index]->geometry->collision_geometry_.get(); - auto frame2 = Frame(fb.pos, fb.rot.normalized()) * collision_link.shapes[shape_index]->frame; - double d = fcl::distance(&shape1, fcl::Transform3f(fcl::Vec3f(pos1.x(), pos1.y(), pos1.z())), shape2, fcl::Transform3f(fcl::Quaternion3f(frame2.rot.w(), frame2.rot.x(), frame2.rot.y(), frame2.rot.z()), fcl::Vec3f(frame2.pos.x(), frame2.pos.y(), frame2.pos.z())), request, result); - d -= offset; - if(d < dmin) dmin = d; - } - } - return dmin * dmin; -} - -void BalanceGoal::describe(GoalContext& context) const -{ - Goal::describe(context); - balance_infos.clear(); - double total = 0.0; - for(auto& link_name : context.getRobotModel().getLinkModelNames()) - { - auto link_urdf = context.getRobotModel().getURDF()->getLink(link_name); - if(!link_urdf) continue; - if(!link_urdf->inertial) continue; - const auto& center_urdf = link_urdf->inertial->origin.position; - tf::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z); - double mass = link_urdf->inertial->mass; - if(!(mass > 0)) continue; - balance_infos.emplace_back(); - balance_infos.back().center = center; - balance_infos.back().weight = mass; - total += mass; - context.addLink(link_name); - } - for(auto& b : balance_infos) - { - b.weight /= total; - } -} - -double BalanceGoal::evaluate(const GoalContext& context) const -{ - tf::Vector3 center = tf::Vector3(0, 0, 0); - for(size_t i = 0; i < balance_infos.size(); i++) - { - auto& info = balance_infos[i]; - auto& frame = context.getLinkFrame(i); - auto c = info.center; - quat_mul_vec(frame.rot, c, c); - c += frame.pos; - center += c * info.weight; - } - center -= target_; - center -= axis_ * axis_.dot(center); - return center.length2(); -} -} diff --git a/src/bio_ik-kinetic-support/src/ik_base.h b/src/bio_ik-kinetic-support/src/ik_base.h deleted file mode 100644 index 607bd5a..0000000 --- a/src/bio_ik-kinetic-support/src/ik_base.h +++ /dev/null @@ -1,214 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include - -#include "forward_kinematics.h" -#include "problem.h" -#include "utils.h" -#include - -#include - -namespace bio_ik -{ - -struct Random -{ - // std::mt19937 rng; - std::minstd_rand rng; - // std::ranlux24 rng; - // std::knuth_b rng; - // std::default_random_engine rng; - - inline double random() { return std::uniform_real_distribution(0, 1)(rng); } - - inline std::size_t random_index(std::size_t s) { return std::uniform_int_distribution(0, s - 1)(rng); } - - std::normal_distribution normal_distribution; - inline double random_gauss() { return normal_distribution(rng); } - - inline double random(double min, double max) { return random() * (max - min) + min; } - - template inline e& random_element(std::vector& l) { return l[random_index(l.size())]; } - - template inline const e& random_element(const std::vector& l) { return l[random_index(l.size())]; } - - XORShift64 _xorshift; - inline size_t fast_random_index(size_t mod) { return _xorshift() % mod; } - template inline const T& fast_random_element(const std::vector& v) { return v[fast_random_index(v.size())]; } - - static const size_t random_buffer_size = 1024 * 1024 * 8; - - const double* make_random_buffer() - { - static std::vector buf; - buf.resize(random_buffer_size); - for(auto& r : buf) - r = random(); - return buf.data(); - } - const double* random_buffer; - size_t random_buffer_index; - inline double fast_random() - { - double r = random_buffer[random_buffer_index & (random_buffer_size - 1)]; - random_buffer_index++; - return r; - } - - const double* make_random_gauss_buffer() - { - // LOG("make_random_gauss_buffer"); - static std::vector buf; - buf.resize(random_buffer_size); - for(auto& r : buf) - r = random_gauss(); - return buf.data(); - } - const double* random_gauss_buffer; - size_t random_gauss_index; - inline double fast_random_gauss() - { - double r = random_gauss_buffer[random_gauss_index & (random_buffer_size - 1)]; - random_gauss_index++; - return r; - } - inline const double* fast_random_gauss_n(size_t n) - { - size_t i = random_gauss_index; - random_gauss_index += n; - if(random_gauss_index >= random_buffer_size) i = 0, random_gauss_index = n; - return random_gauss_buffer + i; - } - - Random() - : rng(std::random_device()()) - { - random_buffer = make_random_buffer(); - random_buffer_index = _xorshift(); - random_gauss_buffer = make_random_gauss_buffer(); - random_gauss_index = _xorshift(); - } -}; - -struct IKBase : Random -{ - IKParams params; - RobotFK model; - RobotInfo modelInfo; - int thread_index; - Problem problem; - std::vector null_tip_frames; - volatile int canceled; - - virtual void step() = 0; - - virtual const std::vector& getSolution() const = 0; - - virtual void setParams(const IKParams& p) {} - - IKBase(const IKParams& p) - : model(p.robot_model) - , modelInfo(p.robot_model) - , params(p) - { - setParams(p); - } - virtual ~IKBase() {} - - virtual void initialize(const Problem& problem) - { - this->problem = problem; - this->problem.initialize2(); - model.initialize(problem.tip_link_indices); - // active_variables = problem.active_variables; - null_tip_frames.resize(problem.tip_link_indices.size()); - } - - double computeSecondaryFitnessActiveVariables(const double* active_variable_positions) { return problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); } - - double computeSecondaryFitnessAllVariables(const std::vector& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); } - - double computeFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } - - double computeFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } - - double computeCombinedFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) - { - double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); - return ret; - } - - double computeCombinedFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) - { - double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); - return ret; - } - - bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); } - - bool checkSolution(const std::vector& variable_positions, const std::vector& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); } - - std::vector temp_active_variable_positions; - - double* extractActiveVariables(const std::vector& variable_positions) - { - temp_active_variable_positions.resize(problem.active_variables.size()); - for(size_t i = 0; i < temp_active_variable_positions.size(); i++) - temp_active_variable_positions[i] = variable_positions[problem.active_variables[i]]; - return temp_active_variable_positions.data(); - } - - double computeFitness(const std::vector& variable_positions, const std::vector& tip_frames) { return computeFitnessActiveVariables(tip_frames, extractActiveVariables(variable_positions)); } - - double computeFitness(const std::vector& variable_positions) - { - model.applyConfiguration(variable_positions); - return computeFitness(variable_positions, model.getTipFrames()); - } - - virtual size_t concurrency() const { return 1; } -}; - -typedef IKBase IKSolver; - -typedef Factory IKFactory; -} diff --git a/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp b/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp deleted file mode 100644 index 790483d..0000000 --- a/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp +++ /dev/null @@ -1,257 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -#include "cppoptlib/boundedproblem.h" -#include "cppoptlib/meta.h" -#include "cppoptlib/problem.h" - -namespace bio_ik -{ - -/* -struct IKOptLibProblem : cppoptlib::Problem -{ - IKBase* ik; - std::vector fk_values; - IKOptLibProblem(IKBase* ik) : ik(ik) - { - } - void initialize() - { - // set all variables to initial guess, including inactive ones - fk_values = ik->problem.initial_guess; - } - double value(const TVector& x) - { - // fill in active variables and compute fitness - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) fk_values[ik->problem.active_variables[i]] = x[i]; - return ik->computeFitness(fk_values); - } - bool callback(const cppoptlib::Criteria& state, const TVector& x) - { - // check ik timeout - return ros::WallTime::now().toSec() < ik->problem.timeout; - } -}; -*/ - -// problem description for cppoptlib -struct IKOptLibProblem : cppoptlib::BoundedProblem -{ - IKBase* ik; - std::vector fk_values; - IKOptLibProblem(IKBase* ik) - : cppoptlib::BoundedProblem(TVector(ik->problem.active_variables.size()), TVector(ik->problem.active_variables.size())) - , ik(ik) - { - // init bounds - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) - { - m_lowerBound[i] = ik->modelInfo.getMin(ik->problem.active_variables[i]); - m_upperBound[i] = ik->modelInfo.getMax(ik->problem.active_variables[i]); - // m_lowerBound[i] = fmax(m_lowerBound[i], -100); - // m_upperBound[i] = fmin(m_upperBound[i], 100); - } - } - void initialize() - { - // set all variables to initial guess, including inactive ones - fk_values = ik->problem.initial_guess; - } - double value(const TVector& x) - { - // fill in active variables and compute fitness - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) - fk_values[ik->problem.active_variables[i]] = x[i]; - // for(size_t i = 0; i < ik->active_variables.size(); i++) LOG(i, x[i]); LOG(""); - // for(size_t i = 0; i < ik->active_variables.size(); i++) std::cerr << ((void*)*(uint64_t*)&x[i]) << " "; std::cerr << std::endl; - // size_t h = 0; for(size_t i = 0; i < ik->active_variables.size(); i++) h ^= (std::hash()(x[i]) << i); LOG((void*)h); - return ik->computeFitness(fk_values); - } - bool callback(const cppoptlib::Criteria& state, const TVector& x) - { - // check ik timeout - return ros::WallTime::now().toSec() < ik->problem.timeout; - } -}; - -// ik solver using cppoptlib -template struct IKOptLib : IKBase -{ - // current solution - std::vector solution, best_solution, temp; - - // cppoptlib solver - std::shared_ptr solver; - - // cppoptlib problem description - IKOptLibProblem f; - - // reset flag - bool reset; - - // stop criteria - cppoptlib::Criteria crit; - - IKOptLib(const IKParams& p) - : IKBase(p) - , f(this) - { - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - // set initial guess - solution = problem.initial_guess; - - // randomize if more than 1 thread - reset = false; - if(thread_index > 0) reset = true; - - // init best solution - best_solution = solution; - - // initialize cppoptlib problem description - f = IKOptLibProblem(this); - f.initialize(); - - // init stop criteria (timeout will be handled explicitly) - crit = cppoptlib::Criteria::defaults(); - // crit.iterations = SIZE_MAX; - crit.gradNorm = 1e-10; - // p.node_handle.param("optlib_stop", crit.gradNorm, crit.gradNorm); - - if(!solver) solver = std::make_shared(); - } - - const std::vector& getSolution() const { return best_solution; } - - void step() - { - // set stop criteria - solver->setStopCriteria(crit); - - // random reset if stuck (and if random resets are enabled) - if(reset) - { - // LOG("RESET"); - reset = false; - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - // set to current positions - temp = solution; - typename SOLVER::TVector x(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - x[i] = temp[problem.active_variables[i]]; - - // solve - solver->minimize(f, x); - - // get results - for(size_t i = 0; i < problem.active_variables.size(); i++) - temp[problem.active_variables[i]] = x[i]; - - // update solution - if(computeFitness(temp) < computeFitness(solution)) - { - solution = temp; - } - else - { - if(reset_if_stuck) reset = true; - } - - // update best solution - if(computeFitness(solution) < computeFitness(best_solution)) - { - best_solution = solution; - } - } - - size_t concurrency() const { return threads; } -}; -} - -static std::string mkoptname(std::string name, int reset, int threads) -{ - name = "optlib_" + name; - if(reset) name += "_r"; - if(threads > 1) name += "_" + std::to_string(threads); - return name; -} - -#define IKCPPOPTX(n, t, reset, threads) static bio_ik::IKFactory::Class, reset, threads>> ik_optlib_##t##reset##threads(mkoptname(n, reset, threads)); - -//#define IKCPPOPT(n, t) static bio_ik::IKFactory::Class>> ik_optlib_##t(n) - -#define IKCPPOPT(n, t) \ - IKCPPOPTX(n, t, 0, 1) \ - IKCPPOPTX(n, t, 0, 2) \ - IKCPPOPTX(n, t, 0, 4) \ - IKCPPOPTX(n, t, 1, 1) \ - IKCPPOPTX(n, t, 1, 2) \ - IKCPPOPTX(n, t, 1, 4) - -#include "cppoptlib/solver/bfgssolver.h" -IKCPPOPT("bfgs", BfgsSolver); - -//#include "cppoptlib/solver/cmaesbsolver.h" -// IKCPPOPT("cmaesb", CMAesBSolver); - -//#include "cppoptlib/solver/cmaessolver.h" -// IKCPPOPT("cmaes", CMAesSolver); - -#include "cppoptlib/solver/conjugatedgradientdescentsolver.h" -IKCPPOPT("cgd", ConjugatedGradientDescentSolver); - -#include "cppoptlib/solver/gradientdescentsolver.h" -IKCPPOPT("gd", GradientDescentSolver); - -#include "cppoptlib/solver/lbfgsbsolver.h" -IKCPPOPT("lbfgsb", LbfgsbSolver); - -#include "cppoptlib/solver/lbfgssolver.h" -IKCPPOPT("lbfgs", LbfgsSolver); - -#include "cppoptlib/solver/neldermeadsolver.h" -IKCPPOPT("nm", NelderMeadSolver); - -#include "cppoptlib/solver/newtondescentsolver.h" -IKCPPOPT("nd", NewtonDescentSolver); diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp deleted file mode 100644 index 5435e10..0000000 --- a/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp +++ /dev/null @@ -1,561 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -struct IKEvolution1 : IKBase -{ - struct Individual - { - std::vector genes; - std::vector gradients; - double extinction; - double fitness; - }; - - class HeuristicErrorTree - { - size_t variable_count, tip_count; - std::vector table; - std::vector chain_lengths; - std::vector> chain_lengths_2; - - public: - HeuristicErrorTree() {} - HeuristicErrorTree(moveit::core::RobotModelConstPtr robot_model, const std::vector& tip_names) - { - tip_count = tip_names.size(); - variable_count = robot_model->getVariableCount(); - table.resize(tip_count * variable_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - auto* joint_model = link_model->getParentJointModel(); - size_t v1 = joint_model->getFirstVariableIndex(); - size_t vn = joint_model->getVariableCount(); - for(size_t variable_index = v1; variable_index < v1 + vn; variable_index++) - table[variable_index * tip_count + tip_index] = 1; - } - } - for(size_t variable_index = 0; variable_index < variable_count; variable_index++) - { - double sum = 0; - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - sum += table[variable_index * tip_count + tip_index]; - if(sum > 0) - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - table[variable_index * tip_count + tip_index] /= sum; - } - - chain_lengths.resize(tip_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - double chain_length = 0; - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - chain_length += Frame(link_model->getJointOriginTransform()).pos.length(); - } - chain_lengths[tip_index] = chain_length; - } - - chain_lengths_2.resize(tip_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - double chain_length = 0; - chain_lengths_2[tip_index].resize(variable_count, 0.0); - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - - auto* joint_model = link_model->getParentJointModel(); - int vmin = joint_model->getFirstVariableIndex(); - int vmax = vmin + joint_model->getVariableCount(); - for(int vi = vmin; vi < vmax; vi++) - chain_lengths_2[tip_index][vi] = chain_length; - chain_length += Frame(link_model->getJointOriginTransform()).pos.length(); - } - } - } - inline double getInfluence(size_t variable_index, size_t tip_index) const { return table[variable_index * tip_count + tip_index]; } - inline double getChainLength(size_t tip_index) const { return chain_lengths[tip_index]; } - inline double getJointVariableChainLength(size_t tip_index, size_t variable_index) const { return chain_lengths_2[tip_index][variable_index]; } - }; - - HeuristicErrorTree heuristicErrorTree; - std::vector solution; - std::vector population; - int populationSize, eliteCount; - std::vector tempPool; - std::vector tempOffspring; - std::vector initialGuess; - - bool opt_no_wipeout; - - bool linear_fitness; - - void setParams(const IKParams& p) - { - opt_no_wipeout = p.opt_no_wipeout; - populationSize = p.population_size; - eliteCount = p.elite_count; - linear_fitness = p.linear_fitness; - } - - bool in_final_adjustment_loop; - - template inline t select(const std::vector& v) - { - // FNPROFILER(); - linear_int_distribution d(v.size()); - size_t index = d(rng); - return v[index]; - } - - inline double clip(double v, size_t i) { return modelInfo.clip(v, i); } - - inline double getMutationStrength(size_t i, const Individual& parentA, const Individual& parentB) - { - double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double span = modelInfo.getSpan(i); - return span * extinction; - } - - double computeAngularScale(size_t tip_index, const Frame& tip_frame) - { - double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI; - return angular_scale; - // return 1; - /*double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI; - //double angular_scale = heuristicErrorTree.getChainLength(tip_index) * (1.0 / M_PI); - if(opt_angular_scale_full_circle) angular_scale *= 0.5; - return angular_scale;*/ - } - - double getHeuristicError(size_t variable_index, bool balanced) - { - // return 1; - - double heuristic_error = 0; - // for(int tip_index = 0; tip_index < tipObjectives.size(); tip_index++) - for(int tip_index = 0; tip_index < problem.goals.size(); tip_index++) - { - double influence = heuristicErrorTree.getInfluence(variable_index, tip_index); - if(influence == 0) continue; - - // const auto& ta = tipObjectives[tip_index]; - const auto& ta = problem.goals[tip_index].frame; - const auto& tb = model.getTipFrame(tip_index); - - double length = heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index); - - // LOG_ALWAYS("a", heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index)); - - // double length = model.getJointVariableFrame(variable_index).pos.distance(model.getTipFrame(tip_index).pos); if(length <= 0.000000001) length = 0; - - // LOG_ALWAYS("b", length); - - if(modelInfo.isPrismatic(variable_index)) - { - // heuristic_error += ta.pos.distance(tb.pos) * influence; - // if(length) heuristic_error += ta.rot.angle(tb.rot) * length * influence; - - if(length) - { - heuristic_error += ta.pos.distance(tb.pos) * influence * 0.5; - heuristic_error += ta.rot.angle(tb.rot) * length * influence * 0.5; - } - else - { - heuristic_error += ta.pos.distance(tb.pos) * influence; - } - } - - if(modelInfo.isRevolute(variable_index)) - { - // if(length) heuristic_error += ta.pos.distance(tb.pos) / length * influence; - // heuristic_error += ta.rot.angle(tb.rot) * influence; - - if(length) - { - heuristic_error += ta.pos.distance(tb.pos) / length * influence * 0.5; - heuristic_error += ta.rot.angle(tb.rot) * influence * 0.5; - } - else - { - heuristic_error += ta.rot.angle(tb.rot) * influence; - } - - // double d = 0.0; - // if(length) d = std::max(d, ta.pos.distance(tb.pos) / length); - // d = std::max(d, ta.rot.angle(tb.rot)); - // heuristic_error += d * influence; - } - } - // heuristic_error *= 0.5; - // LOG_ALWAYS(heuristic_error); - return heuristic_error; - } - - bool in_adjustment_2, in_get_solution_fitness; - - void reroll(Individual& offspring) - { - FNPROFILER(); - // for(size_t i = 0; i < offspring.genes.size(); i++) - for(auto i : problem.active_variables) - { - offspring.genes[i] = random(modelInfo.getMin(i), modelInfo.getMax(i)); - - offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random(0.0, 0.1)); - - offspring.gradients[i] = 0; - } - offspring.fitness = computeFitness(offspring.genes, false); - } - - double computeFitness(const std::vector& genes, bool balanced) - { - if(linear_fitness) - { - model.applyConfiguration(genes); - double fitness_sum = 0.0; - for(size_t goal_index = 0; goal_index < problem.goals.size(); goal_index++) - { - const auto& ta = problem.goals[goal_index].frame; - const auto& tb = model.getTipFrame(problem.goals[goal_index].tip_index); - - double tdist = ta.pos.distance(tb.pos) / computeAngularScale(problem.goals[goal_index].tip_index, ta); - double rdist = ta.rot.angle(tb.rot); - - fitness_sum += mix(tdist, rdist, (balanced || in_final_adjustment_loop) ? 0.5 : random()); - } - return fitness_sum; - } - else - { - return IKBase::computeFitness(genes); - } - } - - bool checkWipeout() - { - FNPROFILER(); - auto& genes = population[0].genes; - // for(size_t i = 0; i < genes.size(); i++) - for(auto i : problem.active_variables) - { - double v0 = genes[i]; - double fitness = computeFitness(genes, true); - double heuristicError = getHeuristicError(i, true); - // double heuristicError = 0.001; - genes[i] = modelInfo.clip(v0 + random(0, heuristicError), i); - double incFitness = computeFitness(genes, true); - genes[i] = modelInfo.clip(v0 - random(0, heuristicError), i); - double decFitness = computeFitness(genes, true); - genes[i] = v0; - if(incFitness < fitness || decFitness < fitness) - { - // LOG("no wipeout"); - return false; - } - } - // LOG("wipeout 1"); - return true; - } - - void computeExtinctions() - { - double min = population.front().fitness; - double max = population.back().fitness; - for(size_t i = 0; i < populationSize; i++) - { - double grading = (double)i / (double)(populationSize - 1); - population[i].extinction = (population[i].fitness + min * (grading - 1)) / max; - } - } - - bool tryUpdateSolution() - { - FNPROFILER(); - double solutionFitness = computeFitness(solution, true); - double candidateFitness = computeFitness(population[0].genes, true); - // LOG_VAR(solutionFitness); - // LOG_VAR(candidateFitness); - if(candidateFitness < solutionFitness) - { - solution = population[0].genes; - // solution = initialGuess; - // for(auto i : problem.active_variables) - // solution[i] = population[0].genes[i]; - return true; - } - return false; - } - - double getMutationProbability(const Individual& parentA, const Individual& parentB) - { - double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double inverse = 1.0 / parentA.genes.size(); - return extinction * (1.0 - inverse) + inverse; - } - - void sortByFitness() - { - FNPROFILER(); - sort(population.begin(), population.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); - } - - double bounce(double v, int i) - { - double c = clip(v, i); - v = c - (v - c) * 2; - // v = c + c - v; - v = clip(v, i); - return v; - } - - void reproduce(Individual& offspring, const Individual& parentA, const Individual& parentB, const Individual& prototype) - { - FNPROFILER(); - for(size_t i = 0; i < offspring.genes.size(); i++) - // for(auto i : problem.active_variables) - { - offspring.genes[i] = mix(parentA.genes[i], parentB.genes[i], random()); - offspring.genes[i] += parentA.gradients[i] * random(); - offspring.genes[i] += parentB.gradients[i] * random(); - - double storage = offspring.genes[i]; - - if(random() < getMutationProbability(parentA, parentB)) offspring.genes[i] += random(-1, 1) * getMutationStrength(i, parentA, parentB); - // offspring.genes[i] += normal_random() * getMutationStrength(i, parentA, parentB); - - offspring.genes[i] += mix(random() * (0.5 * (parentA.genes[i] + parentB.genes[i]) - offspring.genes[i]), random() * (prototype.genes[i] - offspring.genes[i]), random()); - - // offspring.genes[i] = clip(offspring.genes[i], i); - - // offspring.genes[i] += fabs(offspring.genes[i] - storage) * offspring.genes[i] - (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5; - - // offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random() * 0.1 * fabs(offspring.genes[i] - storage) / modelInfo.getSpan(i)); - - offspring.genes[i] = clip(offspring.genes[i], i); - - // offspring.genes[i] = bounce(offspring.genes[i], i); - - offspring.gradients[i] = random() * offspring.gradients[i] + offspring.genes[i] - storage; - } - - offspring.fitness = computeFitness(offspring.genes, false); - } - - void exploit(Individual& individual) - { - FNPROFILER(); - - double fitness_sum = 0; - - // model.incrementalBegin(individual.genes); - - for(auto i : problem.active_variables) - { - double fitness = computeFitness(individual.genes, true); - - double heuristicError = getHeuristicError(i, true); - double v_0 = individual.genes[i]; - - double v_inc = clip(v_0 + random(0, heuristicError), i); - double v_dec = clip(v_0 - random(0, heuristicError), i); - - individual.genes[i] = v_inc; - double inc_fitness = computeFitness(individual.genes, true); - individual.genes[i] = v_dec; - double dec_fitness = computeFitness(individual.genes, true); - - if(inc_fitness < fitness && inc_fitness <= dec_fitness) - { - individual.genes[i] = v_inc; - individual.gradients[i] = v_0 * random() + v_inc - v_0; - fitness_sum += inc_fitness; - } - else if(dec_fitness < fitness && dec_fitness <= inc_fitness) - { - individual.genes[i] = v_dec; - individual.gradients[i] = v_0 * random() + v_dec - v_0; - fitness_sum += dec_fitness; - } - else - { - individual.genes[i] = v_0; - fitness_sum += fitness; - } - } - - // model.incrementalEnd(); - - individual.fitness = fitness_sum / individual.genes.size(); - } - - IKEvolution1(const IKParams& p) - : IKBase(p) - , populationSize(12) - , eliteCount(4) - , in_final_adjustment_loop(false) - , in_adjustment_2(false) - , in_get_solution_fitness(false) - { - setParams(p); - } - - void init() - { - initialGuess = problem.initial_guess; - solution = initialGuess; - - population.resize(populationSize); - - { - auto& p = population[0]; - p.genes = solution; - p.gradients.clear(); - p.gradients.resize(p.genes.size(), 0); - p.fitness = computeFitness(p.genes, false); - } - - for(int i = 1; i < populationSize; i++) - { - auto& p = population[i]; - p.genes = solution; - p.gradients.clear(); - p.gradients.resize(p.genes.size(), 0); - reroll(p); - } - - sortByFitness(); - computeExtinctions(); - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - std::vector tips; - for(auto tip_link_index : problem.tip_link_indices) - tips.push_back(params.robot_model->getLinkModelNames()[tip_link_index]); - heuristicErrorTree = HeuristicErrorTree(params.robot_model, tips); - - init(); - } - - const std::vector& getSolution() const { return solution; } - - double getSolutionFitness() - { - in_get_solution_fitness = true; - double f = computeFitness(solution, true); - in_get_solution_fitness = false; - return f; - } - - const std::vector& getSolutionTipFrames() - { - model.applyConfiguration(solution); - return model.getTipFrames(); - } - - bool evolve() - { - FNPROFILER(); - - auto& offspring = tempOffspring; - offspring = population; - - for(size_t i = 0; i < eliteCount; i++) - { - offspring[i] = population[i]; - exploit(offspring[i]); - } - - auto& pool = tempPool; - pool.resize(populationSize); - iota(pool.begin(), pool.end(), &population[0]); - - for(size_t i = eliteCount; i < populationSize; i++) - { - if(pool.size() > 0) - { - auto& parentA = *select(pool); - auto& parentB = *select(pool); - auto& prototype = *select(pool); - reproduce(offspring[i], parentA, parentB, prototype); - if(offspring[i].fitness < parentA.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentA), pool.end()); - if(offspring[i].fitness < parentB.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentB), pool.end()); - } - else - { - reroll(offspring[i]); - } - } - - population = offspring; - - sortByFitness(); - - computeExtinctions(); - - if(tryUpdateSolution()) return true; - if(opt_no_wipeout) return false; - if(!checkWipeout()) return false; - - init(); - - return tryUpdateSolution(); - } - - void step() - { - in_adjustment_2 = false; - evolve(); - } - - virtual size_t concurrency() const { return 4; } -}; - -static IKFactory::Class cIKEvolution1("bio1"); - -} diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp deleted file mode 100644 index 2a4bb8a..0000000 --- a/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp +++ /dev/null @@ -1,659 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -#ifdef ENABLE_CPP_OPTLIB -#include "cppoptlib/solver/lbfgssolver.h" -#endif - -namespace bio_ik -{ - -// fast evolutionary inverse kinematics -template struct IKEvolution2 : IKBase -{ - struct Individual - { - aligned_vector genes; - aligned_vector gradients; - double fitness; - }; - - struct Species - { - std::vector individuals; - double fitness; - bool improved; - }; - - std::vector initial_guess; - std::vector solution; - std::vector temp_joint_variables; - double solution_fitness; - std::vector species; - std::vector children; - std::vector> phenotypes, phenotypes2, phenotypes3; - std::vector child_indices; - std::vector genotypes; - std::vector phenotype; - std::vector quaternion_genes; - aligned_vector genes_min, genes_max, genes_span; - aligned_vector gradient, temp; - - IKEvolution2(const IKParams& p) - : IKBase(p) - { - } - -#ifdef ENABLE_CPP_OPTLIB - struct OptlibProblem : cppoptlib::Problem - { - IKEvolution2* ik; - OptlibProblem(IKEvolution2* ik) - : ik(ik) - { - } - double value(const TVector& x) - { - const double* genes = x.data(); - ik->model.computeApproximateMutations(1, &genes, ik->phenotypes); - return ik->computeCombinedFitnessActiveVariables(ik->phenotypes[0], genes); - } - }; - typedef cppoptlib::LbfgsSolver OptlibSolver; - std::shared_ptr optlib_solver; - std::shared_ptr optlib_problem; - typename OptlibSolver::TVector optlib_vector; -#endif - - void genesToJointVariables(const Individual& individual, std::vector& variables) - { - auto& genes = individual.genes; - variables.resize(params.robot_model->getVariableCount()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - variables[problem.active_variables[i]] = genes[i]; - } - - const std::vector& getSolution() const { return solution; } - - void initialize(const Problem& problem) - { - BLOCKPROFILER("initialization"); - - IKBase::initialize(problem); - - // init list of quaternion joint genes to be normalized during each mutation - quaternion_genes.clear(); - for(size_t igene = 0; igene < problem.active_variables.size(); igene++) - { - size_t ivar = problem.active_variables[igene]; - auto* joint_model = params.robot_model->getJointOfVariable(ivar); - if(joint_model->getFirstVariableIndex() + 3 != ivar) continue; - if(joint_model->getType() != moveit::core::JointModel::FLOATING) continue; - quaternion_genes.push_back(igene); - } - - // set solution to initial guess - initial_guess = problem.initial_guess; - solution = initial_guess; - solution_fitness = computeFitness(solution); - - // init temporary buffer with positions of inactive joints - temp_joint_variables = initial_guess; - - // params - size_t population_size = 2; - size_t child_count = 16; - - // initialize population on current island - species.resize(2); - for(auto& s : species) - { - // create individuals - s.individuals.resize(population_size); - - // initialize first individual - { - auto& v = s.individuals[0]; - - // init genes - v.genes.resize(problem.active_variables.size()); - // if(thread_index == 0) // on first island? - // if(thread_index % 2 == 0) // on every second island... - if(1) - { - // set to initial_guess - for(size_t i = 0; i < v.genes.size(); i++) - v.genes[i] = initial_guess[problem.active_variables[i]]; - } - else - { - // initialize populations on other islands randomly - for(size_t i = 0; i < v.genes.size(); i++) - v.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); - } - - // set gradients to zero - v.gradients.clear(); - v.gradients.resize(problem.active_variables.size(), 0); - } - - // clone first individual - for(size_t i = 1; i < s.individuals.size(); i++) - { - s.individuals[i].genes = s.individuals[0].genes; - s.individuals[i].gradients = s.individuals[0].gradients; - } - } - - // space for child population - children.resize(population_size + child_count); - for(auto& child : children) - { - child.genes.resize(problem.active_variables.size()); - child.gradients.resize(problem.active_variables.size()); - } - - // init gene infos - // if(genes_min.empty()) - { - genes_min.resize(problem.active_variables.size()); - genes_max.resize(problem.active_variables.size()); - genes_span.resize(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - { - genes_min[i] = modelInfo.getClipMin(problem.active_variables[i]); - genes_max[i] = modelInfo.getClipMax(problem.active_variables[i]); - genes_span[i] = modelInfo.getSpan(problem.active_variables[i]); - } - } - - /* - // init chain mutation masks - chain_mutation_masks.resize(chain_mutation_mask_count); - for(auto& chain_mutation_mask : chain_mutation_masks) - { - temp_mutation_chain.clear(); - if(problem.tip_link_indices.size() > 1) - { - for(auto* chain_link = params.robot_model->getLinkModel(random_element(problem.tip_link_indices)); chain_link; chain_link = chain_link->getParentLinkModel()) - temp_mutation_chain.push_back(chain_link); - temp_mutation_chain.resize(random_index(temp_mutation_chain.size()) + 1); - } - - temp_chain_mutation_var_mask.resize(params.robot_model->getVariableCount()); - for(auto& m : temp_chain_mutation_var_mask) m = 0; - for(auto* chain_link : temp_mutation_chain) - { - auto* chain_joint = chain_link->getParentJointModel(); - for(size_t ivar = chain_joint->getFirstVariableIndex(); ivar < chain_joint->getFirstVariableIndex() + chain_joint->getVariableCount(); ivar++) - temp_chain_mutation_var_mask[ivar] = 1; - } - - chain_mutation_mask.resize(problem.active_variables.size()); - for(size_t igene = 0; igene < problem.active_variables.size(); igene++) - chain_mutation_mask[igene] = temp_chain_mutation_var_mask[problem.active_variables[igene]]; - } - */ - } - - /* - const size_t chain_mutation_mask_count = 256; - std::vector> chain_mutation_masks; - std::vector temp_mutation_chain; - std::vector temp_chain_mutation_var_mask; - */ - - // aligned_vector rmask; - - // create offspring and mutate - __attribute__((hot)) __attribute__((noinline)) - //__attribute__((target_clones("avx2", "avx", "sse2", "default"))) - //__attribute__((target("avx"))) - void - reproduce(const std::vector& population) - { - const auto __attribute__((aligned(32)))* __restrict__ genes_span = this->genes_span.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_min = this->genes_min.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_max = this->genes_max.data(); - - auto gene_count = children[0].genes.size(); - - size_t s = (children.size() - population.size()) * gene_count + children.size() * 4 + 4; - - auto* __restrict__ rr = fast_random_gauss_n(s); - rr = (const double*)(((size_t)rr + 3) / 4 * 4); - - /*rmask.resize(s); - for(auto& m : rmask) m = fast_random() < 0.1 ? 1.0 : 0.0; - double* dm = rmask.data();*/ - - for(size_t child_index = population.size(); child_index < children.size(); child_index++) - { - double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23)); - auto& parent = population[0]; - auto& parent2 = population[1]; - double fmix = (child_index % 2 == 0) * 0.2; - double gradient_factor = child_index % 3; - - auto __attribute__((aligned(32)))* __restrict__ parent_genes = parent.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ parent_gradients = parent.gradients.data(); - - auto __attribute__((aligned(32)))* __restrict__ parent2_genes = parent2.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ parent2_gradients = parent2.gradients.data(); - - auto& child = children[child_index]; - - auto __attribute__((aligned(32)))* __restrict__ child_genes = child.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ child_gradients = child.gradients.data(); - -#pragma unroll -#pragma omp simd aligned(genes_span : 32), aligned(genes_min : 32), aligned(genes_max : 32), aligned(parent_genes : 32), aligned(parent_gradients : 32), aligned(parent2_genes : 32), aligned(parent2_gradients : 32), aligned(child_genes : 32), aligned(child_gradients : 32) aligned(rr : 32) - for(size_t gene_index = 0; gene_index < gene_count; gene_index++) - { - // double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23)); - - double r = rr[gene_index]; - // r *= dm[gene_index]; - double f = mutation_rate * genes_span[gene_index]; - double gene = parent_genes[gene_index]; - double parent_gene = gene; - gene += r * f; - double parent_gradient = mix(parent_gradients[gene_index], parent2_gradients[gene_index], fmix); - double gradient = parent_gradient * gradient_factor; - gene += gradient; - gene = clamp(gene, genes_min[gene_index], genes_max[gene_index]); - child_genes[gene_index] = gene; - child_gradients[gene_index] = mix(parent_gradient, gene - parent_gene, 0.3); - } - rr += (gene_count + 3) / 4 * 4; - // dm += (gene_count + 3) / 4 * 4; - - /*if(problem.tip_link_indices.size() > 1) - { - if(fast_random() < 0.5) - { - auto& mask = chain_mutation_masks[fast_random_index(chain_mutation_mask_count)]; - for(size_t gene_index = 0; gene_index < gene_count; gene_index++) - { - if(!mask[gene_index]) - { - child_genes[gene_index] = parent_genes[gene_index]; - child_gradients[gene_index] = parent_gradients[gene_index]; - } - } - } - }*/ - - for(auto quaternion_gene_index : quaternion_genes) - { - auto& qpos = (*(Quaternion*)&(children[child_index].genes[quaternion_gene_index])); - normalizeFast(qpos); - } - } - } - - void step() - { - FNPROFILER(); - - for(size_t ispecies = 0; ispecies < species.size(); ispecies++) - { - auto& species = this->species[ispecies]; - auto& population = species.individuals; - - { - BLOCKPROFILER("evolution"); - - // initialize forward kinematics approximator - genesToJointVariables(species.individuals[0], temp_joint_variables); - { - BLOCKPROFILER("fk"); - model.applyConfiguration(temp_joint_variables); - model.initializeMutationApproximator(problem.active_variables); - } - - // run evolution for a few generations - size_t generation_count = 16; - if(memetic) generation_count = 8; - for(size_t generation = 0; generation < generation_count; generation++) - { - // BLOCKPROFILER("evolution"); - - if(canceled) break; - - // reproduction - { - BLOCKPROFILER("reproduction"); - reproduce(population); - } - - size_t child_count = children.size(); - - // pre-selection by secondary objectives - if(problem.secondary_goals.size()) - { - BLOCKPROFILER("pre-selection"); - child_count = random_index(children.size() - population.size() - 1) + 1 + population.size(); - for(size_t child_index = population.size(); child_index < children.size(); child_index++) - { - children[child_index].fitness = computeSecondaryFitnessActiveVariables(children[child_index].genes.data()); - } - { - BLOCKPROFILER("pre-selection sort"); - std::sort(children.begin() + population.size(), children.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); - } - } - - // keep parents - { - BLOCKPROFILER("keep alive"); - for(size_t i = 0; i < population.size(); i++) - { - children[i].genes = population[i].genes; - children[i].gradients = population[i].gradients; - } - } - - // genotype-phenotype mapping - { - BLOCKPROFILER("phenotype"); - size_t gene_count = children[0].genes.size(); - genotypes.resize(child_count); - for(size_t i = 0; i < child_count; i++) - genotypes[i] = children[i].genes.data(); - model.computeApproximateMutations(child_count, genotypes.data(), phenotypes); - } - - // fitness - { - BLOCKPROFILER("fitness"); - for(size_t child_index = 0; child_index < child_count; child_index++) - { - children[child_index].fitness = computeFitnessActiveVariables(phenotypes[child_index], genotypes[child_index]); - } - } - - // selection - { - BLOCKPROFILER("selection"); - child_indices.resize(child_count); - for(size_t i = 0; i < child_count; i++) - child_indices[i] = i; - for(size_t i = 0; i < population.size(); i++) - { - size_t jmin = i; - double fmin = children[child_indices[i]].fitness; - for(size_t j = i + 1; j < child_count; j++) - { - double f = children[child_indices[j]].fitness; - if(f < fmin) jmin = j, fmin = f; - } - std::swap(child_indices[i], child_indices[jmin]); - } - for(size_t i = 0; i < population.size(); i++) - { - std::swap(population[i].genes, children[child_indices[i]].genes); - std::swap(population[i].gradients, children[child_indices[i]].gradients); - } - } - } - } - - // memetic optimization - { - BLOCKPROFILER("memetics"); - - if(memetic == 'q' || memetic == 'l') - { - - // init - auto& individual = population[0]; - gradient.resize(problem.active_variables.size()); - if(genotypes.empty()) genotypes.emplace_back(); - phenotypes2.resize(1); - phenotypes3.resize(1); - - // differentiation step size - double dp = 0.0000001; - if(fast_random() < 0.5) dp = -dp; - - for(size_t generation = 0; generation < 8; generation++) - // for(size_t generation = 0; generation < 32; generation++) - { - - if(canceled) break; - - // compute gradient - temp = individual.genes; - genotypes[0] = temp.data(); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f2p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - double fa = f2p + computeSecondaryFitnessActiveVariables(genotypes[0]); - for(size_t i = 0; i < problem.active_variables.size(); i++) - { - double* pp = &(genotypes[0][i]); - genotypes[0][i] = individual.genes[i] + dp; - model.computeApproximateMutation1(problem.active_variables[i], +dp, phenotypes2[0], phenotypes3[0]); - double fb = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - genotypes[0][i] = individual.genes[i]; - double d = fb - fa; - gradient[i] = d; - } - - // normalize gradient - double sum = dp * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) - sum += fabs(gradient[i]); - double f = 1.0 / sum * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) - gradient[i] *= f; - - // sample support points for line search - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = individual.genes[i] - gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); - double f1 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - - double f2 = fa; - - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = individual.genes[i] + gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); - double f3 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - - // quadratic step size - if(memetic == 'q') - { - - // compute step size - double v1 = (f2 - f1); // f / j - double v2 = (f3 - f2); // f / j - double v = (v1 + v2) * 0.5; // f / j - double a = (v1 - v2); // f / j^2 - double step_size = v / a; // (f / j) / (f / j^2) = f / j / f * j * j = j - - // double v1 = (f2 - f1) / dp; - // double v2 = (f3 - f2) / dp; - // double v = (v1 + v2) * 0.5; - // double a = (v2 - v1) / dp; - // // v * x + a * x * x = 0; - // // v = - a * x - // // - v / a = x - // // x = -v / a; - // double step_size = -v / a / dp; - - // for(double f : { 1.0, 0.5, 0.25 }) - { - - double f = 1.0; - - // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = modelInfo.clip(individual.genes[i] + gradient[i] * step_size * f, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - - // accept new position if better - if(f4p < f2p) - { - individual.genes = temp; - continue; - } - else - { - break; - } - } - - // break; - } - - // linear step size - if(memetic == 'l') - { - - // compute step size - double cost_diff = (f3 - f1) * 0.5; // f / j - double step_size = f2 / cost_diff; // f / (f / j) = j - - // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - temp[i] = modelInfo.clip(individual.genes[i] - gradient[i] * step_size, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - - // accept new position if better - if(f4p < f2p) - { - individual.genes = temp; - continue; - } - else - { - break; - } - } - } - } - -#ifdef ENABLE_CPP_OPTLIB - // cppoptlib::LbfgsSolver memetic test - if(memetic == 'o') - { - auto& individual = population[0]; - - // create cppoptlib solver and cppoptlib problem, if not yet existing - if(!optlib_solver) - { - optlib_solver = std::make_shared(); - cppoptlib::Criteria crit; - crit.iterations = 4; - optlib_solver->setStopCriteria(crit); - optlib_problem = std::make_shared(this); - } - - // init starting point - optlib_vector.resize(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - optlib_vector[i] = individual.genes[i]; - - // solve - optlib_solver->minimize(*optlib_problem, optlib_vector); - - // get result - for(size_t i = 0; i < problem.active_variables.size(); i++) - individual.genes[i] = modelInfo.clip(optlib_vector[i], problem.active_variables[i]); - } -#endif - } - } - - { - BLOCKPROFILER("species"); - - // compute species fitness - for(auto& species : this->species) - { - genesToJointVariables(species.individuals[0], temp_joint_variables); - double fitness = computeFitness(temp_joint_variables); - species.improved = (fitness != species.fitness); - species.fitness = fitness; - } - - // sort species by fitness - std::sort(species.begin(), species.end(), [](const Species& a, const Species& b) { return a.fitness < b.fitness; }); - - // wipeouts - for(size_t species_index = 1; species_index < species.size(); species_index++) - { - if(fast_random() < 0.1 || !species[species_index].improved) - // if(fast_random() < 0.05 || !species[species_index].improved) - { - { - auto& individual = species[species_index].individuals[0]; - - for(size_t i = 0; i < individual.genes.size(); i++) - individual.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); - - for(auto& v : individual.gradients) - v = 0; - } - for(size_t i = 0; i < species[species_index].individuals.size(); i++) - species[species_index].individuals[i] = species[species_index].individuals[0]; - } - } - - // update solution - if(species[0].fitness < solution_fitness) - { - genesToJointVariables(species[0].individuals[0], solution); - solution_fitness = species[0].fitness; - } - } - } - - // number of islands - virtual size_t concurrency() const { return 4; } -}; - -static IKFactory::Class> bio2("bio2"); -static IKFactory::Class> bio2_memetic("bio2_memetic"); -static IKFactory::Class> bio2_memetic_l("bio2_memetic_l"); - -#ifdef ENABLE_CPP_OPTLIB -static IKFactory::Class> bio2_memetic_0("bio2_memetic_lbfgs"); -#endif -} diff --git a/src/bio_ik-kinetic-support/src/ik_gradient.cpp b/src/bio_ik-kinetic-support/src/ik_gradient.cpp deleted file mode 100644 index cd33c56..0000000 --- a/src/bio_ik-kinetic-support/src/ik_gradient.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -// pseudoinverse jacobian solver -// (mainly for testing RobotFK_Jacobian::computeJacobian) -template struct IKJacobianBase : BASE -{ - using BASE::modelInfo; - using BASE::model; - using BASE::params; - using BASE::computeFitness; - using BASE::problem; - - std::vector tipObjectives; - - Eigen::VectorXd tip_diffs; - Eigen::VectorXd joint_diffs; - Eigen::MatrixXd jacobian; - std::vector tip_frames_temp; - - IKJacobianBase(const IKParams& p) - : BASE(p) - { - } - - void initialize(const Problem& problem) - { - BASE::initialize(problem); - tipObjectives.resize(problem.tip_link_indices.size()); - for(auto& goal : problem.goals) - tipObjectives[goal.tip_index] = goal.frame; - } - - void optimizeJacobian(std::vector& solution) - { - FNPROFILER(); - - int tip_count = problem.tip_link_indices.size(); - tip_diffs.resize(tip_count * 6); - joint_diffs.resize(problem.active_variables.size()); - - // compute fk - model.applyConfiguration(solution); - - double translational_scale = 1; - double rotational_scale = 1; - - // compute goal diffs - tip_frames_temp = model.getTipFrames(); - for(int itip = 0; itip < tip_count; itip++) - { - auto twist = frameTwist(tip_frames_temp[itip], tipObjectives[itip]); - tip_diffs(itip * 6 + 0) = twist.vel.x() * translational_scale; - tip_diffs(itip * 6 + 1) = twist.vel.y() * translational_scale; - tip_diffs(itip * 6 + 2) = twist.vel.z() * translational_scale; - tip_diffs(itip * 6 + 3) = twist.rot.x() * rotational_scale; - tip_diffs(itip * 6 + 4) = twist.rot.y() * rotational_scale; - tip_diffs(itip * 6 + 5) = twist.rot.z() * rotational_scale; - } - - // compute jacobian - { - model.computeJacobian(problem.active_variables, jacobian); - int icol = 0; - for(auto ivar : problem.active_variables) - { - for(size_t itip = 0; itip < tip_count; itip++) - { - jacobian(itip * 6 + 0, icol) *= translational_scale; - jacobian(itip * 6 + 1, icol) *= translational_scale; - jacobian(itip * 6 + 2, icol) *= translational_scale; - jacobian(itip * 6 + 3, icol) *= rotational_scale; - jacobian(itip * 6 + 4, icol) *= rotational_scale; - jacobian(itip * 6 + 5, icol) *= rotational_scale; - } - icol++; - } - } - - // get pseudoinverse and multiply - joint_diffs = jacobian.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tip_diffs); - // joint_diffs = (jacobian.transpose() * jacobian).ldlt().solve(jacobian.transpose() * tip_diffs); - - // apply joint deltas and clip - { - int icol = 0; - for(auto ivar : problem.active_variables) - { - auto v = solution[ivar] + joint_diffs(icol); - if(!std::isfinite(v)) continue; - v = modelInfo.clip(v, ivar); - solution[ivar] = v; - icol++; - } - } - } -}; - -// simple gradient descent -template struct IKGradientDescent : IKBase -{ - std::vector solution, best_solution, gradient, temp; - bool reset; - - IKGradientDescent(const IKParams& p) - : IKBase(p) - { - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - solution = problem.initial_guess; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - best_solution = solution; - reset = false; - } - - const std::vector& getSolution() const { return best_solution; } - - void step() - { - // random reset if stuck - if(reset) - { - reset = false; - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - // compute gradient direction - temp = solution; - double jd = 0.0001; - gradient.resize(solution.size(), 0); - for(auto ivar : problem.active_variables) - { - temp[ivar] = solution[ivar] - jd; - double p1 = computeFitness(temp); - - temp[ivar] = solution[ivar] + jd; - double p3 = computeFitness(temp); - - temp[ivar] = solution[ivar]; - - gradient[ivar] = p3 - p1; - } - - // normalize gradient direction - double sum = 0.0001; - for(auto ivar : problem.active_variables) - sum += fabs(gradient[ivar]); - double f = 1.0 / sum * jd; - for(auto ivar : problem.active_variables) - gradient[ivar] *= f; - - // initialize line search - temp = solution; - - for(auto ivar : problem.active_variables) - temp[ivar] = solution[ivar] - gradient[ivar]; - double p1 = computeFitness(temp); - - // for(auto ivar : problem.active_variables) temp[ivar] = solution[ivar]; - // double p2 = computeFitness(temp); - - for(auto ivar : problem.active_variables) - temp[ivar] = solution[ivar] + gradient[ivar]; - double p3 = computeFitness(temp); - - double p2 = (p1 + p3) * 0.5; - - // linear step size estimation - double cost_diff = (p3 - p1) * 0.5; - double joint_diff = p2 / cost_diff; - - // apply optimization step - // (move along gradient direction by estimated step size) - for(auto ivar : problem.active_variables) - temp[ivar] = modelInfo.clip(solution[ivar] - gradient[ivar] * joint_diff, ivar); - - if(if_stuck == 'c') - { - // always accept solution and continue - solution = temp; - } - else - { - // has solution improved? - if(computeFitness(temp) < computeFitness(solution)) - { - // solution improved -> accept solution - solution = temp; - } - else - { - if(if_stuck == 'r') - { - // reset if stuck - reset = true; - } - } - } - - // update best solution - if(computeFitness(solution) < computeFitness(best_solution)) best_solution = solution; - } - - size_t concurrency() const { return threads; } -}; - -static IKFactory::Class> gd("gd"); -static IKFactory::Class> gd_2("gd_2"); -static IKFactory::Class> gd_4("gd_4"); -static IKFactory::Class> gd_8("gd_8"); - -static IKFactory::Class> gd_r("gd_r"); -static IKFactory::Class> gd_2_r("gd_r_2"); -static IKFactory::Class> gd_4_r("gd_r_4"); -static IKFactory::Class> gd_8_r("gd_r_8"); - -static IKFactory::Class> gd_c("gd_c"); -static IKFactory::Class> gd_2_c("gd_c_2"); -static IKFactory::Class> gd_4_c("gd_c_4"); -static IKFactory::Class> gd_8_c("gd_c_8"); - -// pseudoinverse jacobian only -template struct IKJacobian : IKJacobianBase -{ - using IKBase::initialize; - std::vector solution; - IKJacobian(const IKParams& p) - : IKJacobianBase(p) - { - } - void initialize(const Problem& problem) - { - IKJacobianBase::initialize(problem); - solution = problem.initial_guess; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - const std::vector& getSolution() const { return solution; } - void step() { optimizeJacobian(solution); } - size_t concurrency() const { return threads; } -}; -static IKFactory::Class> jac("jac"); -static IKFactory::Class> jac_2("jac_2"); -static IKFactory::Class> jac_4("jac_4"); -static IKFactory::Class> jac_8("jac_8"); -} diff --git a/src/bio_ik-kinetic-support/src/ik_neural.cpp b/src/bio_ik-kinetic-support/src/ik_neural.cpp deleted file mode 100644 index 8c4c2a3..0000000 --- a/src/bio_ik-kinetic-support/src/ik_neural.cpp +++ /dev/null @@ -1,690 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -#include -#include - -#include - -#define LOG_LIST(l) \ - { \ - LOG(#l "[]"); \ - for(std::size_t i = 0; i < l.size(); i++) \ - { \ - LOG(#l "[" + std::to_string(i) + "]", l[i]); \ - } \ - } - -namespace bio_ik -{ - -/* -static inline KDL::Twist toTwist(const Frame& frame) -{ - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - auto r = frame.rot.getAxis() * frame.rot.getAngle(); - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - return t; -} - -static inline KDL::Twist frameTwist(const Frame& a, const Frame& b) -{ - auto frame = inverse(a) * b; - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - auto r = frame.rot.getAxis() * frame.rot.getAngle(); - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - return t; -} -*/ - -struct IKNeural : IKBase -{ - std::vector solution; - FANN::neural_net net; - - std::vector> input_minmax, output_minmax; - - static void find_minmax(const std::vector& values, std::vector>& minmax) - { - for(size_t i = 0; i < minmax.size(); i++) - { - minmax[i] = std::make_pair(values[i], values[i]); - } - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - p.first = std::min(p.first, v); - p.second = std::max(p.second, v); - } - } - - static void normalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = (v - p.first) / (p.second - p.first); - } - } - - static void denormalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = v * (p.second - p.first) + p.first; - } - } - - unsigned int input_count, output_count; - - IKNeural(const IKParams& p) - : IKBase(p) - { - trained = false; - } - - bool trained; - - void train() - { - trained = true; - - input_count = problem.active_variables.size() + problem.tip_link_indices.size() * 6; - output_count = problem.active_variables.size(); - - LOG_VAR(input_count); - LOG_VAR(output_count); - - // std::vector levels = {input_count, input_count, input_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, 100, output_count}; - - std::vector levels = {input_count, 50, output_count}; - - net.create_standard_array(levels.size(), levels.data()); - - size_t var_count = params.robot_model->getVariableCount(); - std::vector state1(var_count), state2(var_count); - std::vector frames1, frames2; - - std::vector inputs, outputs; - std::vector input_pp, output_pp; - - LOG("neuro ik generating training data"); - - unsigned int samples = 10000; - - for(size_t iter = 0; iter < samples; iter++) - { - for(size_t ivar = 0; ivar < var_count; ivar++) - { - state1[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar)); - state1[ivar] = modelInfo.clip(state1[ivar], ivar); - // state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * modelInfo.getSpan(ivar), ivar); - state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * 0.1, ivar); - } - - model.applyConfiguration(state1); - frames1 = model.getTipFrames(); - model.applyConfiguration(state2); - frames2 = model.getTipFrames(); - - for(auto ivar : problem.active_variables) - { - inputs.push_back(state1[ivar]); - outputs.push_back(state2[ivar] - state1[ivar]); - } - - for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - - // Frame diff = inverse(frames1[itip]) * frames2[itip]; - // auto twist = toTwist(diff); - auto twist = frameTwist(frames1[itip], frames2[itip]); - - inputs.push_back(frames2[itip].pos.x() - frames1[itip].pos.x()); - inputs.push_back(frames2[itip].pos.y() - frames1[itip].pos.y()); - inputs.push_back(frames2[itip].pos.z() - frames1[itip].pos.z()); - - inputs.push_back(twist.rot.x() * rotational_scale); - inputs.push_back(twist.rot.y() * rotational_scale); - inputs.push_back(twist.rot.z() * rotational_scale); - } - } - - for(auto& v : inputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - for(auto& v : outputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - - input_minmax.resize(input_count); - output_minmax.resize(output_count); - - find_minmax(inputs, input_minmax); - find_minmax(outputs, output_minmax); - - normalize(inputs, input_minmax); - normalize(outputs, output_minmax); - - for(size_t iter = 0; iter < samples; iter++) - { - input_pp.push_back(inputs.data() + iter * input_count); - output_pp.push_back(outputs.data() + iter * output_count); - } - - LOG("neuro ik training"); - - FANN::training_data train; - train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data()); - net.set_callback( - [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) { - if(epochs % epochs_between_reports != 0) return 0; - // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%"); - LOG("training", epochs, net.get_MSE(), desired_error); - return 0; - }, - 0); - - net.set_activation_function_hidden(FANN::SIGMOID); - net.set_activation_function_output(FANN::SIGMOID); - - net.init_weights(train); - - net.train_on_data(train, 1000, 1, 0.0001); - - fann_type err = net.test_data(train); - LOG("neuro ik training error:", err); - - /*std::vector iiv, oov, ttv; - for(size_t iter = 0; iter < 10; iter++) - { - auto* ii = input_pp[iter]; - auto* oo = net.run(ii); - auto* tt = output_pp[iter]; - iiv.assign(ii, ii + input_count); - ttv.assign(tt, tt + output_count); - oov.assign(oo, oo + output_count); - LOG_LIST(iiv); - LOG_LIST(ttv); - LOG_LIST(oov); - }*/ - - LOG("training done"); - } - - size_t iterations = 0; - - void initialize(const Problem& problem) - { - static std::mutex mutex; - std::lock_guard lock(mutex); - IKBase::initialize(problem); - solution = problem.initial_guess; - if(!trained) train(); - iterations = 0; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - const std::vector& getSolution() const { return solution; } - - std::vector inputs, outputs; - - std::vector tip_objectives; - - /*void step() - { - //if(iterations > 1) return; - iterations++; - - inputs.clear(); - for(auto ivar : problem.active_variables) - { - inputs.push_back(solution[ivar]); - } - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - model.applyConfiguration(solution); - auto& frames1 = model.getTipFrames(); - auto& frames2 = tip_objectives; - - double scale = 1.0; - - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - - //Frame diff = inverse(frames1[itip]) * frames2[itip]; - //auto twist = toTwist(diff); - auto twist = frameTwist(frames1[itip], frames2[itip]); - - auto dpos = frames2[itip].pos - frames1[itip].pos; - auto drot = Vector3( - twist.rot.x() * rotational_scale, - twist.rot.y() * rotational_scale, - twist.rot.z() * rotational_scale - ); - - scale = 1.0 / (0.0000001 + dpos.length() + drot.length()); - - dpos = dpos * scale; - drot = drot * scale; - - inputs.push_back(dpos.x()); - inputs.push_back(dpos.y()); - inputs.push_back(dpos.z()); - - inputs.push_back(drot.x()); - inputs.push_back(drot.y()); - inputs.push_back(drot.z()); - } - - normalize(inputs, input_minmax); - - auto* oo = net.run(inputs.data()); - - outputs.assign(oo, oo + output_count); - - denormalize(outputs, output_minmax); - - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 0.1 / scale, ivar); - } - }*/ - - void step() - { - // if(iterations > 1) return; - iterations++; - - inputs.clear(); - for(auto ivar : problem.active_variables) - { - inputs.push_back(solution[ivar]); - } - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - model.applyConfiguration(solution); - auto& frames1 = model.getTipFrames(); - auto& frames2 = tip_objectives; - - double scale = 1.0; - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - auto twist = frameTwist(frames1[itip], frames2[itip]); - auto dpos = frames2[itip].pos - frames1[itip].pos; - auto drot = Vector3(twist.rot.x() * rotational_scale, twist.rot.y() * rotational_scale, twist.rot.z() * rotational_scale); - - /*if(iterations % 2) - { - scale = 1.0 / (0.0000001 + dpos.length()); - inputs.push_back(dpos.x() * scale); - inputs.push_back(dpos.y() * scale); - inputs.push_back(dpos.z() * scale); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(0); - } else { - scale = 1.0 / (0.0000001 + drot.length()); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(drot.x() * scale); - inputs.push_back(drot.y() * scale); - inputs.push_back(drot.z() * scale); - }*/ - - { - scale = 1.0 / (0.0000001 + dpos.length() + drot.length()); - inputs.push_back(dpos.x() * scale); - inputs.push_back(dpos.y() * scale); - inputs.push_back(dpos.z() * scale); - inputs.push_back(drot.x() * scale); - inputs.push_back(drot.y() * scale); - inputs.push_back(drot.z() * scale); - } - } - normalize(inputs, input_minmax); - auto* oo = net.run(inputs.data()); - outputs.assign(oo, oo + output_count); - denormalize(outputs, output_minmax); - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 1 / scale, ivar); - } - } -}; - -static IKFactory::Class neural("neural"); - -struct IKNeural2 : IKBase -{ - std::vector solution; - FANN::neural_net net; - - std::vector> input_minmax, output_minmax; - - /*static void find_minmax(const std::vector& values, std::vector>& minmax) - { - for(size_t i = 0; i < minmax.size(); i++) - { - minmax[i] = std::make_pair(values[i], values[i]); - } - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - p.first = std::min(p.first, v); - p.second = std::max(p.second, v); - } - }*/ - - static void find_minmax(const std::vector& values, std::vector>& minmax) - { - std::vector centers(minmax.size(), 0.0); - for(size_t i = 0; i < values.size(); i++) - centers[i % minmax.size()] += values[i] * (1.0 * minmax.size() / values.size()); - - std::vector ranges2(minmax.size(), 0.0001); - for(size_t i = 0; i < values.size(); i++) - { - double d = values[i] - centers[i % minmax.size()]; - d = d * d; - ranges2[i % minmax.size()] += d * (1.0 * minmax.size() / values.size()); - } - - for(size_t i = 0; i < minmax.size(); i++) - { - auto& p = minmax[i]; - p.first = centers[i] - sqrt(ranges2[i]); - p.second = centers[i] + sqrt(ranges2[i]); - } - } - - static void normalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = (v - p.first) / (p.second - p.first); - } - } - - static void denormalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = v * (p.second - p.first) + p.first; - } - } - - unsigned int input_count, output_count; - - IKNeural2(const IKParams& p) - : IKBase(p) - { - trained = false; - } - - bool trained; - - void train() - { - trained = true; - - input_count = problem.tip_link_indices.size() * 7; - output_count = problem.active_variables.size(); - - LOG_VAR(input_count); - LOG_VAR(output_count); - - // std::vector levels = {input_count, 100, 100, output_count}; - - // std::vector levels = {input_count, input_count, input_count, output_count}; - - std::vector levels = {input_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, output_count}; - - net.create_standard_array(levels.size(), levels.data()); - - size_t var_count = params.robot_model->getVariableCount(); - std::vector state = problem.initial_guess; - std::vector frames; - - std::vector inputs, outputs; - std::vector input_pp, output_pp; - - LOG("neuro ik generating training data"); - - unsigned int samples = 10000; - - for(size_t iter = 0; iter < samples; iter++) - { - for(size_t ivar : problem.active_variables) - state[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar)); - - model.applyConfiguration(state); - frames = model.getTipFrames(); - - for(auto ivar : problem.active_variables) - outputs.push_back(state[ivar]); - - for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++) - { - inputs.push_back(frames[itip].pos.x()); - inputs.push_back(frames[itip].pos.y()); - inputs.push_back(frames[itip].pos.z()); - - auto rot = frames[itip].rot; - rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); - inputs.push_back(rot.x()); - inputs.push_back(rot.y()); - inputs.push_back(rot.z()); - inputs.push_back(rot.w()); - } - } - - for(auto& v : inputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - for(auto& v : outputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - - input_minmax.resize(input_count); - output_minmax.resize(output_count); - - find_minmax(inputs, input_minmax); - find_minmax(outputs, output_minmax); - - normalize(inputs, input_minmax); - normalize(outputs, output_minmax); - - for(size_t iter = 0; iter < samples; iter++) - { - input_pp.push_back(inputs.data() + iter * input_count); - output_pp.push_back(outputs.data() + iter * output_count); - } - - LOG("neuro ik training"); - - FANN::training_data train; - train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data()); - net.set_callback( - [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) { - if(epochs % epochs_between_reports != 0) return 0; - // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%"); - LOG("training", epochs, net.get_MSE(), desired_error); - return 0; - }, - 0); - - net.set_activation_function_hidden(FANN::SIGMOID); - net.set_activation_function_output(FANN::SIGMOID); - - net.init_weights(train); - - net.train_on_data(train, 100, 1, 0.0001); - - fann_type err = net.test_data(train); - LOG("neuro ik training error:", err); - - /*std::vector iiv, oov, ttv; - for(size_t iter = 0; iter < 10; iter++) - { - auto* ii = input_pp[iter]; - auto* oo = net.run(ii); - auto* tt = output_pp[iter]; - iiv.assign(ii, ii + input_count); - ttv.assign(tt, tt + output_count); - oov.assign(oo, oo + output_count); - LOG_LIST(iiv); - LOG_LIST(ttv); - LOG_LIST(oov); - }*/ - - LOG("training done"); - } - - size_t iterations = 0; - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - solution = problem.initial_guess; - if(!trained) train(); - iterations = 0; - } - - const std::vector& getSolution() const { return solution; } - - std::vector inputs, outputs; - - std::vector tip_objectives; - - void step() - { - if(iterations > 1) return; - iterations++; - - inputs.clear(); - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - auto& frames = tip_objectives; - - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - inputs.push_back(frames[itip].pos.x()); - inputs.push_back(frames[itip].pos.y()); - inputs.push_back(frames[itip].pos.z()); - - auto rot = frames[itip].rot; - rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); - inputs.push_back(rot.x()); - inputs.push_back(rot.y()); - inputs.push_back(rot.z()); - inputs.push_back(rot.w()); - } - - normalize(inputs, input_minmax); - - auto* oo = net.run(inputs.data()); - - outputs.assign(oo, oo + output_count); - - denormalize(outputs, output_minmax); - - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(outputs[iout], ivar); - } - } -}; - -static IKFactory::Class neural2("neural2"); -} diff --git a/src/bio_ik-kinetic-support/src/ik_parallel.h b/src/bio_ik-kinetic-support/src/ik_parallel.h deleted file mode 100644 index 8197ca6..0000000 --- a/src/bio_ik-kinetic-support/src/ik_parallel.h +++ /dev/null @@ -1,278 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -#include - -namespace bio_ik -{ - -// executes a function in parallel on pre-allocated threads -class ParallelExecutor -{ - std::function fun; - std::vector threads; - boost::barrier barrier; - volatile bool exit; - double best_fitness; - -public: - template - ParallelExecutor(size_t thread_count, const FUN& f) - : exit(false) - , threads(thread_count) - , fun(f) - , barrier(thread_count) - { - for(size_t i = 1; i < thread_count; i++) - { - std::thread t([this, i]() { - while(true) - { - barrier.wait(); - if(exit) break; - fun(i); - barrier.wait(); - if(exit) break; - } - }); - std::swap(t, threads[i]); - } - } - ~ParallelExecutor() - { - exit = true; - barrier.wait(); - for(auto& t : threads) - if(t.joinable()) t.join(); - } - void run() - { - barrier.wait(); - fun(0); - barrier.wait(); - } -}; - -// runs ik on multiple threads until a stop criterion is met -struct IKParallel -{ - IKParams params; - std::vector> solvers; - std::vector> solver_solutions; - std::vector> solver_temps; - std::vector solver_success; - std::vector solver_fitness; - int thread_count; - // std::vector fk; // TODO: remove - double timeout; - bool success; - std::atomic finished; - std::atomic iteration_count; - std::vector result; - std::unique_ptr par; - Problem problem; - bool enable_counter; - double best_fitness; - - IKParallel(const IKParams& params) - : params(params) - { - // solver class name - std::string name = params.solver_class_name; - - enable_counter = params.enable_counter; - - // create solvers - solvers.emplace_back(IKFactory::create(name, params)); - thread_count = solvers.front()->concurrency(); - if(params.thread_count) { - thread_count = params.thread_count; - } - while(solvers.size() < thread_count) - solvers.emplace_back(IKFactory::clone(solvers.front().get())); - for(size_t i = 0; i < thread_count; i++) - solvers[i]->thread_index = i; - - // while(fk.size() < thread_count) fk.emplace_back(params.robot_model); - - // init buffers - solver_solutions.resize(thread_count); - solver_temps.resize(thread_count); - solver_success.resize(thread_count); - solver_fitness.resize(thread_count); - - // create parallel executor - par.reset(new ParallelExecutor(thread_count, [this](size_t i) { solverthread(i); })); - } - - void initialize(const Problem& problem) - { - this->problem = problem; - // for(auto& f : fk) f.initialize(problem.tip_link_indices); - } - -private: - void solverthread(size_t i) - { - THREADPROFILER("thread", i); - COUNTERPROFILER("solver threads"); - - // initialize ik solvers - { - BLOCKPROFILER("ik solver init"); - solvers[i]->initialize(problem); - } - - // run solver iterations until solution found or timeout - for(size_t iteration = 0; (ros::WallTime::now().toSec() < timeout && finished == 0) || (iteration == 0 && i == 0); iteration++) - { - if(finished) break; - - // run solver for a few steps - solvers[i]->step(); - iteration_count++; - for(int it2 = 1; it2 < 4; it2++) - if(ros::WallTime::now().toSec() < timeout && finished == 0) solvers[i]->step(); - - if(finished) break; - - // get solution and check stop criterion - auto& result = solver_temps[i]; - result = solvers[i]->getSolution(); - auto& fk = solvers[i]->model; - fk.applyConfiguration(result); - bool success = solvers[i]->checkSolution(result, fk.getTipFrames()); - if(success) finished = 1; - solver_success[i] = success; - solver_solutions[i] = result; - solver_fitness[i] = solvers[i]->computeFitness(result, fk.getTipFrames()); - - if(success) break; - } - - finished = 1; - - for(auto& s : solvers) - s->canceled = true; - } - -public: - void solve() - { - BLOCKPROFILER("solve mt"); - - // prepare - iteration_count = 0; - result = problem.initial_guess; - timeout = problem.timeout; - success = false; - finished = 0; - for(auto& s : solver_solutions) - s = problem.initial_guess; - for(auto& s : solver_temps) - s = problem.initial_guess; - for(auto& s : solver_success) - s = 0; - for(auto& f : solver_fitness) - f = DBL_MAX; - for(auto& s : solvers) - s->canceled = false; - - // run solvers - { - BLOCKPROFILER("solve mt 2"); - par->run(); - } - - size_t best_index = 0; - best_fitness = DBL_MAX; - - // if exact primary goal matches have been found ... - for(size_t i = 0; i < thread_count; i++) - { - if(solver_success[i]) - { - double fitness; - if(solvers[0]->problem.secondary_goals.empty()) - { - // ... and if no secondary goals have been specified, - // select the best result according to primary goals - fitness = solver_fitness[i]; - } - else - { - // ... and if secondary goals have been specified, - // select the result that best satisfies primary and secondary goals - fitness = solver_fitness[i] + solvers[0]->computeSecondaryFitnessAllVariables(solver_solutions[i]); - } - if(fitness < best_fitness) - { - best_fitness = fitness; - best_index = i; - } - } - } - - // if no exact primary goal matches have been found, - // select best primary goal approximation - if(best_fitness == DBL_MAX) - { - for(size_t i = 0; i < thread_count; i++) - { - if(solver_fitness[i] < best_fitness) - { - best_fitness = solver_fitness[i]; - best_index = i; - } - } - } - - if(enable_counter) - { - LOG("iterations", iteration_count); - } - - result = solver_solutions[best_index]; - success = solver_success[best_index]; - } - - double getSolutionFitness() const { return best_fitness; } - - bool getSuccess() const { return success; } - - const std::vector& getSolution() const { return result; } -}; -} diff --git a/src/bio_ik-kinetic-support/src/ik_test.cpp b/src/bio_ik-kinetic-support/src/ik_test.cpp deleted file mode 100644 index f1a07ec..0000000 --- a/src/bio_ik-kinetic-support/src/ik_test.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -struct IKTest : IKBase -{ - - RobotFK_MoveIt fkref; - - std::vector temp; - - double d_rot_sum, d_pos_sum, d_div; - - IKTest(const IKParams& params) - : IKBase(params) - , fkref(params.robot_model) - { - d_rot_sum = d_pos_sum = d_div = 0; - } - - /*double tipdiff(const std::vector& fa, const std::vector& fb) - { - double diff = 0.0; - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - //LOG_VAR(fa[i]); - //LOG_VAR(fb[i]); - diff += fa[i].rot.angleShortestPath(fb[i].rot); - diff += fa[i].pos.distance(fb[i].pos); - } - return diff; - }*/ - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - fkref.initialize(problem.tip_link_indices); - model.initialize(problem.tip_link_indices); - - fkref.applyConfiguration(problem.initial_guess); - model.applyConfiguration(problem.initial_guess); - - // double diff = tipdiff(fkref.getTipFrames(), model.getTipFrames()); - // LOG_VAR(diff); - - /*{ - auto& fa = fkref.getTipFrames(); - auto& fb = model.getTipFrames(); - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot)); - LOG("d pos", i, fa[i].pos.distance(fb[i].pos)); - } - }*/ - - { - temp = problem.initial_guess; - for(size_t ivar : problem.active_variables) - if(modelInfo.isRevolute(ivar) || modelInfo.isPrismatic(ivar)) temp[ivar] = modelInfo.clip(temp[ivar] + random(-0.1, 0.1), ivar); - - fkref.applyConfiguration(temp); - auto& fa = fkref.getTipFrames(); - - model.applyConfiguration(problem.initial_guess); - model.initializeMutationApproximator(problem.active_variables); - - std::vector> fbm; - - std::vector mutation_values; - for(size_t ivar : problem.active_variables) - mutation_values.push_back(temp[ivar]); - const double* mutation_ptr = mutation_values.data(); - - model.computeApproximateMutations(1, &mutation_ptr, fbm); - - auto& fb = fbm[0]; - - // auto& fb = model.getTipFrames(); - - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - // LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot)); - // LOG("d pos", i, fa[i].pos.distance(fb[i].pos)); - - d_rot_sum += fa[i].rot.angleShortestPath(fb[i].rot); - d_pos_sum += fa[i].pos.distance(fb[i].pos); - d_div += 1; - } - } - - LOG("d rot avg", d_rot_sum / d_div); - LOG("d pos avg", d_pos_sum / d_div); - } - - void step() {} - - const std::vector& getSolution() const { return problem.initial_guess; } -}; - -static IKFactory::Class test("test"); -} diff --git a/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp b/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp deleted file mode 100644 index 0f5049b..0000000 --- a/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp +++ /dev/null @@ -1,649 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include - -#include "forward_kinematics.h" -#include "ik_base.h" -#include "ik_parallel.h" -#include "problem.h" -#include "utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -//#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -using namespace bio_ik; - -// implement BioIKKinematicsQueryOptions - -namespace bio_ik { - -std::mutex bioIKKinematicsQueryOptionsMutex; -std::unordered_set bioIKKinematicsQueryOptionsList; - -BioIKKinematicsQueryOptions::BioIKKinematicsQueryOptions() - : replace(false), solution_fitness(0) { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - bioIKKinematicsQueryOptionsList.insert(this); -} - -BioIKKinematicsQueryOptions::~BioIKKinematicsQueryOptions() { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - bioIKKinematicsQueryOptionsList.erase(this); -} - -bool isBioIKKinematicsQueryOptions(const void *ptr) { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - return bioIKKinematicsQueryOptionsList.find(ptr) != - bioIKKinematicsQueryOptionsList.end(); -} - -const BioIKKinematicsQueryOptions * -toBioIKKinematicsQueryOptions(const void *ptr) { - if (isBioIKKinematicsQueryOptions(ptr)) - return (const BioIKKinematicsQueryOptions *)ptr; - else - return 0; -} - -} // namespace bio_ik - -// BioIK Kinematics Plugin - -namespace bio_ik_kinematics_plugin { - -// Fallback for older MoveIt versions which don't support lookupParam yet -template -static void lookupParam(const std::string ¶m, T &val, - const T &default_val) { - ros::NodeHandle nodeHandle("~"); - val = nodeHandle.param(param, default_val); -} - -struct BioIKKinematicsPlugin : kinematics::KinematicsBase { - std::vector joint_names, link_names; - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup *joint_model_group; - mutable std::unique_ptr ik; - mutable std::vector state, temp; - mutable std::unique_ptr temp_state; - mutable std::vector tipFrames; - RobotInfo robot_info; - bool enable_profiler; - - BioIKKinematicsPlugin() { enable_profiler = false; } - - virtual const std::vector &getJointNames() const { - LOG_FNC(); - return joint_names; - } - - virtual const std::vector &getLinkNames() const { - LOG_FNC(); - return link_names; - } - - virtual bool getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const { - LOG_FNC(); - return false; - } - - virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { - LOG_FNC(); - return false; - } - - std::vector tip_reference_frames; - - mutable std::vector> default_goals; - - mutable std::vector all_goals; - - IKParams ikparams; - - mutable Problem problem; - - static moveit::core::RobotModelConstPtr - loadRobotModel(const std::string &robot_description) { - static std::map - robot_model_cache; - static std::mutex cache_mutex; - std::lock_guard lock(cache_mutex); - if (robot_model_cache.find(robot_description) == robot_model_cache.end()) { - rdf_loader::RDFLoader rdf_loader(robot_description); - auto srdf = rdf_loader.getSRDF(); - auto urdf_model = rdf_loader.getURDF(); - - if (!urdf_model || !srdf) { - LOG("URDF and SRDF must be loaded for kinematics solver to work."); - return nullptr; - } - robot_model_cache[robot_description] = moveit::core::RobotModelConstPtr( - new robot_model::RobotModel(urdf_model, srdf)); - } - return robot_model_cache[robot_description]; - - // return - // moveit::planning_interface::getSharedRobotModel(robot_description); - } - - bool load(std::string robot_description, std::string group_name) { - LOG_FNC(); - - // LOG_VAR(robot_description); - // LOG_VAR(group_name); - - LOG("bio ik init", ros::this_node::getName()); - - /*rdf_loader::RDFLoader rdf_loader(robot_description_); - auto srdf = rdf_loader.getSRDF(); - auto urdf_model = rdf_loader.getURDF(); - - if(!urdf_model || !srdf) - { - LOG("URDF and SRDF must be loaded for kinematics solver to work."); - return false; - } - - robot_model.reset(new robot_model::RobotModel(urdf_model, srdf));*/ - - robot_model = loadRobotModel(robot_description); - - joint_model_group = robot_model->getJointModelGroup(group_name); - if (!joint_model_group) { - LOG("failed to get joint model group"); - return false; - } - - joint_names.clear(); - - for (auto *joint_model : joint_model_group->getJointModels()) - if (joint_model->getName() != base_frame_ && - joint_model->getType() != moveit::core::JointModel::UNKNOWN && - joint_model->getType() != moveit::core::JointModel::FIXED) - joint_names.push_back(joint_model->getName()); - - auto tips2 = tip_frames_; - joint_model_group->getEndEffectorTips(tips2); - if (!tips2.empty()) - tip_frames_ = tips2; - - link_names = tip_frames_; - - // for(auto& n : joint_names) LOG("joint", n); - // for(auto& n : link_names) LOG("link", n); - - // bool enable_profiler; - lookupParam("profiler", enable_profiler, false); - // if(enable_profiler) Profiler::start(); - - robot_info = RobotInfo(robot_model); - - ikparams.robot_model = robot_model; - ikparams.joint_model_group = joint_model_group; - - // initialize parameters for IKParallel - lookupParam("mode", ikparams.solver_class_name, - std::string("bio2_memetic")); - lookupParam("counter", ikparams.enable_counter, false); - lookupParam("threads", ikparams.thread_count, 0); - - // initialize parameters for Problem - lookupParam("dpos", ikparams.dpos, DBL_MAX); - lookupParam("drot", ikparams.drot, DBL_MAX); - lookupParam("dtwist", ikparams.dtwist, 1e-5); - - // initialize parameters for ik_evolution_1 - lookupParam("no_wipeout", ikparams.opt_no_wipeout, false); - lookupParam("population_size", ikparams.population_size, 8); - lookupParam("elite_count", ikparams.elite_count, 4); - lookupParam("linear_fitness", ikparams.linear_fitness, false); - - temp_state.reset(new moveit::core::RobotState(robot_model)); - - ik.reset(new IKParallel(ikparams)); - - { - - BLOCKPROFILER("default ik goals"); - - default_goals.clear(); - - for (size_t i = 0; i < tip_frames_.size(); i++) { - PoseGoal *goal = new PoseGoal(); - - goal->setLinkName(tip_frames_[i]); - - // LOG_VAR(goal->link_name); - - double rotation_scale = 0.5; - - lookupParam("rotation_scale", rotation_scale, rotation_scale); - - bool position_only_ik = false; - lookupParam("position_only_ik", position_only_ik, position_only_ik); - if (position_only_ik) - rotation_scale = 0; - - goal->setRotationScale(rotation_scale); - - default_goals.emplace_back(goal); - } - - { - double weight = 0; - lookupParam("center_joints_weight", weight, weight); - if (weight > 0.0) { - auto *avoid_joint_limits_goal = new bio_ik::CenterJointsGoal(); - avoid_joint_limits_goal->setWeight(weight); - default_goals.emplace_back(avoid_joint_limits_goal); - } - } - - { - double weight = 0; - lookupParam("avoid_joint_limits_weight", weight, weight); - if (weight > 0.0) { - auto *avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal(); - avoid_joint_limits_goal->setWeight(weight); - default_goals.emplace_back(avoid_joint_limits_goal); - } - } - - { - double weight = 0; - lookupParam("minimal_displacement_weight", weight, weight); - if (weight > 0.0) { - auto *minimal_displacement_goal = - new bio_ik::MinimalDisplacementGoal(); - minimal_displacement_goal->setWeight(weight); - default_goals.emplace_back(minimal_displacement_goal); - } - } - } - - // LOG("init ready"); - - return true; - } - - virtual bool initialize(const std::string &robot_description, - const std::string &group_name, - const std::string &base_frame, - const std::string &tip_frame, - double search_discretization) { - LOG_FNC(); - std::vector tip_frames; - tip_frames.push_back(tip_frame); - initialize(robot_description, group_name, base_frame, tip_frames, - search_discretization); - return true; - } - - virtual bool initialize(const std::string &robot_description, - const std::string &group_name, - const std::string &base_frame, - const std::vector &tip_frames, - double search_discretization) { - LOG_FNC(); - setValues(robot_description, group_name, base_frame, tip_frames, - search_discretization); - load(robot_description, group_name); - return true; - } - - virtual bool - searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, std::vector(), - solution, IKCallbackFn(), error_code, options); - } - - virtual bool - searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, double timeout, - const std::vector &consistency_limits, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, consistency_limits, - solution, IKCallbackFn(), error_code, options); - } - - virtual bool - searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, std::vector(), - solution, solution_callback, error_code, options); - } - - virtual bool - searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, consistency_limits, - solution, solution_callback, error_code, options); - } - - /*struct OptMod : kinematics::KinematicsQueryOptions - { - int test; - };*/ - - virtual bool - searchPositionIK(const std::vector &ik_poses, - const std::vector &ik_seed_state, double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState *context_state = NULL) const { - double t0 = ros::WallTime::now().toSec(); - - // timeout = 0.1; - - // LOG("a"); - - if (enable_profiler) - Profiler::start(); - - auto *bio_ik_options = toBioIKKinematicsQueryOptions(&options); - - LOG_FNC(); - - FNPROFILER(); - - // LOG(typeid(options).name()); - // LOG(((OptMod*)&options)->test); - - // get variable default positions / context state - state.resize(robot_model->getVariableCount()); - robot_model->getVariableDefaultPositions(state); - if (context_state) - for (size_t i = 0; i < robot_model->getVariableCount(); i++) - state[i] = context_state->getVariablePositions()[i]; - - // overwrite used variables with seed state - solution = ik_seed_state; - { - int i = 0; - for (auto &joint_name : getJointNames()) { - auto *joint_model = robot_model->getJointModel(joint_name); - if (!joint_model) - continue; - for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++) - state.at(joint_model->getFirstVariableIndex() + vi) = - solution.at(i++); - } - } - - if (!bio_ik_options || !bio_ik_options->replace) { - // transform tips to baseframe - tipFrames.clear(); - for (size_t i = 0; i < ik_poses.size(); i++) { - Eigen::Affine3d p, r; - tf::poseMsgToEigen(ik_poses[i], p); - if (context_state) { - r = context_state->getGlobalLinkTransform(getBaseFrame()); - } else { - if (i == 0) - temp_state->setToDefaultValues(); - r = temp_state->getGlobalLinkTransform(getBaseFrame()); - } - tipFrames.emplace_back(r * p); - } - } - - // init ik - - problem.timeout = t0 + timeout; - problem.initial_guess = state; - - // for(auto& v : state) LOG("var", &v - &state.front(), v); - - // problem.tip_objectives = tipFrames; - - /*for(size_t i = 0; i < problem.goals.size(); i++) - { - problem.goals[i].frame = tipFrames[i]; - }*/ - - // LOG("---"); - - /*{ - BLOCKPROFILER("ik goals"); - std::vector> goals; - for(size_t i = 0; i < tip_frames_.size(); i++) - { - //if(rand() % 2) break; - PoseGoal* goal = new PoseGoal(); - goal->link_name = tip_frames_[i]; - goal->position = tipFrames[i].pos; - goal->orientation = tipFrames[i].rot; - goals.emplace_back(goal); - //if(rand() % 20) break; - } - //std::random_shuffle(goals.begin(), goals.end()); - //LOG_VAR(goals.size()); - setRequestGoals(problem, goals, ikparams); - }*/ - - { - - if (!bio_ik_options || !bio_ik_options->replace) { - for (size_t i = 0; i < tip_frames_.size(); i++) { - auto *goal = (PoseGoal *)default_goals[i].get(); - goal->setPosition(tipFrames[i].pos); - goal->setOrientation(tipFrames[i].rot); - } - } - - all_goals.clear(); - - if (!bio_ik_options || !bio_ik_options->replace) - for (auto &goal : default_goals) - all_goals.push_back(goal.get()); - - if (bio_ik_options) - for (auto &goal : bio_ik_options->goals) - all_goals.push_back(goal.get()); - - { - BLOCKPROFILER("problem init"); - problem.initialize(ikparams.robot_model, ikparams.joint_model_group, - ikparams, all_goals, bio_ik_options); - // problem.setGoals(default_goals, ikparams); - } - } - - { - BLOCKPROFILER("ik init"); - ik->initialize(problem); - } - - // run ik solver - { - BLOCKPROFILER("ik_solve"); - ik->solve(); - } - - // get solution - state = ik->getSolution(); - - // wrap angles - for (auto ivar : problem.active_variables) { - auto v = state[ivar]; - if (robot_info.isRevolute(ivar) && - robot_model->getMimicJointModels().empty()) { - auto r = problem.initial_guess[ivar]; - auto lo = robot_info.getMin(ivar); - auto hi = robot_info.getMax(ivar); - - // move close to initial guess - if (r < v - M_PI || r > v + M_PI) { - v -= r; - v /= (2 * M_PI); - v += 0.5; - v -= std::floor(v); - v -= 0.5; - v *= (2 * M_PI); - v += r; - } - - // wrap at joint limits - if (v > hi) - v -= std::ceil(std::max(0.0, v - hi) / (2 * M_PI)) * (2 * M_PI); - if (v < lo) - v += std::ceil(std::max(0.0, lo - v) / (2 * M_PI)) * (2 * M_PI); - - // clamp at edges - if (v < lo) - v = lo; - if (v > hi) - v = hi; - } - state[ivar] = v; - } - - // wrap angles - robot_model->enforcePositionBounds(state.data()); - - // map result to jointgroup variables - { - solution.clear(); - for (auto &joint_name : getJointNames()) { - auto *joint_model = robot_model->getJointModel(joint_name); - if (!joint_model) - continue; - for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++) - solution.push_back( - state.at(joint_model->getFirstVariableIndex() + vi)); - } - } - - // set solution fitness - if (bio_ik_options) { - bio_ik_options->solution_fitness = ik->getSolutionFitness(); - } - - // return an error if an accurate solution was requested, but no accurate - // solution was found - if (!ik->getSuccess() && !options.return_approximate_solution) { - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - // callback? - if (!solution_callback.empty()) { - // run callback - solution_callback(ik_poses.front(), solution, error_code); - - // return success if callback has accepted the solution - return error_code.val == error_code.SUCCESS; - } else { - // return success - error_code.val = error_code.SUCCESS; - return true; - } - } - - virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg, - std::string *error_text_out = 0) const { - LOG_FNC(); - // LOG_VAR(jmg->getName()); - return true; - } -}; -} // namespace bio_ik_kinematics_plugin - -// register plugin - -#undef LOG -#undef ERROR -PLUGINLIB_EXPORT_CLASS(bio_ik_kinematics_plugin::BioIKKinematicsPlugin, - kinematics::KinematicsBase); diff --git a/src/bio_ik-kinetic-support/src/problem.cpp b/src/bio_ik-kinetic-support/src/problem.cpp deleted file mode 100644 index ca6759e..0000000 --- a/src/bio_ik-kinetic-support/src/problem.cpp +++ /dev/null @@ -1,340 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "ik_base.h" - -#include -#include - -#include - -#include - -#include - -namespace bio_ik -{ - -enum class Problem::GoalType -{ - Unknown, - Position, - Orientation, - Pose, -}; - -size_t Problem::addTipLink(const moveit::core::LinkModel* link_model) -{ - if(link_tip_indices[link_model->getLinkIndex()] < 0) - { - link_tip_indices[link_model->getLinkIndex()] = tip_link_indices.size(); - tip_link_indices.push_back(link_model->getLinkIndex()); - } - return link_tip_indices[link_model->getLinkIndex()]; -} - -Problem::Problem() - : ros_params_initrd(false) -{ -} - -void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options) -{ - if(robot_model != this->robot_model) - { - modelInfo = RobotInfo(robot_model); - collision_links.clear(); - collision_links.resize(robot_model->getLinkModelCount()); - } - - this->robot_model = robot_model; - this->joint_model_group = joint_model_group; - this->params = params; - - if(!ros_params_initrd) - { - ros_params_initrd = true; - dpos = params.dpos; - drot = params.drot; - dtwist = params.dtwist; - if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX; - if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX; - if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX; - } - - link_tip_indices.clear(); - link_tip_indices.resize(robot_model->getLinkModelCount(), -1); - tip_link_indices.clear(); - - active_variables.clear(); - auto addActiveVariable = [this, robot_model, joint_model_group, options](const std::string& name) -> ssize_t { - if(options) - { - auto& joint_name = robot_model->getJointOfVariable(name)->getName(); - for(auto& fixed_joint_name : options->fixed_joints) - { - if(fixed_joint_name == joint_name) - { - return (ssize_t)-1 - (ssize_t)robot_model->getVariableIndex(name); - } - } - } - for(size_t i = 0; i < active_variables.size(); i++) - if(name == robot_model->getVariableNames()[active_variables[i]]) return i; - for(auto& n : joint_model_group->getVariableNames()) - { - if(n == name) - { - active_variables.push_back(robot_model->getVariableIndex(name)); - return active_variables.size() - 1; - } - } - ERROR("joint variable not found", name); - }; - - goals.clear(); - secondary_goals.clear(); - for(auto& goal : goals2) - { - GoalInfo goal_info; - - goal_info.goal = goal; - - goal->describe(goal_info.goal_context); - - for(auto& link_name : goal_info.goal_context.goal_link_names_) - { - auto* link_model = robot_model->getLinkModel(link_name); - if(!link_model) ERROR("link not found", link_name); - goal_info.goal_context.goal_link_indices_.push_back(addTipLink(link_model)); - } - - for(auto& variable_name : goal_info.goal_context.goal_variable_names_) - { - goal_info.goal_context.goal_variable_indices_.push_back(addActiveVariable(variable_name)); - } - - goal_info.weight = goal_info.goal_context.goal_weight_; - goal_info.weight_sq = goal_info.weight * goal_info.weight; - - goal_info.goal_type = GoalType::Unknown; - - goal_info.frame = Frame::identity(); - goal_info.tip_index = 0; - if(goal_info.goal_context.goal_link_indices_.size()) goal_info.tip_index = goal_info.goal_context.goal_link_indices_[0]; - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Position; - goal_info.frame.pos = g->getPosition(); - } - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Orientation; - goal_info.frame.rot = g->getOrientation(); - } - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Pose; - goal_info.frame.pos = g->getPosition(); - goal_info.frame.rot = g->getOrientation(); - } - - goal_info.goal_context.joint_model_group_ = joint_model_group; - goal_info.goal_context.initial_guess_ = initial_guess; - - if(goal_info.goal_context.goal_secondary_) - secondary_goals.push_back(goal_info); - else - goals.push_back(goal_info); - - // if(goal_info.variable_indices.size() > temp_variables.size()) temp_variables.resize(goal_info.variable_indices.size()); - - // if(goal_info.link_indices.size() > temp_frames.size()) temp_frames.resize(goal_info.link_indices.size()); - } - - // update active variables from active subtree - joint_usage.resize(robot_model->getJointModelCount()); - for(auto& u : joint_usage) - u = 0; - for(auto tip_index : tip_link_indices) - for(auto* link_model = robot_model->getLinkModels()[tip_index]; link_model; link_model = link_model->getParentLinkModel()) - joint_usage[link_model->getParentJointModel()->getJointIndex()] = 1; - if(options) - for(auto& fixed_joint_name : options->fixed_joints) - joint_usage[robot_model->getJointModel(fixed_joint_name)->getJointIndex()] = 0; - for(auto* joint_model : joint_model_group->getActiveJointModels()) - if(joint_usage[joint_model->getJointIndex()] && !joint_model->getMimic()) - for(auto& n : joint_model->getVariableNames()) - addActiveVariable(n); - - // init weights for minimal displacement goals - { - minimal_displacement_factors.resize(active_variables.size()); - double s = 0; - for(auto ivar : active_variables) - s += modelInfo.getMaxVelocityRcp(ivar); - if(s > 0) - { - for(size_t i = 0; i < active_variables.size(); i++) - { - auto ivar = active_variables[i]; - minimal_displacement_factors[i] = modelInfo.getMaxVelocityRcp(ivar) / s; - } - } - else - { - for(size_t i = 0; i < active_variables.size(); i++) - minimal_displacement_factors[i] = 1.0 / active_variables.size(); - } - } - - initialize2(); -} - -void Problem::initialize2() -{ - for(auto* gg : {&goals, &secondary_goals}) - { - for(auto& g : *gg) - { - g.goal_context.problem_active_variables_ = active_variables; - g.goal_context.problem_tip_link_indices_ = tip_link_indices; - g.goal_context.velocity_weights_ = minimal_displacement_factors; - g.goal_context.robot_info_ = &modelInfo; - } - } -} - -double Problem::computeGoalFitness(GoalInfo& goal_info, const Frame* tip_frames, const double* active_variable_positions) -{ - goal_info.goal_context.tip_link_frames_ = tip_frames; - goal_info.goal_context.active_variable_positions_ = active_variable_positions; - return goal_info.goal->evaluate(goal_info.goal_context) * goal_info.weight_sq; -} - -double Problem::computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions) -{ - double sum = 0.0; - for(auto& goal : goals) - sum += computeGoalFitness(goal, tip_frames, active_variable_positions); - return sum; -} - -bool Problem::checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) -{ - for(auto& goal : goals) - { - const auto& fa = goal.frame; - const auto& fb = tip_frames[goal.tip_index]; - - switch(goal.goal_type) - { - - case GoalType::Position: - { - if(dpos != DBL_MAX) - { - double p_dist = (fb.pos - fa.pos).length(); - if(!(p_dist <= dpos)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.vel, KDL::Twist::Zero().vel, dtwist)) return false; - } - continue; - } - - case GoalType::Orientation: - { - if(drot != DBL_MAX) - { - double r_dist = fb.rot.angleShortestPath(fa.rot); - r_dist = r_dist * 180 / M_PI; - if(!(r_dist <= drot)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.rot, KDL::Twist::Zero().rot, dtwist)) return false; - } - continue; - } - - case GoalType::Pose: - { - if(dpos != DBL_MAX || drot != DBL_MAX) - { - double p_dist = (fb.pos - fa.pos).length(); - double r_dist = fb.rot.angleShortestPath(fa.rot); - r_dist = r_dist * 180 / M_PI; - if(!(p_dist <= dpos)) return false; - if(!(r_dist <= drot)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff, KDL::Twist::Zero(), dtwist)) return false; - } - continue; - } - - default: - { - double dmax = DBL_MAX; - dmax = std::fmin(dmax, dpos); - dmax = std::fmin(dmax, dtwist); - double d = computeGoalFitness(goal, tip_frames.data(), active_variable_positions); - if(!(d < dmax * dmax)) return false; - } - } - } - - // LOG("checkSolutionActiveVariables true"); - - return true; -} -} diff --git a/src/bio_ik-kinetic-support/src/problem.h b/src/bio_ik-kinetic-support/src/problem.h deleted file mode 100644 index da8d260..0000000 --- a/src/bio_ik-kinetic-support/src/problem.h +++ /dev/null @@ -1,142 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include "utils.h" -#include - -#include - -#include - -#include -#include - -#include - -namespace bio_ik -{ - -class Problem -{ -private: - bool ros_params_initrd; - std::vector joint_usage; - std::vector link_tip_indices; - std::vector minimal_displacement_factors; - std::vector joint_transmission_goal_temp, joint_transmission_goal_temp2; - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup* joint_model_group; - IKParams params; - RobotInfo modelInfo; - double dpos, drot, dtwist; - struct CollisionShape - { - std::vector vertices; - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - collision_detection::FCLGeometryConstPtr geometry; - Frame frame; - std::vector> edges; - }; - struct CollisionLink - { - bool initialized; - std::vector> shapes; - CollisionLink() - : initialized(false) - { - } - }; - std::vector collision_links; - size_t addTipLink(const moveit::core::LinkModel* link_model); - -public: - /*enum class GoalType; - struct BalanceGoalInfo - { - ssize_t tip_index; - double mass; - Vector3 center; - }; - struct GoalInfo - { - const Goal* goal; - GoalType goal_type; - size_t tip_index; - double weight; - double weight_sq; - double rotation_scale; - double rotation_scale_sq; - Frame frame; - tf::Vector3 target; - tf::Vector3 direction; - tf::Vector3 axis; - double distance; - ssize_t active_variable_index; - double variable_position; - std::vector variable_indices; - mutable size_t last_collision_vertex; - std::vector balance_goal_infos; - };*/ - enum class GoalType; - // std::vector temp_frames; - // std::vector temp_variables; - struct GoalInfo - { - const Goal* goal; - double weight_sq; - double weight; - GoalType goal_type; - size_t tip_index; - Frame frame; - GoalContext goal_context; - }; - double timeout; - std::vector initial_guess; - std::vector active_variables; - std::vector tip_link_indices; - std::vector goals; - std::vector secondary_goals; - Problem(); - void initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options); - void initialize2(); - double computeGoalFitness(GoalInfo& goal, const Frame* tip_frames, const double* active_variable_positions); - double computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions); - bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions); -}; -} diff --git a/src/bio_ik-kinetic-support/src/utils.h b/src/bio_ik-kinetic-support/src/utils.h deleted file mode 100644 index a8edc22..0000000 --- a/src/bio_ik-kinetic-support/src/utils.h +++ /dev/null @@ -1,521 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -//#include - -//#include -//#include - -namespace bio_ik -{ - -struct IKParams -{ - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup* joint_model_group; - - // IKParallel parameters - std::string solver_class_name; - bool enable_counter; - int thread_count; - - //Problem parameters - double dpos; - double drot; - double dtwist; - - // ik_evolution_1 parameters - bool opt_no_wipeout; - int population_size; - int elite_count; - bool linear_fitness; -}; - -#define ENABLE_LOG - -#define ENABLE_PROFILER - -// logging - -//#define LOG_STREAM (std::cerr << std::fixed) -//#define LOG_STREAM (std::cerr << std::scientific) -#define LOG_STREAM (std::cerr) - -template inline void vprint(std::ostream& s, const T& a) { s << a << std::endl; } -template inline void vprint(std::ostream& s, const T& a, const AA&... aa) -{ - s << a << " "; - vprint(s, aa...); -}; - -#define LOG2(...) vprint(LOG_STREAM, "ikbio ", __VA_ARGS__) - -#ifdef ENABLE_LOG -#define LOG(...) LOG2(__VA_ARGS__) -#else -#define LOG(...) -#endif - -#define LOG_VAR(v) LOG2(#v, (v)); - -//#define LOG_FNC() LOG("fun", __func__, __LINE__) -#define LOG_FNC() - -// show error and abort -// #define ERROR(...) { LOG("ERROR", __VA_ARGS__); exit(-1); } -// #define ERROR(a, ...) { LOG(a, __VA_ARGS__); LOG_STREAM.flush(); throw std::runtime_error(a); } -#define ERROR(...) \ - { \ - LOG2(__VA_ARGS__); \ - LOG_STREAM.flush(); \ - std::stringstream ss; \ - vprint(ss, __VA_ARGS__); \ - throw std::runtime_error(ss.str()); \ - } -// #define ERROR(...) { LOG_ALWAYS(__VA_ARGS__); std::raise(SIGINT); } - -// profiler - -#ifdef ENABLE_PROFILER - -// embeddable sampling profiler - -// profiled block or function -struct ProfilerBin -{ - const char* volatile name; // name of scope or function, also used as indicator if it is currently being executed - std::atomic counter; // only used by CounterScope / COUNTERPROFILER - ProfilerBin() - : name(0) - { - } -}; - -// allocate globally unique profiler buffer via template -template ProfilerBin* getProfilerBuffer() -{ - static std::vector buffer(10000); - return buffer.data(); -} - -// reserve profiler buffer segment for current compilation unit -template ProfilerBin* getProfilerSegment() -{ - static size_t index = 0; - return getProfilerBuffer() + (index++) * 20; -} -static ProfilerBin* profiler_segment = getProfilerSegment(); - -// identifies currently profiled thread -// null if profiler is disabled -struct ProfilerInfo -{ - void* stack_begin; - void* stack_end; -}; - -// declare globally unique profiler info via template -template ProfilerInfo& getProfilerInfo() -{ - static ProfilerInfo info; - return info; -} -static ProfilerInfo& profiler_info = getProfilerInfo(); - -// profiles a scope or function -template struct ProfilerScope -{ - __attribute__((always_inline)) inline ProfilerScope(const char* name) - { - if(profiler_info.stack_begin == 0) return; - if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return; - profiler_segment[ID].name = name; - } - __attribute__((always_inline)) inline ~ProfilerScope() - { - if(profiler_info.stack_begin == 0) return; - if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return; - profiler_segment[ID].name = 0; - } -}; -#define FNPROFILER() volatile ProfilerScope<__COUNTER__> _profilerscope(__func__); -#define BLOCKPROFILER(name) volatile ProfilerScope<__COUNTER__> _profilerscope(name); - -// per-thread profiling -struct ThreadScope -{ - size_t id; - __attribute__((always_inline)) inline ThreadScope(const char* name, size_t id) - : id(id) - { - if(profiler_info.stack_begin == 0) return; - profiler_segment[id].name = name; - } - __attribute__((always_inline)) inline ~ThreadScope() - { - if(profiler_info.stack_begin == 0) return; - profiler_segment[id].name = 0; - } -}; -#define THREADPROFILER(name, id) \ - static const char* _threadscope_names[] = {name "0", name "1", name "2", name "3"}; \ - volatile ThreadScope _threadscope(_threadscope_names[id], __COUNTER__ + id); \ - (__COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__); - -// profiling across multiple threads -struct CounterScope -{ - size_t id; - __attribute__((always_inline)) inline CounterScope(const char* name, size_t id) - : id(id) - { - if(profiler_info.stack_begin == 0) return; - if((profiler_segment[id].counter++) == 0) profiler_segment[id].name = name; - } - __attribute__((always_inline)) inline ~CounterScope() - { - if(profiler_info.stack_begin == 0) return; - if((--profiler_segment[id].counter) == 0) profiler_segment[id].name = 0; - } -}; -#define COUNTERPROFILER(name) volatile CounterScope _counterscope(name, __COUNTER__); - -// starts profiler and periodically writes results to log -struct Profiler -{ - std::thread thread; - volatile int exit_flag; - Profiler() - { - pthread_attr_t attr; - pthread_getattr_np(pthread_self(), &attr); - void* stack_addr; - size_t stack_size; - pthread_attr_getstack(&attr, &stack_addr, &stack_size); - profiler_info.stack_begin = stack_addr; - profiler_info.stack_end = (char*)stack_addr + stack_size; - const size_t maxbin = 1000; - static std::mutex mutex; - static std::unordered_map samples; - exit_flag = 0; - std::thread t([this]() { - auto* profiler_bins = getProfilerBuffer(); - while(true) - { - for(int iter = 0; iter < 100; iter++) - { - for(int iter = 0; iter < 100; iter++) - { - int i = rand() % maxbin; - const char* p = profiler_bins[i].name; - if(p) samples[p]++; - } - if(exit_flag) break; - std::this_thread::sleep_for(std::chrono::duration(rand() % 1000)); - } - { - double thistime = ros::WallTime::now().toSec(); - static double lasttime = 0.0; - if(thistime < lasttime + 1) continue; - lasttime = thistime; - static std::vector> data; - data.clear(); - for(auto& p : samples) - data.push_back(p); - std::sort(data.begin(), data.end(), [](const std::pair& a, const std::pair& b) { return a.second > b.second; }); - LOG(""); - LOG("profiler"); - for(auto& d : data) - { - double v = d.second * 100.0 / data[0].second; - char s[32]; - sprintf(s, "%6.2f%%", v); - LOG("p", s, d.first); - } - LOG(""); - } - if(exit_flag) break; - } - }); - std::swap(thread, t); - } - ~Profiler() - { - exit_flag = true; - thread.join(); - } - static void start() { static Profiler profiler; } -}; - -#else - -#define FNPROFILER() -#define BLOCKPROFILER(name) -#define THREADPROFILER(name, id) -#define COUNTERPROFILER(name) - -struct Profiler -{ - static void start() {} -}; - -#endif - -__attribute__((always_inline)) inline double mix(double a, double b, double f) { return a * (1.0 - f) + b * f; } - -__attribute__((always_inline)) inline double clamp(double v, double lo, double hi) -{ - if(v < lo) v = lo; - if(v > hi) v = hi; - return v; -} - -__attribute__((always_inline)) inline double clamp2(double v, double lo, double hi) -{ - if(__builtin_expect(v < lo, 0)) v = lo; - if(__builtin_expect(v > hi, 0)) v = hi; - return v; -} - -__attribute__((always_inline)) inline double smoothstep(float a, float b, float v) -{ - v = clamp((v - a) / (b - a), 0.0, 1.0); - return v * v * (3.0 - 2.0 * v); -} - -__attribute__((always_inline)) inline double sign(double f) -{ - if(f < 0.0) f = -1.0; - if(f > 0.0) f = +1.0; - return f; -} - -template class linear_int_distribution -{ - std::uniform_int_distribution base; - t n; - -public: - inline linear_int_distribution(t vrange) - : n(vrange) - , base(0, vrange) - { - } - template inline t operator()(generator& g) - { - while(true) - { - t v = base(g) + base(g); - if(v < n) return n - v - 1; - } - } -}; - -struct XORShift64 -{ - uint64_t v; - -public: - XORShift64() - : v(88172645463325252ull) - { - } - __attribute__((always_inline)) inline uint64_t operator()() - { - v ^= v << 13; - v ^= v >> 7; - v ^= v << 17; - return v; - } -}; - -// class factory -// -// registering a class: -// static Factory::Class reg("Derived"); -// -// instantiation: -// Base* obj = Factory::create("Derived"); -// -// cloning and object: -// p = Factory::clone(o); -// -template class Factory -{ - typedef BASE* (*Constructor)(ARGS...); - struct ClassBase - { - std::string name; - std::type_index type; - virtual BASE* create(ARGS... args) const = 0; - virtual BASE* clone(const BASE*) const = 0; - ClassBase() - : type(typeid(void)) - { - } - }; - typedef std::set MapType; - static MapType& classes() - { - static MapType ff; - return ff; - } - -public: - template struct Class : ClassBase - { - BASE* create(ARGS... args) const { return new DERIVED(args...); } - BASE* clone(const BASE* o) const { return new DERIVED(*(const DERIVED*)o); } - Class(const std::string& name) - { - this->name = name; - this->type = typeid(DERIVED); - classes().insert(this); - } - ~Class() { classes().erase(this); } - }; - static BASE* create(const std::string& name, ARGS... args) - { - for(auto* f : classes()) - if(f->name == name) return f->create(args...); - ERROR("class not found", name); - } - template static DERIVED* clone(const DERIVED* o) - { - for(auto* f : classes()) - if(f->type == typeid(*o)) return (DERIVED*)f->clone(o); - ERROR("class not found", typeid(*o).name()); - } -}; - -// Alloctes memory properly aligned for SIMD operations -template struct aligned_allocator : public std::allocator -{ - typedef size_t size_type; - typedef ptrdiff_t difference_type; - typedef T* pointer; - typedef const T* const_pointer; - typedef T& reference; - typedef const T& const_reference; - typedef T value_type; - T* allocate(size_t s, const void* hint = 0) - { - void* p; - if(posix_memalign(&p, A, sizeof(T) * s + 64)) throw std::bad_alloc(); - return (T*)p; - } - void deallocate(T* ptr, size_t s) { free(ptr); } - template struct rebind - { - typedef aligned_allocator other; - }; -}; - -// std::vector typedef with proper memory alignment for SIMD operations -template struct aligned_vector : std::vector> -{ -}; - -// Helper class for reading structured data from ROS parameter server -class XmlRpcReader -{ - typedef XmlRpc::XmlRpcValue var; - var& v; - -public: - XmlRpcReader(var& v) - : v(v) - { - } - -private: - XmlRpcReader at(int i) { return v[i]; } - void conv(bool& r) { r = (bool)v; } - void conv(double& r) { r = (v.getType() == var::TypeInt) ? ((double)(int)v) : ((double)v); } - void conv(tf::Vector3& r) - { - double x, y, z; - at(0).conv(x); - at(1).conv(y); - at(2).conv(z); - r = tf::Vector3(x, y, z); - } - void conv(tf::Quaternion& r) - { - double x, y, z, w; - at(0).conv(x); - at(1).conv(y); - at(2).conv(z); - at(3).conv(w); - r = tf::Quaternion(x, y, z, w).normalized(); - } - void conv(std::string& r) { r = (std::string)v; } - -public: - template void param(const char* key, T& r) - { - if(!v.hasMember(key)) return; - try - { - XmlRpcReader(v[key]).conv(r); - } - catch(const XmlRpc::XmlRpcException& e) - { - LOG(key); - throw; - } - } -}; -}